feat(map_based_prediction): incorporate crosswalk user history (autowarefoundation#6905)

Signed-off-by: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>

Author: dkoldaev

perception/map_based_prediction/config/map_based_prediction.param.yaml

@@ -27,6 +27,9 @@
       timeout_set_for_no_intention_to_walk: [1.0, 0.0] # [s] values of map
     # parameter for shoulder lane prediction
     prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8
+    use_crosswalk_user_history:
+      match_lost_and_appeared_users: false
+      remember_lost_users: false
 
     # parameters for lc prediction
     lane_change_detection:

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -48,6 +48,7 @@
 #include <optional>
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -85,6 +86,12 @@ struct ObjectData
     Maneuver::UNINITIALIZED};  // output maneuver considering previous one shot maneuvers
 };
 
+struct CrosswalkUserData
+{
+  std_msgs::msg::Header header;
+  autoware_auto_perception_msgs::msg::TrackedObject tracked_object;
+};
+
 struct LaneletData
 {
   lanelet::Lanelet lanelet;
@@ -135,8 +142,10 @@ class MapBasedPredictionNode : public rclcpp::Node
   std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
 
   // Object History
-  std::unordered_map<std::string, std::deque<ObjectData>> objects_history_;
+  std::unordered_map<std::string, std::deque<ObjectData>> road_users_history;
   std::map<std::pair<std::string, lanelet::Id>, rclcpp::Time> stopped_times_against_green_;
+  std::unordered_map<std::string, std::deque<CrosswalkUserData>> crosswalk_users_history_;
+  std::unordered_map<std::string, std::string> known_matches_;
 
   // Lanelet Map Pointers
   std::shared_ptr<lanelet::LaneletMap> lanelet_map_ptr_;
@@ -200,6 +209,9 @@ class MapBasedPredictionNode : public rclcpp::Node
   std::vector<double> distance_set_for_no_intention_to_walk_;
   std::vector<double> timeout_set_for_no_intention_to_walk_;
 
+  bool match_lost_and_appeared_crosswalk_users_;
+  bool remember_lost_crosswalk_users_;
+
   std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 
   // Member Functions
@@ -222,8 +234,7 @@ class MapBasedPredictionNode : public rclcpp::Node
 
   PredictedObject getPredictedObjectAsCrosswalkUser(const TrackedObject & object);
 
-  void removeOldObjectsHistory(
-    const double current_time, const TrackedObjects::ConstSharedPtr in_objects);
+  void removeStaleTrafficLightInfo(const TrackedObjects::ConstSharedPtr in_objects);
 
   LaneletsData getCurrentLanelets(const TrackedObject & object);
   bool checkCloseLaneletCondition(
@@ -232,10 +243,14 @@ class MapBasedPredictionNode : public rclcpp::Node
     const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const;
   void updateObjectData(TrackedObject & object);
 
-  void updateObjectsHistory(
+  void updateRoadUsersHistory(
     const std_msgs::msg::Header & header, const TrackedObject & object,
     const LaneletsData & current_lanelets_data);
-
+  void updateCrosswalkUserHistory(
+    const std_msgs::msg::Header & header, const TrackedObject & object,
+    const std::string & object_id);
+  std::string tryMatchNewObjectToDisappeared(
+    const std::string & object_id, std::unordered_map<std::string, TrackedObject> & current_users);
   std::vector<PredictedRefPath> getPredictedReferencePath(
     const TrackedObject & object, const LaneletsData & current_lanelets_data,
     const double object_detected_time);