fix(traffic_light_multi_camera_fusion): assign multiple regulatory el…

…ement id for one traffic light (#777) fix(traffic_light_multi_camera_fusion): support multiple regulatory element id Signed-off-by: Tomohito Ando <[email protected]>
tier4 · Aug 30, 2023 · a75e30d · a75e30d
1 parent 64e4d76
commit a75e30d
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 9 deletions.
diff --git a/...tion/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp b/...tion/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp
@@ -93,7 +93,7 @@ class MultiCameraFusion : public rclcpp::Node
     const std::map<IdType, FusionRecord> & grouped_record_map, NewSignalArrayType & msg_out);
 
   void groupFusion(
-    std::map<IdType, FusionRecord> & fusioned_record_map,
+    const std::map<IdType, FusionRecord> & fusioned_record_map,
     std::map<IdType, FusionRecord> & grouped_record_map);
 
   typedef mf::sync_policies::ExactTime<CamInfoType, RoiArrayType, SignalArrayType> ExactSyncPolicy;
@@ -113,7 +113,7 @@ class MultiCameraFusion : public rclcpp::Node
   /*
   the mappping from traffic light id (instance id) to regulatory element id (group id)
   */
-  std::map<lanelet::Id, lanelet::Id> traffic_light_id_to_regulatory_ele_id_;
+  std::map<lanelet::Id, std::vector<lanelet::Id>> traffic_light_id_to_regulatory_ele_id_;
   /*
   save record arrays by increasing timestamp order.
   use multiset in case there are multiple cameras publishing images at exactly the same time

diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/traffic_light_multi_camera_fusion/src/node.cpp
@@ -216,7 +216,7 @@ void MultiCameraFusion::mapCallback(
 
     auto lights = tl->trafficLights();
     for (const auto & light : lights) {
-      traffic_light_id_to_regulatory_ele_id_[light.id()] = tl->id();
+      traffic_light_id_to_regulatory_ele_id_[light.id()].emplace_back(tl->id());
     }
   }
 }
@@ -290,7 +290,7 @@ void MultiCameraFusion::multiCameraFusion(std::map<IdType, FusionRecord> & fusio
 }
 
 void MultiCameraFusion::groupFusion(
-  std::map<IdType, FusionRecord> & fusioned_record_map,
+  const std::map<IdType, FusionRecord> & fusioned_record_map,
   std::map<IdType, FusionRecord> & grouped_record_map)
 {
   grouped_record_map.clear();
@@ -302,11 +302,15 @@ void MultiCameraFusion::groupFusion(
     if (traffic_light_id_to_regulatory_ele_id_.count(roi_id) == 0) {
       RCLCPP_WARN_STREAM(
         get_logger(), "Found Traffic Light Id = " << roi_id << " which is not defined in Map");
-    } else {
-      /*
-      keep the best record for every regulatory element id
-      */
-      IdType reg_ele_id = traffic_light_id_to_regulatory_ele_id_[p.second.roi.traffic_light_id];
+      continue;
+    }
+
+    /*
+    keep the best record for every regulatory element id
+    */
+    const auto reg_ele_id_vec =
+      traffic_light_id_to_regulatory_ele_id_[p.second.roi.traffic_light_id];
+    for (const auto & reg_ele_id : reg_ele_id_vec) {
       if (
         grouped_record_map.count(reg_ele_id) == 0 ||
         ::compareRecord(p.second, grouped_record_map[reg_ele_id]) >= 0) {