diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index b7a9248d569c1..396798ee18519 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -44,8 +44,6 @@ using geometry_msgs::msg::Twist; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -// TODO(sugahara) move to util -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index a4c2c2e695752..adb180065f476 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -88,8 +88,6 @@ std::vector> getSortedLaneIds( const RouteHandler & route_handler, const Pose & current_pose, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); - lanelet::ConstLanelets getTargetPreferredLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction & direction, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index e907df8c7a58a..b42502d9edec8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -102,6 +102,8 @@ PathWithLaneId calcCenterLinePath( const std::shared_ptr & planner_data, const Pose & ref_pose, const double longest_dist_to_shift_line, const std::optional & prev_module_path = std::nullopt); + +PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_ 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 09fb9911f7386..f2dba684de991 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 @@ -44,7 +44,6 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; -PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2); PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const Pose & backed_pose, const double velocity); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index d1de12a3bd5f9..44eb74ed90f76 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -125,7 +125,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() const auto found_extended_path = extendPath(); if (found_extended_path) { - *output.path = utils::lane_change::combineReferencePath(*output.path, *found_extended_path); + *output.path = utils::combinePath(*output.path, *found_extended_path); } extendOutputDrivableArea(output); output.reference_path = std::make_shared(getReferencePath()); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 4f24d1b682f3d..038ca76a7fffd 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -44,16 +44,6 @@ namespace behavior_path_planner { namespace goal_planner_utils { -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) -{ - PathWithLaneId path; - path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); - - // skip overlapping point - path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - - return path; -} lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index e4fbf41e5af00..01d6cf4acae88 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -180,17 +180,6 @@ std::vector getAccelerationValues( return sampled_values; } -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) -{ - PathWithLaneId path; - path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); - - // skip overlapping point - path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - - return path; -} - lanelet::ConstLanelets getTargetPreferredLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction & direction, @@ -355,7 +344,7 @@ std::optional constructCandidatePath( return std::nullopt; } - candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); candidate_path.shifted_path = shifted_path; return std::optional{candidate_path}; diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index 7673c9a1de9b4..509b31ad3da2b 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -561,4 +561,25 @@ PathWithLaneId calcCenterLinePath( return centerline_path; } + +PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2) +{ + if (path1.points.empty()) { + return path2; + } + if (path2.points.empty()) { + return path1; + } + + PathWithLaneId path{}; + path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); + + // skip overlapping point + path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); + + PathWithLaneId filtered_path = path; + filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); + return filtered_path; +} + } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp index 8e5d6b7545c1d..945c386668dee 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -97,8 +97,7 @@ boost::optional FreespacePullOut::plan(const Pose & start_pose, con const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); last_path = utils::resamplePathWithSpline( - start_planner_utils::combineReferencePath(last_path, road_center_line_path), - parameters_.center_line_path_interval); + utils::combinePath(last_path, road_center_line_path), parameters_.center_line_path_interval); const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps; utils::correctDividedPathVelocity(partial_paths); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 5472885359c31..bc071e2a2a288 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #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_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -26,7 +27,6 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; namespace behavior_path_planner { -using start_planner_utils::combineReferencePath; using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters) @@ -106,7 +106,7 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con std::make_pair(velocity, velocity * velocity / (2 * arc_length_on_second_arc_path))); } else { const auto partial_paths = planner_.getPaths(); - const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); + const auto combined_path = utils::combinePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); // Calculate the acceleration required to reach the forward parking velocity at the center of 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 2de72d781994a..b82157cc17e41 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 @@ -32,7 +32,6 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; namespace behavior_path_planner { -using start_planner_utils::combineReferencePath; using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( 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 f5b4c9b979ee3..40b100a1bc070 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp @@ -38,19 +38,6 @@ namespace behavior_path_planner::start_planner_utils { -PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2) -{ - PathWithLaneId path; - path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); - - // skip overlapping point - path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - - PathWithLaneId filtered_path = path; - filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); - return filtered_path; -} - PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & shoulder_lanes, const Pose & current_pose, const Pose & backed_pose, const double velocity)