From b000f6eca0954a1e8eecf8198182800eb7e6b9dd Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 13 Sep 2024 20:02:28 +0900 Subject: [PATCH] feat(crosswalk)!: update stop position caluculation (#8853) Signed-off-by: Yuki Takagi --- .../README.md | 29 +++- .../config/crosswalk.param.yaml | 14 +- .../src/manager.cpp | 16 +- .../src/scene_crosswalk.cpp | 161 +++++++++++++----- .../src/scene_crosswalk.hpp | 12 +- 5 files changed, 172 insertions(+), 60 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md index 3cf62b92bb659..857f590104aea 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md @@ -12,7 +12,6 @@ This module judges whether the ego should stop in front of the crosswalk in orde ```plantuml @startuml -skinparam monochrome true title modifyPathVelocity start @@ -38,8 +37,36 @@ group apply stop :planStop; endif end group +stop +@enduml +``` +```plantuml +@startuml + +title checkStopForCrosswalkUsers +start +group calculate the candidate stop + :pick the closest stop point against the pedestrians and stop point on map as the preferred stop; + if (the weak brake distance is closer than the preferred stop?) then (yes) + :plan to stop at the preferred stop; + else (no) + if (the weak brake distance is closer than the limit stop position against the nearest pedestrian?) then (yes) + :plan to stop by the weak brake distance; + else (no) + :plan to stop at the limit stop position against the nearest pedestrian; + endif + endif +end group +group check if the candidate stop pose is acceptable for braking distance + if (the stop pose candidate is closer than the acceptable stop dist?) then (yes) + :abort to stop.; + else (no) + :plan to stop at the candidate stop pose; + endif +end group stop + @enduml ``` diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 9ff4ccc808b7c..bc0d4f613f10b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -16,7 +16,10 @@ # For the case where the crosswalk width is very wide far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object). # For the case where the stop position is determined according to the object position. - stop_distance_from_object: 3.0 # [m] the vehicle decelerates to be able to stop in front of object with margin + stop_distance_from_object_preferred: 3.0 # [m] + stop_distance_from_object_limit: 3.0 # [m] + min_acc_preferred: -1.0 # min acceleration [m/ss] + min_jerk_preferred: -1.0 # min jerk [m/sss] # params for ego's slow down velocity. These params are not used for the case of "enable_rtc: false". slow_down: @@ -45,11 +48,10 @@ 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. - min_acc: -1.0 # min acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] + no_stop_decision: # parameters to determine stop cancel. {-$overrun_threshold_length + f($min_acc, $min_jerk)} is compared against distance to stop pose. + min_acc: -1.5 # min acceleration [m/ss] + min_jerk: -1.5 # min jerk [m/sss] + overrun_threshold_length: 1.0 # [m] required to avoid giving up against backward movement of the stop position due to approaching pedestrians, etc. stop_object_velocity_threshold: 0.25 # [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) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index 0bd708f7b1705..949a8de361409 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -47,12 +47,18 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) // param for stop position cp.stop_distance_from_crosswalk = getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_crosswalk"); - cp.stop_distance_from_object = - getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object"); + cp.stop_distance_from_object_preferred = + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object_preferred"); + cp.stop_distance_from_object_limit = + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object_limit"); cp.far_object_threshold = getOrDeclareParameter(node, ns + ".stop_position.far_object_threshold"); cp.stop_position_threshold = getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); + cp.min_acc_preferred = + getOrDeclareParameter(node, ns + ".stop_position.min_acc_preferred"); + cp.min_jerk_preferred = + getOrDeclareParameter(node, ns + ".stop_position.min_jerk_preferred"); // param for restart suppression cp.min_dist_to_stop_for_restart_suppression = @@ -98,14 +104,12 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) 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 = getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.min_acc"); - cp.max_jerk_for_no_stop_decision = - getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.max_jerk"); cp.min_jerk_for_no_stop_decision = getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.min_jerk"); + cp.overrun_threshold_length_for_no_stop_decision = getOrDeclareParameter( + node, ns + ".pass_judge.no_stop_decision.overrun_threshold_length"); cp.stop_object_velocity = getOrDeclareParameter(node, ns + ".pass_judge.stop_object_velocity_threshold"); cp.min_object_velocity = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 639ee214bca9c..d7e8dbda57f3b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -316,15 +316,9 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, const geometry_msgs::msg::Point & last_path_point_on_crosswalk, - const std::optional & default_stop_pose) + const std::optional & default_stop_pose_opt) { const auto & ego_pos = planner_data_->current_odometry->pose.position; - const double ego_vel = planner_data_->current_velocity->twist.linear.x; - const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; - - const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto dist_ego_to_stop = - calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose->position); // Calculate attention range for crosswalk const auto crosswalk_attention_range = getAttentionRange( @@ -334,28 +328,20 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const auto attention_area = getAttentionArea(sparse_resample_path, crosswalk_attention_range); // Update object state + // This exceptional handling should be done in update(), but is compromised by history + const double dist_default_stop = + default_stop_pose_opt.has_value() + ? calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position) + : std::numeric_limits::max(); updateObjectState( - dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area); - - // Check if ego moves forward enough to ignore yield. - const auto & p = planner_param_; - const double dist_ego2crosswalk = - calcSignedArcLength(ego_path.points, ego_pos, first_path_point_on_crosswalk); - const auto braking_distance_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( - ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision, - p.min_jerk_for_no_stop_decision); - const double braking_distance = braking_distance_opt ? *braking_distance_opt : 0.0; - if ( - dist_ego2crosswalk - base_link2front - braking_distance < p.max_offset_to_crosswalk_for_yield) { - return {}; - } + dist_default_stop, sparse_resample_path, crosswalk_attention_range, attention_area); // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop auto isVehicleType = [](const uint8_t label) { return label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE; }; - std::optional> nearest_stop_info; + std::optional dist_nearest_cp; std::vector stop_factor_points; const std::optional ego_crosswalk_passage_direction = findEgoPassageDirectionAlongPath(sparse_resample_path); @@ -381,39 +367,126 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( stop_factor_points.push_back(object.position); const auto dist_ego2cp = - calcSignedArcLength(sparse_resample_path.points, ego_pos, collision_point.collision_point) - - planner_param_.stop_distance_from_object; - if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) { - nearest_stop_info = - std::make_pair(collision_point.collision_point, dist_ego2cp - base_link2front); + calcSignedArcLength(sparse_resample_path.points, ego_pos, collision_point.collision_point); + if (!dist_nearest_cp || dist_ego2cp < dist_nearest_cp) { + dist_nearest_cp = dist_ego2cp; } } } + if (!dist_nearest_cp) { + return {}; + } - // Check if stop is required - if (!nearest_stop_info) { + const auto decided_stop_pose_opt = + calcStopPose(ego_path, dist_nearest_cp.value(), default_stop_pose_opt); + if (!decided_stop_pose_opt.has_value()) { return {}; } + return createStopFactor(decided_stop_pose_opt.value(), stop_factor_points); +} - // Check if the ego should stop at the stop line or the other points. - const bool stop_at_stop_line = - dist_ego_to_stop < nearest_stop_info->second && - nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold; +std::optional CrosswalkModule::calcStopPose( + const PathWithLaneId & ego_path, double dist_nearest_cp, + const std::optional & default_stop_pose_opt) +{ + struct StopCandidate + { + geometry_msgs::msg::Pose pose; + double dist; + }; + + const auto & p = planner_param_; + const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto & ego_pos = planner_data_->current_odometry->pose.position; + const double ego_vel_non_negative = + std::max(0.0, planner_data_->current_velocity->twist.linear.x); + const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; - if (stop_at_stop_line) { - // Stop at the stop line - if (default_stop_pose) { - return createStopFactor(*default_stop_pose, stop_factor_points); + const auto default_stop_opt = [&]() -> std::optional { + if (!default_stop_pose_opt.has_value()) return std::nullopt; + return StopCandidate{ + default_stop_pose_opt.value(), + calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position)}; + }(); + + const auto ped_stop_pref_opt = [&]() -> std::optional { + const double dist = + dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object_preferred; + const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist); + if (!pose_opt.has_value()) return std::nullopt; + return StopCandidate{pose_opt.value(), dist}; + }(); + + const auto without_acc_pref_stop_opt = [&]() -> std::optional { + if (!ped_stop_pref_opt.has_value()) { + RCLCPP_INFO(logger_, "Failure to calculate pref_stop."); + return std::nullopt; + } else if ( + default_stop_opt.has_value() && ped_stop_pref_opt->dist > default_stop_opt->dist && + ped_stop_pref_opt->dist < default_stop_opt->dist + planner_param_.far_object_threshold) { + return default_stop_opt; + } else { + return ped_stop_pref_opt; } - } else { - const auto stop_pose = calcLongitudinalOffsetPose( - ego_path.points, nearest_stop_info->first, - -base_link2front - planner_param_.stop_distance_from_object); - if (stop_pose) { - return createStopFactor(*stop_pose, stop_factor_points); + }(); + + const auto ped_stop_limit = [&]() -> std::optional { + const double dist = + dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object_limit; + const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist); + if (!pose_opt.has_value()) return std::nullopt; + return StopCandidate{pose_opt.value(), dist}; + }(); + if (!ped_stop_limit.has_value()) { + RCLCPP_WARN( + logger_, + "Stop is canceled. " + "Failure to calculate stop_pose against the nearest pedestrian with a limit margin."); + return std::nullopt; + } + + const auto weak_brk_stop = [&]() -> std::optional { + const auto dist_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( + ego_vel_non_negative, 0.0, ego_acc, p.min_acc_preferred, 10.0, p.min_jerk_preferred); + if (!dist_opt.has_value()) return std::nullopt; + const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist_opt.value()); + if (!pose_opt.has_value()) return std::nullopt; + return StopCandidate{pose_opt.value(), dist_opt.value()}; + }(); + if (!weak_brk_stop.has_value()) { + RCLCPP_ERROR(logger_, "Failure to calculate braking distance. Stop will be canceled."); + return std::nullopt; + } + + const auto selected_stop = [&]() { + if ( + without_acc_pref_stop_opt.has_value() && + weak_brk_stop->dist < without_acc_pref_stop_opt->dist) { + return without_acc_pref_stop_opt.value(); + } else if (weak_brk_stop->dist < ped_stop_limit->dist) { + return weak_brk_stop.value(); + } else { + return ped_stop_limit.value(); } + }(); + + const double strong_brk_dist = [&]() { + const auto strong_brk_dist_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( + ego_vel_non_negative, 0.0, ego_acc, p.min_acc_for_no_stop_decision, 10.0, + p.min_jerk_for_no_stop_decision); + return strong_brk_dist_opt ? strong_brk_dist_opt.value() : 0.0; + }(); + if (selected_stop.dist < strong_brk_dist - p.overrun_threshold_length_for_no_stop_decision) { + RCLCPP_INFO( + logger_, + "Abandon to stop. " + "Can not stop against the nearest pedestrian with a specified deceleration. " + "dist to stop: %f, braking distance: %f", + selected_stop.dist, strong_brk_dist); + return std::nullopt; } - return {}; + + return std::make_optional(selected_stop.pose); } std::pair CrosswalkModule::getAttentionRange( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index be2a7d199c666..cd4b8eb46af6c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -112,10 +112,13 @@ class CrosswalkModule : public SceneModuleInterface { bool show_processing_time; // param for stop position - double stop_distance_from_object; + double stop_distance_from_object_preferred; + double stop_distance_from_object_limit; double stop_distance_from_crosswalk; double far_object_threshold; double stop_position_threshold; + double min_acc_preferred; + double min_jerk_preferred; // param for restart suppression double min_dist_to_stop_for_restart_suppression; double max_dist_to_stop_for_restart_suppression; @@ -140,10 +143,9 @@ class CrosswalkModule : public SceneModuleInterface 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; double min_jerk_for_no_stop_decision; + double overrun_threshold_length_for_no_stop_decision; double stop_object_velocity; double min_object_velocity; bool disable_yield_for_new_stopped_object; @@ -352,6 +354,10 @@ class CrosswalkModule : public SceneModuleInterface const PathWithLaneId & ego_path, const geometry_msgs::msg::Point & first_path_point_on_crosswalk) const; + std::optional calcStopPose( + const PathWithLaneId & ego_path, double dist_nearest_cp, + const std::optional & default_stop_pose_opt); + std::optional checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, const geometry_msgs::msg::Point & first_path_point_on_crosswalk,