From 0c7a31bc44894460f626b46e0a488b19cf079196 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 5 Sep 2024 21:05:16 +0900 Subject: [PATCH] add new stop pos Signed-off-by: Yuki Takagi --- .../src/scene_crosswalk.cpp | 113 ++++++++++-------- 1 file changed, 66 insertions(+), 47 deletions(-) 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 caf9098d66708..92f0503baeaa7 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 @@ -341,14 +341,6 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( 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 {}; - } // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop @@ -360,60 +352,87 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const std::optional ego_crosswalk_passage_direction = findEgoPassageDirectionAlongPath(sparse_resample_path); for (const auto & object : object_info_manager_.getObject()) { - const auto & collision_point_opt = object.collision_point; - if (collision_point_opt) { - const auto & collision_point = collision_point_opt.value(); - const auto & collision_state = object.collision_state; - if (collision_state != CollisionState::YIELD) { + if (!object.collision_point || object.collision_state != CollisionState::YIELD) { + continue; + } + const auto & collision_point = object.collision_point.value(); + + if ( + isVehicleType(object.classification) && ego_crosswalk_passage_direction && + collision_point.crosswalk_passage_direction) { + const double direction_diff = autoware::universe_utils::normalizeRadian( + collision_point.crosswalk_passage_direction.value() - + ego_crosswalk_passage_direction.value()); + if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) { continue; } - if ( - isVehicleType(object.classification) && ego_crosswalk_passage_direction && - collision_point.crosswalk_passage_direction) { - const double direction_diff = autoware::universe_utils::normalizeRadian( - collision_point.crosswalk_passage_direction.value() - - ego_crosswalk_passage_direction.value()); - if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) { - continue; - } - } + } - stop_factor_points.push_back(object.position); + 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); - } + 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); } } - // Check if stop is required if (!nearest_stop_info) { return {}; } - // Check if the ego should stop at the stop line or the other points. - const bool stop_at_stop_line = dist_ego_to_default_stop_opt.has_value() && - dist_ego_to_default_stop_opt.value() < nearest_stop_info->second && - nearest_stop_info->second < dist_ego_to_default_stop_opt.value() + - planner_param_.far_object_threshold; + // decide stop position including cancel + const auto stop_pose_against_nearest_ped_opt = calcLongitudinalOffsetPose( + ego_path.points, nearest_stop_info->first, + -base_link2front - planner_param_.stop_distance_from_object); + if (!stop_pose_against_nearest_ped_opt) { + RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin."); + return {}; + } + + const auto braking_dist_strong = [&]() { + const auto braking_dist_strong_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); + return braking_dist_strong_opt ? *braking_dist_strong_opt : 0.0; + }(); + const auto braking_dist_weak = [&]() { + const auto braking_dist_weak_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); + return braking_dist_weak_opt ? *braking_dist_weak_opt : 0.0; + }(); - if (stop_at_stop_line) { - // Stop at the stop line - if (default_stop_pose) { - return createStopFactor(*default_stop_pose, stop_factor_points); + const auto stop_pose_without_acc_constraint = [&]() { + if ( + !default_stop_pose.has_value() || + nearest_stop_info->second < dist_ego_to_default_stop_opt.value() || + dist_ego_to_default_stop_opt.value() + planner_param_.far_object_threshold < + nearest_stop_info->second) { + return stop_pose_against_nearest_ped_opt.value(); + } else { + return default_stop_pose.value(); } - } 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); + }(); + + if ( + braking_dist_weak < + calcSignedArcLength(ego_path.points, ego_pos, stop_pose_without_acc_constraint.position)) { + return createStopFactor(stop_pose_without_acc_constraint, stop_factor_points); + } else if (braking_dist_weak < nearest_stop_info->second) { + const auto stop_pose_with_weak_brake = + calcLongitudinalOffsetPose(ego_path.points, ego_pos, braking_dist_weak); + if (stop_pose_with_weak_brake.has_value()) { + return createStopFactor(stop_pose_with_weak_brake.value(), stop_factor_points); } + } else if (braking_dist_strong < nearest_stop_info->second) { + return createStopFactor(stop_pose_against_nearest_ped_opt.value(), stop_factor_points); } + // abort stop return {}; }