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 <[email protected]> * feat: move operation into `crosswalk_traffic_light_estimator` Signed-off-by: ktro2828 <[email protected]> --------- Signed-off-by: ktro2828 <[email protected]>
Interplai · May 28, 2024 · 0e97ab6 · 0e97ab6
1 parent 5690d8c
commit 0e97ab6
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 0 deletions.
diff --git a/...tion/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/...tion/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
@@ -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>