From f02176db833ba32b0bffb3be9889e29f278a5836 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 12 Sep 2023 17:38:16 +0900 Subject: [PATCH] fix(start/goal_planner): fix inverse order path points (#4950) fix(start_planner): fix inverse order path points Signed-off-by: kosuke55 --- .../utils/start_goal_planner_common/utils.hpp | 15 +++++++++++++++ .../src/utils/goal_planner/shift_pull_over.cpp | 5 +++++ .../src/utils/start_goal_planner_common/utils.cpp | 15 +++++++++++++++ .../src/utils/start_planner/shift_pull_out.cpp | 4 ++++ 4 files changed, 39 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp index 6100156407bce..96945e1aae918 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -56,6 +56,21 @@ void updatePathProperty( std::pair getPairsTerminalVelocityAndAccel( const PullOutPath & pull_out_path, const size_t current_path_idx); +/** + * @brief removeInverseOrderPathPoints function + * + * This function is designed to handle a situation that can arise when shifting paths on a curve, + * where the positions of the path points may become inverted (i.e., a point further along the path + * comes to be located before an earlier point). It corrects for this by using the function + * tier4_autoware_utils::isDrivingForward(p1, p2) to check if each pair of adjacent points is in + * the correct order (with the second point being 'forward' of the first). Any points which fail + * this test are omitted from the returned PathWithLaneId. + * + * @param path A path with points possibly in incorrect order. + * @return Returns a new PathWithLaneId that has all points in the correct order. + */ +PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path); + } // namespace behavior_path_planner::utils::start_goal_planner_common #endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 46e683dd34e58..8eb03ad08634c 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -16,6 +16,8 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include @@ -170,6 +172,9 @@ boost::optional ShiftPullOver::generatePullOverPath( shifted_path.path.points.push_back(p); } + shifted_path.path = + utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path); + // set the same z as the goal for (auto & p : shifted_path.path.points) { p.point.pose.position.z = goal_pose.position.z; diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp index 3e5f3b16bacb9..9f67a220abcbf 100644 --- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -84,4 +84,19 @@ std::pair getPairsTerminalVelocityAndAccel( return pull_out_path.pairs_terminal_velocity_and_accel.at(current_path_idx); } +PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path) +{ + PathWithLaneId fixed_path; + fixed_path.header = path.header; + fixed_path.points.push_back(path.points.at(0)); + for (size_t i = 1; i < path.points.size(); i++) { + const auto p1 = path.points.at(i - 1).point.pose; + const auto p2 = path.points.at(i).point.pose; + if (tier4_autoware_utils::isDrivingForward(p1, p2)) { + fixed_path.points.push_back(path.points.at(i)); + } + } + return fixed_path; +} + } // namespace behavior_path_planner::utils::start_goal_planner_common 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 7ed6fdef1e809..5a0ff2d316542 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 @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -297,6 +298,9 @@ std::vector ShiftPullOut::calcPullOutPaths( continue; } + shifted_path.path = + utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path); + // set velocity const size_t pull_out_end_idx = findNearestIndex(shifted_path.path.points, shift_end_pose_ptr->position);