fix(behavior_path_planner): add empty guard to combinePath (autowaref…

…oundation#5147)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

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);

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp

@@ -80,8 +80,6 @@ std::vector<std::vector<int64_t>> getSortedLaneIds(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes, const double rough_shift_length);
 
-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,

planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp

@@ -102,6 +102,8 @@ PathWithLaneId calcCenterLinePath(
   const std::shared_ptr<const PlannerData> & planner_data, const Pose & ref_pose,
   const double longest_dist_to_shift_line,
   const std::optional<PathWithLaneId> & 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_

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);

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,

planning/behavior_path_planner/src/utils/lane_change/utils.cpp

@@ -142,17 +142,6 @@ std::vector<double> 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,
@@ -289,7 +278,7 @@ std::optional<LaneChangePath> 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<LaneChangePath>{candidate_path};

planning/behavior_path_planner/src/utils/path_utils.cpp

@@ -538,4 +538,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

planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp

@@ -96,7 +96,8 @@ boost::optional<PullOutPath> FreespacePullOut::plan(const Pose start_pose, const
   const bool path_terminal_is_goal = path_end_info.second;
 
   const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end);
-  last_path = start_planner_utils::combineReferencePath(last_path, road_center_line_path);
+  last_path =
+    utils::resamplePathWithSpline(utils::combinePath(last_path, road_center_line_path), 1.0);
 
   const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps;
   utils::correctDividedPathVelocity(partial_paths);

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)
@@ -103,7 +103,7 @@ boost::optional<PullOutPath> GeometricPullOut::plan(Pose start_pose, Pose goal_p
       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

planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp

@@ -31,7 +31,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(

planning/behavior_path_planner/src/utils/start_planner/util.cpp

@@ -39,20 +39,6 @@
 
 namespace behavior_path_planner::start_planner_utils
 {
-using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams;
-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)