From 8f4157994e3bdbfa58f6b2ad05a1d9c6bda5b008 Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <42mehmetdogru42@gmail.com> Date: Tue, 22 Nov 2022 01:19:45 +0300 Subject: [PATCH] feat: parameterize ego_yield_query_stop_duration for crosswalk module Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> --- .../behavior_velocity_planner/crosswalk.param.yaml | 1 + .../behavior_velocity_planner/config/crosswalk.param.yaml | 1 + planning/behavior_velocity_planner/crosswalk-design.md | 1 + .../include/scene_module/crosswalk/scene_crosswalk.hpp | 1 + .../src/scene_module/crosswalk/manager.cpp | 2 ++ .../src/scene_module/crosswalk/scene_crosswalk.cpp | 4 +++- 6 files changed, 9 insertions(+), 1 deletion(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml index 2cc5260e4ea9b..2bbc5d31fcaff 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml @@ -26,6 +26,7 @@ stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) max_yield_timeout: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + ego_yield_query_stop_duration: 0.1 # [s] the amount of time which ego should be stopping to query whether it yields or not # param for input data tl_state_timeout: 3.0 # [s] timeout threshold for traffic light signal diff --git a/planning/behavior_velocity_planner/config/crosswalk.param.yaml b/planning/behavior_velocity_planner/config/crosswalk.param.yaml index 2cc5260e4ea9b..2bbc5d31fcaff 100644 --- a/planning/behavior_velocity_planner/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_planner/config/crosswalk.param.yaml @@ -26,6 +26,7 @@ stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) max_yield_timeout: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + ego_yield_query_stop_duration: 0.1 # [s] the amount of time which ego should be stopping to query whether it yields or not # param for input data tl_state_timeout: 3.0 # [s] timeout threshold for traffic light signal diff --git a/planning/behavior_velocity_planner/crosswalk-design.md b/planning/behavior_velocity_planner/crosswalk-design.md index a24312d836f97..4897737b98c0c 100644 --- a/planning/behavior_velocity_planner/crosswalk-design.md +++ b/planning/behavior_velocity_planner/crosswalk-design.md @@ -93,6 +93,7 @@ Also see algorithm section. | `stop_object_velocity_threshold` | double | [m/s] velocity threshold for the module to judge whether the objects is stopped | | `min_object_velocity` | double | [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV.) | | `max_yield_timeout` | double | [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. | +| `ego_yield_query_stop_duration` | double | [s] the amount of time which ego should be stopping to query whether it yields or not. | #### Parameters for input data diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp index 723d2178d67b2..ec69bcd1dd1ca 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp @@ -81,6 +81,7 @@ class CrosswalkModule : public SceneModuleInterface double stop_object_velocity; double min_object_velocity; double max_yield_timeout; + double ego_yield_query_stop_duration; // param for input data double external_input_timeout; double tl_state_timeout; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp index 65acffbc47a56..6028932ec2a94 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp @@ -110,6 +110,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".stop_object_velocity_threshold", 0.5 / 3.6); cp.min_object_velocity = node.declare_parameter(ns + ".min_object_velocity", 5.0 / 3.6); cp.max_yield_timeout = node.declare_parameter(ns + ".max_yield_timeout", 3.0); + cp.ego_yield_query_stop_duration = + node.declare_parameter(ns + ".ego_yield_query_stop_duration", 0.1); // param for input data cp.external_input_timeout = node.declare_parameter(ns + ".external_input_timeout", 1.0); diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp index 3d0102a85b1a2..eac948942f76c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -403,7 +403,9 @@ boost::optional CrosswalkModule::findNearestStopPoint dist_ego2cp - base_link2front < planner_param_.stop_position_threshold || p_stop_line.get().first - base_link2front < planner_param_.stop_position_threshold; - const auto is_yielding_now = planner_data_->isVehicleStopped(0.1) && reached_stop_point; + const auto is_yielding_now = + planner_data_->isVehicleStopped(planner_param_.ego_yield_query_stop_duration) && + reached_stop_point; if (!is_yielding_now) { continue; }