diff --git a/planning/behavior_velocity_traffic_light_module/README.md b/planning/behavior_velocity_traffic_light_module/README.md index 866ab87070369..e85a171495e38 100644 --- a/planning/behavior_velocity_traffic_light_module/README.md +++ b/planning/behavior_velocity_traffic_light_module/README.md @@ -19,7 +19,13 @@ This module is activated when there is traffic light in ego lane. 1. Obtains a traffic light mapped to the route and a stop line correspond to the traffic light from a map information. -2. Uses the highest reliability one of the traffic light recognition result and if the color of that was red, generates a stop point. + - If a corresponding traffic light signal have never been found, it treats as a signal to pass. + + - If a corresponding traffic light signal is found but timed out, it treats as a signal to stop. + +2. Uses the highest reliability one of the traffic light recognition result and if the color of that was not green or corresponding arrow signal, generates a stop point. + + - If an elapsed time to receive stop signal is less than `stop_time_hysteresis`, it treats as a signal to pass. This feature is to prevent chattering. 3. When vehicle current velocity is @@ -63,12 +69,13 @@ This module is activated when there is traffic light in ego lane. #### Module Parameters -| Parameter | Type | Description | -| -------------------- | ------ | ----------------------------------------------- | -| `stop_margin` | double | [m] margin before stop point | -| `tl_state_timeout` | double | [s] time out for detected traffic light result. | -| `yellow_lamp_period` | double | [s] time for yellow lamp | -| `enable_pass_judge` | bool | [-] whether to use pass judge | +| Parameter | Type | Description | +| ---------------------- | ------ | -------------------------------------------------------------------- | +| `stop_margin` | double | [m] margin before stop point | +| `tl_state_timeout` | double | [s] time out for detected traffic light result. | +| `stop_time_hysteresis` | double | [s] time threshold to decide stop planning for chattering prevention | +| `yellow_lamp_period` | double | [s] time for yellow lamp | +| `enable_pass_judge` | bool | [-] whether to use pass judge | #### Flowchart @@ -91,7 +98,7 @@ if (state is APPROACH) then (yes) :change state to GO_OUT; stop elseif (no stop signal) then (yes) - :change previous state to STOP; + :change previous state to PASS; stop elseif (not pass through) then (yes) :insert stop pose; diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index a80c97779edaf..89be8a2ba6952 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -3,6 +3,7 @@ traffic_light: stop_margin: 0.0 tl_state_timeout: 1.0 + stop_time_hysteresis: 0.1 yellow_lamp_period: 2.75 enable_pass_judge: true enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 8ef247847c503..f1e9497f5d39d 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -38,6 +38,8 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) const std::string ns(getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); planner_param_.tl_state_timeout = getOrDeclareParameter(node, ns + ".tl_state_timeout"); + planner_param_.stop_time_hysteresis = + getOrDeclareParameter(node, ns + ".stop_time_hysteresis"); planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 233d553d15e4e..3dcb69273a18b 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -220,14 +220,35 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (signed_arc_length_to_stop_point < signed_deadline_length) { RCLCPP_INFO(logger_, "APPROACH -> GO_OUT"); state_ = State::GO_OUT; + stop_signal_received_time_ptr_.reset(); return true; } first_ref_stop_path_point_index_ = stop_line_point_idx; // Check if stop is coming. - setSafe(!isStopSignal()); + const bool is_stop_signal = isStopSignal(); + // Update stop signal received time + if (is_stop_signal) { + if (!stop_signal_received_time_ptr_) { + stop_signal_received_time_ptr_ = std::make_unique