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..946741b14cf78 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -281,15 +281,25 @@ bool TrafficLightModule::isStopSignal() { updateTrafficSignal(); - // If it never receives traffic signal, it will PASS. - if (!traffic_signal_stamp_) { + // 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; } + // 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; + } + + // 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_); }