feat(traffic_light_estimator): add flag to use last detect signal

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Jun 29, 2022 · 420dc7c · 420dc7c
1 parent dcf02bc
commit 420dc7c
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 1 deletion.
diff --git a/perception/traffic_light_estimator/include/traffic_light_estimator/node.hpp b/perception/traffic_light_estimator/include/traffic_light_estimator/node.hpp
@@ -89,6 +89,9 @@ class TrafficLightEstimatorNode : public rclcpp::Node
 
   uint8_t getLastDetectedTrafficSignal(const lanelet::Id & id) const;
 
+  // Node param
+  bool use_last_detect_color_;
+
   // Signal history
   std::unordered_map<uint32_t, uint8_t> last_detect_color_;
 

diff --git a/perception/traffic_light_estimator/src/node.cpp b/perception/traffic_light_estimator/src/node.cpp
@@ -80,6 +80,8 @@ TrafficLightEstimatorNode::TrafficLightEstimatorNode(const rclcpp::NodeOptions &
 {
   using std::placeholders::_1;
 
+  use_last_detect_color_ = this->declare_parameter("use_last_detect_color", true);
+
   sub_map_ = create_subscription<HADMapBin>(
     "~/input/vector_map", rclcpp::QoS{1}.transient_local(),
     std::bind(&TrafficLightEstimatorNode::onMap, this, _1));
@@ -231,7 +233,7 @@ lanelet::ConstLanelets TrafficLightEstimatorNode::getGreenLanelets(
 
     const auto last_detected_signal = getLastDetectedTrafficSignal(tl_reg_elem->id());
     const auto was_green = current_detected_signal == TrafficLight::UNKNOWN &&
-                           last_detected_signal == TrafficLight::GREEN;
+                           last_detected_signal == TrafficLight::GREEN && use_last_detect_color_;
 
     updateLastDetectedSignal(tl_reg_elem->id(), current_detected_signal);