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 4ea3229d46e56..2cc5260e4ea9b 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 @@ -10,7 +10,7 @@ stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk # param for ego velocity - slow_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) + min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph) diff --git a/planning/behavior_velocity_planner/config/crosswalk.param.yaml b/planning/behavior_velocity_planner/config/crosswalk.param.yaml index 4ea3229d46e56..2cc5260e4ea9b 100644 --- a/planning/behavior_velocity_planner/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_planner/config/crosswalk.param.yaml @@ -10,7 +10,7 @@ stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk # param for ego velocity - slow_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) + min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph) 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 3fdd9509caba1..013757976d473 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 @@ -68,7 +68,7 @@ class CrosswalkModule : public SceneModuleInterface double stop_line_margin; double stop_position_threshold; // param for ego velocity - double slow_velocity; + float min_slow_down_velocity; double max_slow_down_jerk; double max_slow_down_accel; double no_relax_velocity; 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 58e8dc259a9eb..569f8217eff15 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp @@ -103,7 +103,7 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.stop_position_threshold = node.declare_parameter(ns + ".stop_position_threshold", 1.0); // param for ego velocity - cp.slow_velocity = node.declare_parameter(ns + ".slow_velocity", 5.0 / 3.6); + cp.min_slow_down_velocity = node.declare_parameter(ns + ".min_slow_down_velocity", 5.0 / 3.6); cp.max_slow_down_jerk = node.declare_parameter(ns + ".max_slow_down_jerk", -0.7); cp.max_slow_down_accel = node.declare_parameter(ns + ".max_slow_down_accel", -2.5); cp.no_relax_velocity = node.declare_parameter(ns + ".no_relax_velocity", 2.78); 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 f34ceee1178a5..d61ea7f7c2d27 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 @@ -363,20 +363,23 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto setSafe(!nearest_stop_point); - const auto target_velocity = - nearest_stop_point ? calcTargetVelocity(nearest_stop_point.get().second, ego_path) : 0.0; - if (isActivated()) { - if (1e-3 < target_velocity) { - insertDecelPoint(nearest_stop_point.get(), target_velocity, *path); + if (!nearest_stop_point) { + return true; } + + const auto target_velocity = calcTargetVelocity(nearest_stop_point.get().second, ego_path); + insertDecelPoint( + nearest_stop_point.get(), std::max(planner_param_.min_slow_down_velocity, target_velocity), + *path); + return true; } if (nearest_stop_point) { - insertDecelPoint(nearest_stop_point.get(), target_velocity, *path); + insertDecelPoint(nearest_stop_point.get(), 0.0, *path); } else if (rtc_stop_point) { - insertDecelPoint(rtc_stop_point.get(), target_velocity, *path); + insertDecelPoint(rtc_stop_point.get(), 0.0, *path); } RCLCPP_INFO_EXPRESSION( @@ -442,13 +445,6 @@ boost::optional> CrosswalkModule::findNea bool found_pedestrians = false; bool found_stuck_vehicle = false; - const bool external_slowdown = isTargetExternalInputStatus(CrosswalkStatus::SLOWDOWN); - const bool external_stop = isTargetExternalInputStatus(CrosswalkStatus::STOP); - const bool external_go = isTargetExternalInputStatus(CrosswalkStatus::GO); - - if (external_go) { - return {}; - } const auto crosswalk_attention_range = getAttentionRange(ego_path); const auto & ego_pos = planner_data_->current_pose.pose.position; @@ -540,8 +536,7 @@ boost::optional> CrosswalkModule::findNea } } - const auto need_to_stop = - found_pedestrians || found_stuck_vehicle || external_stop || external_slowdown; + const auto need_to_stop = found_pedestrians || found_stuck_vehicle; if (!need_to_stop) { return {}; } @@ -646,18 +641,6 @@ void CrosswalkModule::insertDecelPoint( float CrosswalkModule::calcTargetVelocity( const PathPointWithLaneId & stop_point, const PathWithLaneId & ego_path) const { - if (!isActivated()) { - return 0.0; - } - - if (isTargetExternalInputStatus(CrosswalkStatus::SLOWDOWN)) { - return planner_param_.slow_velocity; - } - - if (isTargetExternalInputStatus(CrosswalkStatus::STOP)) { - return 0.0; - } - const auto & max_jerk = planner_param_.max_slow_down_jerk; const auto & max_accel = planner_param_.max_slow_down_accel; const auto & ego_pos = planner_data_->current_pose.pose.position;