feat: move operation into crosswalk_traffic_light_estimator

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

Hide file tree

Showing 4 changed files with 19 additions and 17 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>

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,8 +90,6 @@ 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,7 +200,6 @@ 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);
 }
@@ -241,20 +240,6 @@ 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();