fix: add operation to remove traffic signals with duplicated ids

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>
autowarefoundation · Dec 4, 2023 · d52a128 · d52a128
1 parent ea957b4
commit d52a128
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 0 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
@@ -90,6 +90,8 @@ class MultiCameraFusion : public rclcpp::Node
   void convertOutputMsg(
     const std::map<IdType, FusionRecord> & grouped_record_map, NewSignalArrayType & msg_out);
 
+  void removeDuplicateIds(NewSignalArrayType & signal_array) const;
+
   void groupFusion(
     std::map<IdType, FusionRecord> & fused_record_map,
     std::map<IdType, FusionRecord> & grouped_record_map);

diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/traffic_light_multi_camera_fusion/src/node.cpp
@@ -200,6 +200,7 @@ void MultiCameraFusion::trafficSignalRoiCallback(
 
   NewSignalArrayType msg_out;
   convertOutputMsg(grouped_record_map, msg_out);
+  removeDuplicateIds(msg_out);
   msg_out.stamp = cam_info_msg->header.stamp;
   signal_pub_->publish(msg_out);
 }
@@ -240,6 +241,20 @@ void MultiCameraFusion::convertOutputMsg(
   }
 }
 
+void MultiCameraFusion::removeDuplicateIds(NewSignalArrayType & signal_array) const
+{
+  auto & signals = signal_array.signals;
+  std::sort(signals.begin(), signals.end(), [](const auto & s1, const auto & s2) {
+    return s1.traffic_signal_id < s2.traffic_signal_id;
+  });
+
+  signals.erase(
+    std::unique(
+      signals.begin(), signals.end(),
+      [](const auto & s1, const auto s2) { return s1.traffic_signal_id == s2.traffic_signal_id; }),
+    signals.end());
+}
+
 void MultiCameraFusion::multiCameraFusion(std::map<IdType, FusionRecord> & fused_record_map)
 {
   fused_record_map.clear();