From 31e10cd105116e4f18da567dd5315f189716bc4e Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 16 Aug 2023 09:50:38 +0900 Subject: [PATCH] fix(start_planner): fix path when goal is out of current lanes (#4636) * fix(start_planner): fix path when goal is not preffered lanes Signed-off-by: kosuke55 * add calcEndArcLength Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/utils/start_planner/util.cpp Co-authored-by: Kyoichi Sugahara * Update planning/behavior_path_planner/src/utils/start_planner/util.cpp * Update planning/behavior_path_planner/src/utils/start_planner/util.cpp * Update planning/behavior_path_planner/src/utils/start_planner/util.cpp * fix build Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 Co-authored-by: Kyoichi Sugahara --- .../utils/start_planner/util.hpp | 8 +++++++ .../geometric_parallel_parking.cpp | 14 +++++------- .../utils/start_planner/shift_pull_out.cpp | 13 +++++------ .../src/utils/start_planner/util.cpp | 22 +++++++++++++++++++ 4 files changed, 41 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 6c7c6804ba6bb..63e374e074a5a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -28,6 +28,7 @@ #include #include +#include #include namespace behavior_path_planner::start_planner_utils @@ -47,6 +48,13 @@ lanelet::ConstLanelets getPullOutLanes( const std::shared_ptr & planner_data, const double backward_length); Pose getBackedPose( const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); +/** + * @brief calculate end arc length to generate reference path considering the goal position + * @return a pair of s_end and terminal_is_goal + */ +std::pair calcEndArcLength( + const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, + const Pose & goal_pose); } // namespace behavior_path_planner::start_planner_utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 4f8c7ff9a94b4..a5ccfc9517771 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -272,13 +273,10 @@ bool GeometricParallelParking::planPullOut( // get road center line path from pull_out end to goal, and combine after the second arc path const double s_start = getArcCoordinates(road_lanes, *end_pose).length; - const double s_goal = getArcCoordinates(road_lanes, goal_pose).length; - const double road_lanes_length = std::accumulate( - road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) { - return sum + lanelet::utils::getLaneletLength2d(lane); - }); - const bool goal_is_behind = s_goal < s_start; - const double s_end = goal_is_behind ? road_lanes_length : s_goal; + const auto path_end_info = start_planner_utils::calcEndArcLength( + s_start, planner_data_->parameters.forward_path_length, road_lanes, goal_pose); + const double s_end = path_end_info.first; + const bool path_terminal_is_goal = path_end_info.second; PathWithLaneId road_center_line_path = planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); @@ -305,7 +303,7 @@ bool GeometricParallelParking::planPullOut( paths.back().points = motion_utils::removeOverlapPoints(paths.back().points); // if the end point is the goal, set the velocity to 0 - if (!goal_is_behind) { + if (path_terminal_is_goal) { paths.back().points.back().point.longitudinal_velocity_mps = 0.0; } diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 54f19ff121b55..cbe06fa66c306 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -150,13 +150,10 @@ std::vector ShiftPullOut::calcPullOutPaths( // generate road lane reference path const auto arc_position_start = getArcCoordinates(road_lanes, start_pose); const double s_start = std::max(arc_position_start.length - backward_path_length, 0.0); - const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose); - - // if goal is behind start pose, use path with forward_path_length - const bool goal_is_behind = arc_position_goal.length < s_start; - const double s_forward_length = s_start + forward_path_length; - const double s_end = - goal_is_behind ? s_forward_length : std::min(arc_position_goal.length, s_forward_length); + const auto path_end_info = + start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + const double s_end = path_end_info.first; + const bool path_terminal_is_goal = path_end_info.second; constexpr double RESAMPLE_INTERVAL = 1.0; PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline( @@ -276,7 +273,7 @@ std::vector ShiftPullOut::calcPullOutPaths( } } // if the end point is the goal, set the velocity to 0 - if (!goal_is_behind) { + if (path_terminal_is_goal) { shifted_path.path.points.back().point.longitudinal_velocity_mps = 0.0; } diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_planner/src/utils/start_planner/util.cpp index 8ef659b7df7b0..9ee0491ca017e 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp @@ -118,4 +118,26 @@ lanelet::ConstLanelets getPullOutLanes( /*forward_length*/ std::numeric_limits::max(), /*forward_only_in_route*/ true); } + +std::pair calcEndArcLength( + const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, + const Pose & goal_pose) +{ + const double s_forward_length = s_start + forward_path_length; + // use forward length if the goal pose is not in the lanelets + if (!utils::isInLanelets(goal_pose, road_lanes)) { + return {s_forward_length, false}; + } + + const double s_goal = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; + + // If the goal is behind the start or beyond the forward length, use forward length. + if (s_goal < s_start || s_goal >= s_forward_length) { + return {s_forward_length, false}; + } + + // path end is goal + return {s_goal, true}; +} + } // namespace behavior_path_planner::start_planner_utils