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

@@ -88,8 +88,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/scene_module/lane_change/normal.cpp

@@ -121,7 +121,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<PathWithLaneId>(getReferencePath());

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

@@ -175,17 +175,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,
@@ -338,7 +327,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

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

@@ -97,8 +97,7 @@ boost::optional<PullOutPath> 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);

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(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

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

@@ -30,7 +30,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

@@ -37,19 +37,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)