diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 5afec90283c0e..7ea0f6d245837 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -161,7 +161,9 @@ class GoalPlannerModule : public SceneModuleInterface // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. - double approximate_pull_over_distance_{20.0}; + const double approximate_pull_over_distance_{20.0}; + // ego may exceed the stop distance, so add a buffer + const double stop_distance_buffer_{2.0}; // for parking policy bool left_side_parking_{true}; @@ -186,7 +188,8 @@ class GoalPlannerModule : public SceneModuleInterface // stop or decelerate void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const; - void decelerateBeforeSearchStart(const Pose & search_start_pose, PathWithLaneId & path) const; + void decelerateBeforeSearchStart( + const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); boost::optional calcFeasibleDecelDistance(const double target_velocity) const; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 8392cab32ca4b..fc059c60cc83a 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -539,13 +539,14 @@ void GoalPlannerModule::selectSafePullOverPath() // decelerate before the search area start if (status_.is_safe) { - const auto search_start_pose = calcLongitudinalOffsetPose( + const auto search_start_offset_pose = calcLongitudinalOffsetPose( status_.pull_over_path->getFullPath().points, refined_goal_pose_.position, - -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front); + -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - + approximate_pull_over_distance_); auto & first_path = status_.pull_over_path->partial_paths.front(); - if (search_start_pose) { - decelerateBeforeSearchStart(*search_start_pose, first_path); + if (search_start_offset_pose) { + decelerateBeforeSearchStart(*search_start_offset_pose, first_path); } else { // if already passed the search start pose, set pull_over_velocity to first_path. for (auto & p : first_path.points) { @@ -577,8 +578,10 @@ void GoalPlannerModule::setLanes() void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { if (status_.is_safe) { - // safe: use pull over path - status_.stop_pose.reset(); + // clear stop pose when the path is safe and activated + if (isActivated()) { + status_.stop_pose.reset(); + } // keep stop if not enough time passed, // because it takes time for the trajectory to be reflected @@ -904,24 +907,26 @@ PathWithLaneId GoalPlannerModule::generateStopPath() // stop point priority is // 1. actual start pose // 2. closest candidate start pose - // 3. search start pose + // 3. pose offset by approximate_pull_over_distance_ from search start pose. // (In the case of the curve lane, the position is not aligned due to the // difference between the outer and inner sides) // 4. feasible stop - const auto search_start_pose = calcLongitudinalOffsetPose( + const auto search_start_offset_pose = calcLongitudinalOffsetPose( reference_path.points, refined_goal_pose_.position, - -parameters_->backward_goal_search_length - common_parameters.base_link2front); - if (!status_.is_safe && !closest_start_pose_ && !search_start_pose) { + -parameters_->backward_goal_search_length - common_parameters.base_link2front - + approximate_pull_over_distance_); + if (!status_.is_safe && !closest_start_pose_ && !search_start_offset_pose) { return generateFeasibleStopPath(); } - const Pose stop_pose = - status_.is_safe ? status_.pull_over_path->start_pose - : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_pose); + + const Pose stop_pose = status_.is_safe ? status_.pull_over_path->start_pose + : (closest_start_pose_ ? closest_start_pose_.value() + : *search_start_offset_pose); // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); const auto min_stop_distance = calcFeasibleDecelDistance(0.0); - if (min_stop_distance && ego_to_stop_distance < *min_stop_distance) { + if (min_stop_distance && ego_to_stop_distance + stop_distance_buffer_ < *min_stop_distance) { return generateFeasibleStopPath(); } @@ -930,8 +935,8 @@ PathWithLaneId GoalPlannerModule::generateStopPath() status_.stop_pose = stop_pose; // slow down before the search area. - if (search_start_pose) { - decelerateBeforeSearchStart(*search_start_pose, reference_path); + if (search_start_offset_pose) { + decelerateBeforeSearchStart(*search_start_offset_pose, reference_path); } else { // if already passed the search start pose, set pull_over_velocity to reference_path. for (auto & p : reference_path.points) { @@ -1132,15 +1137,19 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c const Pose & current_pose = planner_data_->self_odometry->pose.pose; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - // once stopped, the vehicle cannot start again if start_pose is close. + // when the path is separated and start_pose is close, + // once stopped, the vehicle cannot start again. // so need enough distance to restart. // distance to restart should be less than decide_path_distance. // otherwise, the goal would change immediately after departure. + const bool is_separated_path = status_.pull_over_path->partial_paths.size() > 1; constexpr double eps_vel = 0.01; const double distance_to_start = calcSignedArcLength( pull_over_path.getFullPath().points, current_pose.position, pull_over_path.start_pose.position); const double distance_to_restart = parameters_->decide_path_distance / 2; - if (std::abs(current_vel) < eps_vel && distance_to_start < distance_to_restart) { + if ( + is_separated_path && std::abs(current_vel) < eps_vel && + distance_to_start < distance_to_restart) { return false; } @@ -1149,7 +1158,7 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c return false; } - if (distance_to_start < *current_to_stop_distance) { + if (distance_to_start + stop_distance_buffer_ < *current_to_stop_distance) { return false; } @@ -1250,7 +1259,7 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith } void GoalPlannerModule::decelerateBeforeSearchStart( - const Pose & search_start_pose, PathWithLaneId & path) const + const Pose & search_start_offset_pose, PathWithLaneId & path) const { const double pull_over_velocity = parameters_->pull_over_velocity; const Pose & current_pose = planner_data_->self_odometry->pose.pose; @@ -1258,7 +1267,8 @@ void GoalPlannerModule::decelerateBeforeSearchStart( // slow down before the search area. const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); if (min_decel_distance) { - const double distance_to_search_start = calcSignedArcLengthFromEgo(path, search_start_pose); + const double distance_to_search_start = + calcSignedArcLengthFromEgo(path, search_start_offset_pose); const double distance_to_decel = std::max(*min_decel_distance, distance_to_search_start - approximate_pull_over_distance_); insertDecelPoint(current_pose.position, distance_to_decel, pull_over_velocity, path.points);