diff --git a/perception/traffic_light_estimator/include/traffic_light_estimator/node.hpp b/perception/traffic_light_estimator/include/traffic_light_estimator/node.hpp index 1642b71ef6502..322cec682baf3 100644 --- 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 last_detect_color_; diff --git a/perception/traffic_light_estimator/src/node.cpp b/perception/traffic_light_estimator/src/node.cpp index a0c807bebbfba..247290592de5e 100644 --- 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( "~/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);