feat(crosswalk)!: change ego min assumed speed (autowarefoundation#6904)

Signed-off-by: Yuki Takagi <[email protected]> Signed-off-by: vividf <[email protected]>
vividf · May 16, 2024 · 10c3636 · 10c3636
1 parent e848a63
commit 10c3636
Show file tree

Hide file tree

Showing 4 changed files with 5 additions and 3 deletions.
diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml
@@ -43,6 +43,7 @@
         ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition)
         ego_pass_later_margin_y: [13.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition)
         ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering
+        ego_min_assumed_speed: 2.0 # [m/s] assumed speed to calculate the time to collision point
 
         no_stop_decision: # parameters to determine if it is safe to attempt stopping before the crosswalk
           max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk.

diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp
@@ -90,6 +90,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<std::vector<double>>(node, ns + ".pass_judge.ego_pass_later_margin_y");
   cp.ego_pass_later_additional_margin =
     getOrDeclareParameter<double>(node, ns + ".pass_judge.ego_pass_later_additional_margin");
+  cp.ego_min_assumed_speed =
+    getOrDeclareParameter<double>(node, ns + ".pass_judge.ego_min_assumed_speed");
   cp.max_offset_to_crosswalk_for_yield = getOrDeclareParameter<double>(
     node, ns + ".pass_judge.no_stop_decision.max_offset_to_crosswalk_for_yield");
   cp.min_acc_for_no_stop_decision =

diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -810,8 +810,6 @@ CollisionPoint CrosswalkModule::createCollisionPoint(
   const geometry_msgs::msg::Vector3 & obj_vel,
   const std::optional<double> object_crosswalk_passage_direction) const
 {
-  constexpr double min_ego_velocity = 1.38;  // [m/s]
-
   const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y);
   const auto velocity = std::max(planner_param_.min_object_velocity, estimated_velocity);
 
@@ -824,7 +822,7 @@ CollisionPoint CrosswalkModule::createCollisionPoint(
   // Hence, here, we use the length that would be appropriate for the ego_pass_first judge.
   collision_point.time_to_collision =
     std::max(0.0, dist_ego2cp - planner_data_->vehicle_info_.min_longitudinal_offset_m) /
-    std::max(ego_vel.x, min_ego_velocity);
+    std::max(ego_vel.x, planner_param_.ego_min_assumed_speed);
   collision_point.time_to_vehicle = std::max(0.0, dist_obj2cp) / velocity;
 
   return collision_point;

diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -135,6 +135,7 @@ class CrosswalkModule : public SceneModuleInterface
     std::vector<double> ego_pass_later_margin_x;
     std::vector<double> ego_pass_later_margin_y;
     double ego_pass_later_additional_margin;
+    double ego_min_assumed_speed;
     double max_offset_to_crosswalk_for_yield;
     double min_acc_for_no_stop_decision;
     double max_jerk_for_no_stop_decision;