From 0e97ab61c4f09a30c3a92f172d7eea488bb320ea Mon Sep 17 00:00:00 2001
From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com>
Date: Fri, 8 Dec 2023 10:20:39 +0900
Subject: [PATCH] fix(crosswalk_traffic_light_estimator): add operation to
 remove traffic signals with duplicated ids (#5653)

* fix: add operation to remove traffic signals with duplicated ids

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

* feat: move operation into `crosswalk_traffic_light_estimator`

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

---------

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>
---
 .../crosswalk_traffic_light_estimator/node.hpp  |  2 ++
 .../src/node.cpp                                | 17 +++++++++++++++++
 2 files changed, 19 insertions(+)

diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp
index e38b747a6ce67..848293f6334bb 100644
--- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp
+++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp
@@ -92,6 +92,8 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
   boost::optional<uint8_t> getHighestConfidenceTrafficSignal(
     const lanelet::Id & id, const TrafficLightIdMap & traffic_light_id_map) const;
 
+  void removeDuplicateIds(TrafficSignalArray & signal_array) const;
+
   // Node param
   bool use_last_detect_color_;
   double last_detect_color_hold_time_;
diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp
index 870b8bc7c13f5..6527f0662bcbf 100644
--- a/perception/crosswalk_traffic_light_estimator/src/node.cpp
+++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp
@@ -174,6 +174,8 @@ void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray(
     setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, output);
   }
 
+  removeDuplicateIds(output);
+
   updateLastDetectedSignal(traffic_light_id_map);
 
   pub_traffic_light_array_->publish(output);
@@ -383,6 +385,21 @@ boost::optional<uint8_t> CrosswalkTrafficLightEstimatorNode::getHighestConfidenc
 
   return ret;
 }
+
+void CrosswalkTrafficLightEstimatorNode::removeDuplicateIds(TrafficSignalArray & 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());
+}
+
 }  // namespace traffic_light
 
 #include <rclcpp_components/register_node_macro.hpp>