From 932579d7f4c29a189c432bd4eab90decaa49ddf0 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 18 Apr 2024 10:12:17 +0900 Subject: [PATCH] fix(behavior_velocity_traffic_light): stop for outdated signals and set prev_state_stop when using time to red signal (#1259) * fix(behavior_velocity_traffic_light): stop when the signal is timed out Signed-off-by: Tomohito Ando * fix(behavior_velocity_traffic_light): set prev_state_stop when using time to red signal Signed-off-by: Tomohito Ando --------- Signed-off-by: Tomohito Ando --- .../src/scene.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 0b1383662d7e1..83443bc726bbd 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -233,6 +233,8 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const bool is_unknown_signal = isUnknownSignal(looking_tl_state_); const bool is_signal_timed_out = isTrafficSignalTimedOut(); + const bool is_stop_signal = isStopSignal(); + const bool is_stop_required = is_unknown_signal || is_signal_timed_out || is_stop_signal; // Decide if stop or proceed using the remaining time to red signal // If the upcoming traffic signal color is unknown or timed out, do not use the time to red and @@ -241,20 +243,20 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); const bool is_time_to_red_signal_available = (rest_time_to_red_signal.has_value() && !isDataTimeout(rest_time_to_red_signal->stamp)); - const bool is_stop_required = is_unknown_signal || is_signal_timed_out; if (planner_param_.v2i_use_rest_time && is_time_to_red_signal_available && !is_stop_required) { if (!canPassStopLineBeforeRed(*rest_time_to_red_signal, signed_arc_length_to_stop_point)) { *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + is_prev_state_stop_ = true; + return true; } + + is_prev_state_stop_ = false; return true; } - // Check if stop is coming. - const bool is_stop_signal = isStopSignal(); - // Update stop signal received time - if (is_stop_signal || is_unknown_signal || is_signal_timed_out) { + if (is_stop_required) { if (!stop_signal_received_time_ptr_) { stop_signal_received_time_ptr_ = std::make_unique