feat(behavior_path_planner): cut overlapped path (autowarefoundation#…

…2451)

* feat(behavior_path_planner): cut overlapped path

Signed-off-by: yutaka <purewater0901@gmail.com>

* clean code

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -289,6 +289,10 @@ std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets &
 std::vector<DrivableLanes> generateDrivableLanesWithShoulderLanes(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes);
 
+size_t getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes);
+std::vector<DrivableLanes> cutOverlappedLanes(
+  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes);
+
 void occupancyGridToImage(const OccupancyGrid & occupancy_grid, cv::Mat * cv_image);
 
 void imageToOccupancyGrid(const cv::Mat & cv_image, OccupancyGrid * occupancy_grid);

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2007,14 +2007,16 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c
     drivable_lanes.push_back(current_drivable_lanes);
   }
 
-  drivable_lanes = util::expandLanelets(
-    drivable_lanes, parameters_->drivable_area_left_bound_offset,
+  const auto shorten_lanes = util::cutOverlappedLanes(shifted_path->path, drivable_lanes);
+
+  const auto extended_lanes = util::expandLanelets(
+    shorten_lanes, parameters_->drivable_area_left_bound_offset,
     parameters_->drivable_area_right_bound_offset, {"road_border"});
 
   {
     const auto & p = planner_data_->parameters;
     shifted_path->path.drivable_area = util::generateDrivableArea(
-      shifted_path->path, drivable_lanes, p.drivable_area_resolution, p.vehicle_length,
+      shifted_path->path, extended_lanes, p.drivable_area_resolution, p.vehicle_length,
       planner_data_);
   }
 }

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -274,8 +274,9 @@ PathWithLaneId LaneChangeModule::getReferencePath() const
     lane_change_buffer);
 
   const auto drivable_lanes = util::generateDrivableLanes(current_lanes);
+  const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes);
   const auto expanded_lanes = util::expandLanelets(
-    drivable_lanes, parameters_->drivable_area_left_bound_offset,
+    shorten_lanes, parameters_->drivable_area_left_bound_offset,
     parameters_->drivable_area_right_bound_offset);
 
   reference_path.drivable_area = util::generateDrivableArea(
@@ -620,8 +621,9 @@ void LaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path)
   const auto & route_handler = planner_data_->route_handler;
   const auto drivable_lanes = lane_change_utils::generateDrivableLanes(
     *route_handler, status_.current_lanes, status_.lane_change_lanes);
+  const auto shorten_lanes = util::cutOverlappedLanes(path, drivable_lanes);
   const auto expanded_lanes = util::expandLanelets(
-    drivable_lanes, parameters_->drivable_area_left_bound_offset,
+    shorten_lanes, parameters_->drivable_area_left_bound_offset,
     parameters_->drivable_area_right_bound_offset);
 
   const double & resolution = common_parameters.drivable_area_resolution;

planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp

@@ -137,8 +137,10 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
       lane_change_buffer);
   }
 
+  const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes);
+
   const auto expanded_lanes = util::expandLanelets(
-    drivable_lanes, parameters_.drivable_area_left_bound_offset,
+    shorten_lanes, parameters_.drivable_area_left_bound_offset,
     parameters_.drivable_area_right_bound_offset);
 
   reference_path.drivable_area = util::generateDrivableArea(

planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp

@@ -185,8 +185,9 @@ BehaviorModuleOutput PullOutModule::plan()
     path = status_.backward_path;
   }
 
+  const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes);
   const auto expanded_lanes = util::expandLanelets(
-    status_.lanes, parameters_.drivable_area_left_bound_offset,
+    shorten_lanes, parameters_.drivable_area_left_bound_offset,
     parameters_.drivable_area_right_bound_offset);
   path.drivable_area = util::generateDrivableArea(
     path, expanded_lanes, planner_data_->parameters.drivable_area_resolution,

planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp

@@ -92,8 +92,9 @@ boost::optional<PullOutPath> ShiftPullOut::plan(Pose start_pose, Pose goal_pose)
 
     // Generate drivable area
     const double resolution = common_parameters.drivable_area_resolution;
+    const auto shorten_lanes = util::cutOverlappedLanes(shift_path, drivable_lanes);
     shift_path.drivable_area = util::generateDrivableArea(
-      shift_path, drivable_lanes, resolution, common_parameters.vehicle_length, planner_data_);
+      shift_path, shorten_lanes, resolution, common_parameters.vehicle_length, planner_data_);
 
     shift_path.header = planner_data_->route_handler->getRouteHeader();
 

planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp

@@ -438,8 +438,9 @@ BehaviorModuleOutput PullOverModule::plan()
   for (size_t i = status_.current_path_idx; i < status_.pull_over_path.partial_paths.size(); ++i) {
     auto & path = status_.pull_over_path.partial_paths.at(i);
     const auto p = planner_data_->parameters;
+    const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes);
     const auto lane = util::expandLanelets(
-      status_.lanes, parameters_.drivable_area_left_bound_offset,
+      shorten_lanes, parameters_.drivable_area_left_bound_offset,
       parameters_.drivable_area_right_bound_offset);
     path.drivable_area = util::generateDrivableArea(
       path, lane, p.drivable_area_resolution, p.vehicle_length, planner_data_);
@@ -610,8 +611,9 @@ PathWithLaneId PullOverModule::getReferencePath() const
     -calcMinimumShiftPathDistance(), parameters_.deceleration_interval);
 
   const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes);
+  const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes);
   const auto lanes = util::expandLanelets(
-    drivable_lanes, parameters_.drivable_area_left_bound_offset,
+    shorten_lanes, parameters_.drivable_area_left_bound_offset,
     parameters_.drivable_area_right_bound_offset);
 
   reference_path.drivable_area = util::generateDrivableArea(
@@ -659,8 +661,9 @@ PathWithLaneId PullOverModule::generateStopPath() const
   }
 
   const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes);
+  const auto shorten_lanes = util::cutOverlappedLanes(stop_path, drivable_lanes);
   const auto lanes = util::expandLanelets(
-    drivable_lanes, parameters_.drivable_area_left_bound_offset,
+    shorten_lanes, parameters_.drivable_area_left_bound_offset,
     parameters_.drivable_area_right_bound_offset);
 
   stop_path.drivable_area = util::generateDrivableArea(

planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp

@@ -416,7 +416,8 @@ void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const
     -parameters_.drivable_area_right_bound_offset);
 
   const auto drivable_lanes = util::generateDrivableLanes(current_lanelets_);
-  const auto expanded_lanes = util::expandLanelets(drivable_lanes, left_offset, right_offset);
+  const auto shorten_lanes = util::cutOverlappedLanes(path->path, drivable_lanes);
+  const auto expanded_lanes = util::expandLanelets(shorten_lanes, left_offset, right_offset);
 
   {
     const auto & p = planner_data_->parameters;

planning/behavior_path_planner/src/utilities.cpp

@@ -21,6 +21,8 @@
 #include "autoware_auto_perception_msgs/msg/predicted_object.hpp"
 #include "autoware_auto_perception_msgs/msg/predicted_path.hpp"
 
+#include <boost/assign/list_of.hpp>
+
 #include <algorithm>
 #include <limits>
 #include <memory>
@@ -1258,6 +1260,81 @@ std::vector<DrivableLanes> generateDrivableLanesWithShoulderLanes(
   return drivable_lanes;
 }
 
+size_t getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes)
+{
+  auto overlaps = [](const DrivableLanes & lanes, const DrivableLanes & target_lanes) {
+    const auto lanelets = transformToLanelets(lanes);
+    const auto target_lanelets = transformToLanelets(target_lanes);
+
+    for (const auto & lanelet : lanelets) {
+      for (const auto & target_lanelet : target_lanelets) {
+        if (boost::geometry::overlaps(
+              lanelet.polygon2d().basicPolygon(), target_lanelet.polygon2d().basicPolygon())) {
+          return true;
+        }
+      }
+    }
+
+    // No overlapping
+    return false;
+  };
+
+  if (lanes.size() <= 2) {
+    return lanes.size();
+  }
+
+  size_t overlapped_idx = lanes.size();
+  for (size_t i = 0; i < lanes.size() - 2; ++i) {
+    for (size_t j = i + 2; j < lanes.size(); ++j) {
+      if (overlaps(lanes.at(i), lanes.at(j))) {
+        overlapped_idx = std::min(overlapped_idx, j);
+      }
+    }
+  }
+
+  return overlapped_idx;
+}
+
+std::vector<DrivableLanes> cutOverlappedLanes(
+  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes)
+{
+  const size_t overlapped_lanelet_id = getOverlappedLaneletId(lanes);
+  if (overlapped_lanelet_id == lanes.size()) {
+    return lanes;
+  }
+
+  const std::vector<DrivableLanes> shorten_lanes{
+    lanes.begin(), lanes.begin() + overlapped_lanelet_id};
+  const std::vector<DrivableLanes> removed_lanes{
+    lanes.begin() + overlapped_lanelet_id, lanes.end()};
+
+  const auto transformed_lanes = util::transformToLanelets(removed_lanes);
+
+  auto isIncluded = [&transformed_lanes](const std::vector<int64_t> & lane_ids) {
+    if (transformed_lanes.empty() || lane_ids.empty()) return false;
+
+    for (const auto & transformed_lane : transformed_lanes) {
+      for (const auto & lane_id : lane_ids) {
+        if (lane_id == transformed_lane.id()) {
+          return true;
+        }
+      }
+    }
+
+    return false;
+  };
+
+  for (size_t i = 0; i < path.points.size(); ++i) {
+    const auto & lane_ids = path.points.at(i).lane_ids;
+    if (isIncluded(lane_ids)) {
+      path.points.erase(path.points.begin() + i, path.points.end());
+      break;
+    }
+  }
+
+  return shorten_lanes;
+}
+
 // input lanes must be in sequence
 // NOTE: lanes in the path argument is used to calculate the size of the drivable area to cover
 // designated forward and backward length by getPathScope function.