From a75e30d07af55924ac3ea95a8323d25572396a59 Mon Sep 17 00:00:00 2001
From: Tomohito ANDO <tomohito.ando@tier4.jp>
Date: Wed, 30 Aug 2023 09:43:29 +0900
Subject: [PATCH] fix(traffic_light_multi_camera_fusion): assign multiple
 regulatory element id for one traffic light (#777)

fix(traffic_light_multi_camera_fusion): support multiple regulatory element id

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
---
 .../traffic_light_multi_camera_fusion/node.hpp |  4 ++--
 .../src/node.cpp                               | 18 +++++++++++-------
 2 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp
index bc63f34b191e7..e00912b9e84ff 100644
--- a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp
+++ b/perception/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
index 892b6be62ef9d..0f523f640c4cd 100644
--- 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) {