From de3b2b5f51c086ad9b178a7c65fe03cd63bb62d6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 27 Aug 2024 14:04:32 +0900 Subject: [PATCH] fix(goal_planner): fix zero velocity in middle of path (#8563) * fix(goal_planner): fix zero velocity in middle of path Signed-off-by: kosuke55 * add comment Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../util.hpp | 27 ++++++++++++++++--- .../src/goal_planner_module.cpp | 10 ++++++- .../src/shift_pull_over.cpp | 6 ++++- .../src/util.cpp | 11 +++++--- 4 files changed, 45 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 9e4d6d2f85f50..b527fa3e1dac0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -84,12 +84,31 @@ bool isReferencePath( std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose); PathWithLaneId cropForwardPoints( const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length); + +/** + * @brief extend target_path by extend_length + * @param target_path original target path to extend + * @param reference_path reference path to extend + * @param extend_length length to extend + * @param remove_connected_zero_velocity flag to remove zero velocity if the last point of + * target_path has zero velocity + * @return extended path + */ PathWithLaneId extendPath( - const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, - const double extend_length); + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const double extend_length, const bool remove_connected_zero_velocity); +/** + * @brief extend target_path to extend_pose + * @param target_path original target path to extend + * @param reference_path reference path to extend + * @param extend_pose pose to extend + * @param remove_connected_zero_velocity flag to remove zero velocity if the last point of + * target_path has zero velocity + * @return extended path + */ PathWithLaneId extendPath( - const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, - const Pose & extend_pose); + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const Pose & extend_pose, const bool remove_connected_zero_velocity); std::vector createPathFootPrints( const PathWithLaneId & path, const double base_to_front, const double base_to_rear, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index c67983993de01..4d4d31446aa3f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -283,6 +283,10 @@ void GoalPlannerModule::onTimer() RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; } + // TODO(someone): The generated path inherits the velocity of the path of the previous module. + // Therefore, if the velocity of the path of the previous module changes (e.g. stop points are + // inserted, deleted), the path should be regenerated. + return false; }); if (!need_update) { @@ -1628,8 +1632,12 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const const double s_end = s_current + common_parameters.forward_path_length; return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); }); + + // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it by + // setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is the + // role of the goal planner, and the intermediate zero velocity after extension is unnecessary. const auto extended_prev_path = goal_planner_utils::extendPath( - getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length); + getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length, true); // calculate search start offset pose from the closest goal candidate pose with // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp index 23ca76432859d..fd6b0cb639387 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -148,8 +148,12 @@ std::optional ShiftPullOver::generatePullOverPath( lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length > lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length; if (extend_previous_module_path) { // case1 + // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it + // by setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is + // the role of the goal planner, and the intermediate zero velocity after extension is + // unnecessary. return goal_planner_utils::extendPath( - prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose); + prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose, true); } else { // case2 return goal_planner_utils::cropPath(prev_module_path, shift_end_pose); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index f4b11f4f8e46d..6bceec531b653 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -389,7 +389,7 @@ PathWithLaneId cropForwardPoints( PathWithLaneId extendPath( const PathWithLaneId & target_path, const PathWithLaneId & reference_path, - const double extend_length) + const double extend_length, const bool remove_connected_zero_velocity) { const auto & target_terminal_pose = target_path.points.back().point.pose; @@ -409,6 +409,11 @@ PathWithLaneId extendPath( } auto extended_path = target_path; + auto & target_terminal_vel = extended_path.points.back().point.longitudinal_velocity_mps; + if (remove_connected_zero_velocity && target_terminal_vel < 0.01) { + target_terminal_vel = clipped_path.points.front().point.longitudinal_velocity_mps; + } + const auto start_point = std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) { const bool is_forward = @@ -427,7 +432,7 @@ PathWithLaneId extendPath( PathWithLaneId extendPath( const PathWithLaneId & target_path, const PathWithLaneId & reference_path, - const Pose & extend_pose) + const Pose & extend_pose, const bool remove_connected_zero_velocity) { const auto & target_terminal_pose = target_path.points.back().point.pose; const size_t target_path_terminal_idx = autoware::motion_utils::findNearestSegmentIndex( @@ -435,7 +440,7 @@ PathWithLaneId extendPath( const double extend_distance = autoware::motion_utils::calcSignedArcLength( reference_path.points, target_path_terminal_idx, extend_pose.position); - return extendPath(target_path, reference_path, extend_distance); + return extendPath(target_path, reference_path, extend_distance, remove_connected_zero_velocity); } std::vector createPathFootPrints(