From d6d26ee21475d1d2a12389e20bb7d21356dbca9d Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Wed, 21 Feb 2024 15:22:26 +0900 Subject: [PATCH 1/3] feat(behavior_velocity_traffic_light): ensure stopping if a signal, once received, is not received again." Signed-off-by: Tomohito Ando --- planning/behavior_velocity_planner/src/node.cpp | 2 ++ .../behavior_velocity_planner_common/planner_data.hpp | 3 +++ .../behavior_velocity_traffic_light_module/src/scene.cpp | 7 ++++++- 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 9fa5634c6dd65..9f18a1a6de816 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -323,6 +323,8 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( { std::lock_guard lock(mutex_); + planner_data_.has_received_signal_ = true; + // clear previous observation planner_data_.traffic_light_id_map_raw_.clear(); const auto traffic_light_id_map_last_observed_old = diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 606e41ad4b1d1..23997e34fbfea 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -83,6 +83,9 @@ struct PlannerData std::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; + // this value becomes true once the signal message is received + bool has_received_signal_ = false; + // velocity smoother std::shared_ptr velocity_smoother_; // route handler diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 96cb2c4e1c790..aaca066f88cbe 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -282,10 +282,15 @@ bool TrafficLightModule::isStopSignal() updateTrafficSignal(); // If it never receives traffic signal, it will PASS. - if (!traffic_signal_stamp_) { + if (!planner_data_->has_received_signal_) { return false; } + // If the signal data is not received, the ego stops. + if (!traffic_signal_stamp_) { + return true; + } + if (isTrafficSignalTimedOut()) { return true; } From 518cda64994a10e7230d798ce458f43bc2580112 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Wed, 21 Feb 2024 15:23:50 +0900 Subject: [PATCH 2/3] add comments for clarity Signed-off-by: Tomohito Ando --- planning/behavior_velocity_traffic_light_module/src/scene.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index aaca066f88cbe..3fdb5d514a0e9 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -291,6 +291,7 @@ bool TrafficLightModule::isStopSignal() return true; } + // If the signal data is timeout, the ego stops. if (isTrafficSignalTimedOut()) { return true; } From ee820149faa55a274d8bb011c199ad1c90a18ee7 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Thu, 22 Feb 2024 09:50:56 +0900 Subject: [PATCH 3/3] fix comments for clarity Signed-off-by: Tomohito Ando --- .../src/scene.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 3fdb5d514a0e9..946741b14cf78 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -281,21 +281,25 @@ bool TrafficLightModule::isStopSignal() { updateTrafficSignal(); - // If it never receives traffic signal, it will PASS. + // Pass through if no traffic signal information has been received yet + // This is to prevent stopping on the planning simulator if (!planner_data_->has_received_signal_) { return false; } - // If the signal data is not received, the ego stops. + // Stop if there is no upcoming traffic signal information + // This is to safely stop in cases such that traffic light recognition is not working properly or + // the map is incorrect if (!traffic_signal_stamp_) { return true; } - // If the signal data is timeout, the ego stops. + // Stop if the traffic signal information has timed out if (isTrafficSignalTimedOut()) { return true; } + // Check if the current traffic signal state requires stopping return traffic_light_utils::isTrafficSignalStop(lane_, looking_tl_state_); }