create crosswalk user history

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

Author: Dmitrii Koldaev

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -85,6 +85,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;
@@ -137,6 +143,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   // Object History
   std::unordered_map<std::string, std::deque<ObjectData>> objects_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_;
 
   // Lanelet Map Pointers
   std::shared_ptr<lanelet::LaneletMap> lanelet_map_ptr_;
@@ -234,6 +241,9 @@ class MapBasedPredictionNode : public rclcpp::Node
   void updateObjectsHistory(
     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::vector<PredictedRefPath> getPredictedReferencePath(
     const TrackedObject & object, const LaneletsData & current_lanelets_data,

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -967,6 +967,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
   removeOldObjectsHistory(objects_detected_time, object_buffer_time_length_, objects_history_);
   cleanupOldStoppedOnGreenTimes(in_objects);
 
+  auto invalidated_crosswalk_users = removeOldObjectsHistory(
+    objects_detected_time, object_buffer_time_length_, crosswalk_users_history_);
+
   // result output
   PredictedObjects output;
   output.header = in_objects->header;
@@ -1172,6 +1175,19 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
     "debug/processing_time_ms", processing_time_ms);
 }
 
+void MapBasedPredictionNode::updateCrosswalkUserHistory(
+  const std_msgs::msg::Header & header, const TrackedObject & object, const std::string & object_id)
+{
+  CrosswalkUserData crosswalk_user_data;
+  crosswalk_user_data.header = header;
+  crosswalk_user_data.tracked_object = object;
+  if (crosswalk_users_history_.count(object_id) == 0) {
+    crosswalk_users_history_.emplace(object_id, std::deque<CrosswalkUserData>{crosswalk_user_data});
+  } else {
+    crosswalk_users_history_.at(object_id).push_back(crosswalk_user_data);
+  }
+}
+
 bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path)
 {
   const lanelet::ConstLineStrings3d & all_fences =