feat(behavior_velocity_planner): parameterize ego_yield_query_stop_du…

…ration for crosswalk module (autowarefoundation#2346)

feat: parameterize ego_yield_query_stop_duration for crosswalk module

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

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

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

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
 

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;

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);

planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp

@@ -403,7 +403,9 @@ boost::optional<geometry_msgs::msg::Point> 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;
       }