diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index f8b9e5dbf1327..67b923a8d4cd8 100644 --- 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 index aeff5e27cea7b..ad56df5f76944 100644 --- 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>(node, ns + ".pass_judge.ego_pass_later_margin_y"); cp.ego_pass_later_additional_margin = getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_later_additional_margin"); + cp.ego_min_assumed_speed = + getOrDeclareParameter(node, ns + ".pass_judge.ego_min_assumed_speed"); cp.max_offset_to_crosswalk_for_yield = getOrDeclareParameter( 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 index 7caf8651d8b33..dff47083ba018 100644 --- 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 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 index d2468e7f31aa7..59371dc44d41a 100644 --- 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 ego_pass_later_margin_x; std::vector 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;