From a98a8f61513d99355b4de660507f117e8b52b42e Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 4 Feb 2025 15:37:32 +0900 Subject: [PATCH] feat(autoware_behavior_velocity_traffic_light_module): adjust velocity threshold for ensure stop at yellow light (#10064) Signed-off-by: tomoya.kimura --- .../README.md | 19 ++++++++++--------- .../config/traffic_light.param.yaml | 1 + .../src/manager.cpp | 2 ++ .../src/scene.cpp | 3 ++- .../src/scene.hpp | 1 + 5 files changed, 16 insertions(+), 10 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/README.md index e85a171495e38..15e707ee32c62 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/README.md @@ -29,9 +29,9 @@ This module is activated when there is traffic light in ego lane. 3. When vehicle current velocity is - - higher than 2.0m/s ⇒ pass judge(using next slide formula) + - higher than `yellow_light_stop_velocity` m/s ⇒ pass judge(using next slide formula) - - lower than 2.0m/s ⇒ stop + - lower than `yellow_light_stop_velocity` m/s ⇒ stop 4. When it to be judged that vehicle can’t stop before stop line, autoware chooses one of the following behaviors @@ -69,13 +69,14 @@ 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. | -| `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 | +| 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 | +| `yellow_light_stop_velocity` | double | [m/s] velocity threshold for always stopping at a yellow light. | +| `enable_pass_judge` | bool | [-] whether to use pass judge | #### Flowchart diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index 23746a61b6043..253fb69043f7c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -5,5 +5,6 @@ tl_state_timeout: 1.0 stop_time_hysteresis: 0.1 yellow_lamp_period: 2.75 + yellow_light_stop_velocity: 1.0 # Velocity threshold (m/s) below which the vehicle will always stop before the traffic light when the signal turns yellow, regardless of the pass_judge decision. enable_pass_judge: true enable_rtc: false # 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_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 1d1ee7fc996b2..dc008083bb0dd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -42,6 +42,8 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); + planner_param_.yellow_light_stop_velocity = + getOrDeclareParameter(node, ns + ".yellow_light_stop_velocity"); pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 3a8692e9c53dd..f46edd6c52f81 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -213,7 +213,8 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const delay_response_time); const bool distance_stoppable = pass_judge_line_distance < signed_arc_length; - const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 2.0; + const bool slow_velocity = + planner_data_->current_velocity->twist.linear.x < planner_param_.yellow_light_stop_velocity; const bool stoppable = distance_stoppable || slow_velocity; const bool reachable = signed_arc_length < reachable_distance; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index 71f0855a4af6f..12e1ba2537f92 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -61,6 +61,7 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC double stop_margin; double tl_state_timeout; double yellow_lamp_period; + double yellow_light_stop_velocity; double stop_time_hysteresis; bool enable_pass_judge; };