fix(crosswalk_traffic_light_estimator): add operation to remove traff…

…ic signals with duplicated ids (autowarefoundation#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>

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_;

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>