diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index c65a7eae189e3..db06564d299f7 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -1392,7 +1392,8 @@ void insertOrientation(T & points, const bool is_driving_forward) /** * @brief Remove points with invalid orientation differences from a given points container - * (trajectory, path, ...) + * (trajectory, path, ...). Check the difference between the angles of two points and the difference + * between the azimuth angle between the two points and the angle of the next point. * @param points Points of trajectory, path, or other point container (input / output) * @param max_yaw_diff Maximum acceptable yaw angle difference between two consecutive points in * radians (default: M_PI_2) @@ -1401,10 +1402,14 @@ template void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) { for (size_t i = 1; i < points.size();) { - const double yaw1 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i - 1)).orientation); - const double yaw2 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i)).orientation); - - if (max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2))) { + const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1)); + const auto p2 = tier4_autoware_utils::getPose(points.at(i)); + const double yaw1 = tf2::getYaw(p1.orientation); + const double yaw2 = tf2::getYaw(p2.orientation); + + if ( + max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) || + !tier4_autoware_utils::isDrivingForward(p1, p2)) { points.erase(points.begin() + i); } else { ++i; 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 29ea6b5637a1d..4cb213c0b87fe 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 @@ -89,20 +89,10 @@ std::pair getPairsTerminalVelocityAndAccel( const std::vector> & pairs_terminal_velocity_and_accel, 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); +std::optional generateFeasibleStopPath( + PathWithLaneId & current_path, std::shared_ptr planner_data, + geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, + const double maximum_jerk); } // namespace behavior_path_planner::utils::start_goal_planner_common 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 d6a3746187b11..21295f8c28585 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 @@ -172,9 +172,6 @@ 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/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index eb752220d3690..4fc7b168d77fb 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -56,6 +56,7 @@ namespace behavior_path_planner using motion_utils::findNearestIndex; using motion_utils::insertOrientation; using motion_utils::removeInvalidOrientationPoints; +using motion_utils::removeOverlapPoints; void PathShifter::setPath(const PathWithLaneId & path) { @@ -147,9 +148,18 @@ bool PathShifter::generate( type == SHIFT_TYPE::SPLINE ? applySplineShifter(shifted_path, offset_back) : applyLinearShifter(shifted_path); - const bool is_driving_forward = true; - insertOrientation(shifted_path->path.points, is_driving_forward); + shifted_path->path.points = removeOverlapPoints(shifted_path->path.points); + // Use orientation before shift to remove points in reverse order + // before setting wrong azimuth orientation removeInvalidOrientationPoints(shifted_path->path.points); + size_t previous_size{shifted_path->path.points.size()}; + do { + previous_size = shifted_path->path.points.size(); + // Set the azimuth orientation to the next point at each point + insertOrientation(shifted_path->path.points, true); + // Use azimuth orientation to remove points in reverse order + removeInvalidOrientationPoints(shifted_path->path.points); + } while (previous_size != shifted_path->path.points.size()); // DEBUG RCLCPP_DEBUG_STREAM_THROTTLE( 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 10163011a365a..8c2aa908aadcf 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 @@ -152,19 +152,36 @@ std::pair getPairsTerminalVelocityAndAccel( return pairs_terminal_velocity_and_accel.at(current_path_idx); } -PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path) +std::optional generateFeasibleStopPath( + PathWithLaneId & current_path, std::shared_ptr planner_data, + geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, + const double maximum_jerk) { - 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)); - } + if (current_path.points.empty()) { + return {}; + } + + // try to insert stop point in current_path after approval + // but if can't stop with constraints(maximum deceleration, maximum jerk), don't insert stop point + const auto min_stop_distance = + behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance( + planner_data, maximum_deceleration, maximum_jerk, 0.0); + + if (!min_stop_distance) { + return {}; } - return fixed_path; + + // set stop point + const auto stop_idx = motion_utils::insertStopPoint( + planner_data->self_odometry->pose.pose, *min_stop_distance, current_path.points); + + if (!stop_idx) { + return {}; + } + + stop_pose = current_path.points.at(*stop_idx).point.pose; + + return current_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 5a0ff2d316542..5e507b3156e29 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 @@ -298,9 +298,6 @@ 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);