From 420dc7c576c35d9344636c4b7ffc03e2889fcc7c Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Wed, 29 Jun 2022 10:52:24 +0900 Subject: [PATCH] feat(traffic_light_estimator): add flag to use last detect signal Signed-off-by: satoshi-ota --- .../include/traffic_light_estimator/node.hpp | 3 +++ perception/traffic_light_estimator/src/node.cpp | 4 +++- 2 files changed, 6 insertions(+), 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 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);