From 99a45bd4541a87a70b8ae0372675e3167cc25f29 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Thu, 25 Jan 2024 15:28:41 +0900 Subject: [PATCH] feat(crosswalk)!: improve stuck prevention on crosswalk (#6150) * improve algorithm * sync param Signed-off-by: Yuki Takagi --- .../config/crosswalk.param.yaml | 2 +- .../src/manager.cpp | 4 +- .../src/scene_crosswalk.cpp | 54 +++++++++---------- .../src/scene_crosswalk.hpp | 2 +- 4 files changed, 31 insertions(+), 31 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index aa5dfed1e4bc5..29079b99b6718 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -30,7 +30,7 @@ enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection stuck_vehicle_velocity: 1.0 # [m/s] threshold velocity whether other vehicles are assumed to be stuck or not. max_stuck_vehicle_lateral_offset: 2.0 # [m] The feature does not handle the vehicles farther than this value to the ego's path. - stuck_vehicle_attention_range: 10.0 # [m] Ego does not enter the crosswalk area if a stuck vehicle exists within this distance from the end of the crosswalk on the ego's path. + required_clearance: 6.0 # [m] clearance to be secured between the ego and the ahead vehicle min_acc: -1.0 # min acceleration [m/ss] min_jerk: -1.0 # min jerk [m/sss] max_jerk: 1.0 # max jerk [m/sss] diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index dc3ce32be8505..094369b0a7a3c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -68,8 +68,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); cp.max_stuck_vehicle_lateral_offset = getOrDeclareParameter(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); - cp.stuck_vehicle_attention_range = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_attention_range"); + cp.required_clearance = + getOrDeclareParameter(node, ns + ".stuck_vehicle.required_clearance"); cp.min_acc_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.min_acc"); cp.max_jerk_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.max_jerk"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 48ae5ff03b8bd..cee6975df3155 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -906,48 +906,48 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( continue; } - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pos); + const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pose.position); if (p.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { continue; } - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto ego_vel = planner_data_->current_velocity->twist.linear.x; - const auto ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; - - const double near_attention_range = - calcSignedArcLength(ego_path.points, ego_pos, path_intersects.back()); - const double far_attention_range = near_attention_range + p.stuck_vehicle_attention_range; - - const auto dist_ego2obj = calcSignedArcLength(ego_path.points, ego_pos, obj_pos); - - if (near_attention_range < dist_ego2obj && dist_ego2obj < far_attention_range) { - // Plan STOP considering min_acc, max_jerk and min_jerk. - const auto min_feasible_dist_ego2stop = calcDecelDistWithJerkAndAccConstraints( - ego_vel, 0.0, ego_acc, p.min_acc_for_stuck_vehicle, p.max_jerk_for_stuck_vehicle, - p.min_jerk_for_stuck_vehicle); - if (!min_feasible_dist_ego2stop) { - continue; + // check if STOP is required + const double crosswalk_front_to_obj_rear = + calcSignedArcLength(ego_path.points, path_intersects.front(), obj_pose.position) - + object.shape.dimensions.x / 2.0; + const double crosswalk_back_to_obj_rear = + calcSignedArcLength(ego_path.points, path_intersects.back(), obj_pose.position) - + object.shape.dimensions.x / 2.0; + const double required_space_length = + planner_data_->vehicle_info_.vehicle_length_m + planner_param_.required_clearance; + + if (crosswalk_front_to_obj_rear > 0.0 && crosswalk_back_to_obj_rear < required_space_length) { + // If there exists at least one vehicle ahead, plan STOP considering min_acc, max_jerk and + // min_jerk. Note that nearest search is not required because the stop pose independents to + // the vehicles. + const auto braking_distance = calcDecelDistWithJerkAndAccConstraints( + planner_data_->current_velocity->twist.linear.x, 0.0, + planner_data_->current_acceleration->accel.accel.linear.x, p.min_acc_for_stuck_vehicle, + p.max_jerk_for_stuck_vehicle, p.min_jerk_for_stuck_vehicle); + if (!braking_distance) { + return {}; } + const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position); - const double feasible_dist_ego2stop = std::max(*min_feasible_dist_ego2stop, dist_ego2stop); + const double feasible_dist_ego2stop = std::max(*braking_distance, dist_ego2stop); const double dist_to_ego = calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos); - const auto feasible_stop_pose = calcLongitudinalOffsetPose(ego_path.points, 0, dist_to_ego + feasible_dist_ego2stop); if (!feasible_stop_pose) { - continue; + return {}; } - setObjectsOfInterestData( - object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); - - // early return may not appropriate because the nearest in range object should be handled - return createStopFactor(*feasible_stop_pose, {obj_pos}); + setObjectsOfInterestData(obj_pose, object.shape, ColorName::RED); + return createStopFactor(*feasible_stop_pose, {obj_pose.position}); } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 247907ad2dc80..ed1e342df9f7a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -124,7 +124,7 @@ class CrosswalkModule : public SceneModuleInterface bool enable_stuck_check_in_intersection{false}; double stuck_vehicle_velocity; double max_stuck_vehicle_lateral_offset; - double stuck_vehicle_attention_range; + double required_clearance; double min_acc_for_stuck_vehicle; double max_jerk_for_stuck_vehicle; double min_jerk_for_stuck_vehicle;