some drivable area appears

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

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

@@ -231,6 +231,11 @@ std::vector<geometry_msgs::msg::Point> calcBound(
   const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
   const bool is_left);
 
+lanelet::ConstPoints3d getBoundWithParkingSpace(
+  const std::vector<lanelet::ConstPoint3d> & original_bound,
+  const std::shared_ptr<RouteHandler> & route_handler,
+  const std::vector<DrivableLanes> & drivable_lanes, const bool is_left);
+
 std::vector<lanelet::ConstPoint3d> getBoundWithHatchedRoadMarkings(
   const std::vector<lanelet::ConstPoint3d> & original_bound,
   const std::shared_ptr<RouteHandler> & route_handler);

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -921,7 +921,7 @@ void compensateDetectionLost(
 }
 
 void filterTargetObjects(
-  ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug,
+  ObjectDataArray & objects, AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug,
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {

planning/behavior_path_planner/src/utils/utils.cpp

@@ -1838,51 +1838,144 @@ std::vector<geometry_msgs::msg::Point> calcBound(
       getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left);
   }
 
+  {
+    bound_points = getBoundWithParkingSpace(bound_points, route_handler, drivable_lanes, is_left);
+  }
+
   return motion_utils::removeOverlapPoints(to_ros_point(bound_points));
 }
 
-std::vector<lanelet::ConstPoint3d> getBoundWithParkingSpace(
+lanelet::ConstPoints3d getBoundWithParkingSpace(
   const std::vector<lanelet::ConstPoint3d> & original_bound,
-  [[maybe_unused]]const std::shared_ptr<RouteHandler> & route_handler,
+  const std::shared_ptr<RouteHandler> & route_handler,
   const std::vector<DrivableLanes> & drivable_lanes, const bool is_left)
 {
+  if(!is_left){
+    return original_bound;
+  }
+
+  const auto found_intersection =
+    [](
+       const lanelet::ConstPoints3d & drivable_lane_bound,
+       const lanelet::ConstPolygon3d & parking_lot) -> lanelet::ConstPoints3d {
+    if (drivable_lane_bound.size() < 2 || parking_lot.size() < 2) {
+      return {};
+    }
+
+    const auto lane_front_pt = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.front());
+    const auto lane_back_pt = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.back());
+    const auto bound_front_pt = lanelet::utils::conversion::toGeomMsgPt(parking_lot.front());
+    const auto bound_back_pt = lanelet::utils::conversion::toGeomMsgPt(parking_lot.back());
+
+    const auto rough_intersection =
+      tier4_autoware_utils::intersect(lane_front_pt, lane_back_pt, bound_front_pt, bound_back_pt);
+
+    // just check once if there exist intersection. reduce the need to iterate too much.
+    if (!rough_intersection) {
+      return {};
+    }
+
+    const auto front_point_within_parking_lot = boost::geometry::within(drivable_lane_bound.front(), parking_lot);
+
+    lanelet::ConstPoints3d intersect_insertion_bound;
+    for (size_t i = 0; i < drivable_lane_bound.size(); ++i) {
+      size_t next_i = (i + 1) % drivable_lane_bound.size();
+      const auto lane_pt1 = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.at(i));
+      const auto lane_pt2 = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.at(next_i));
+      for (size_t j = 0; j < parking_lot.size(); ++j) {
+        size_t next_j = (j + 1) % parking_lot.size();
+        const auto bound_pt1 = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.at(j));
+        const auto bound_pt2 = lanelet::utils::conversion::toGeomMsgPt(drivable_lane_bound.at(next_j));
+        const auto intersection_point =
+          tier4_autoware_utils::intersect(lane_pt1, lane_pt2, bound_pt1, bound_pt2);
+
+        if(!intersection_point){
+          continue;
+        }
+
+        const auto intersection_point_lanelet_point = lanelet::utils::conversion::toLaneletPoint(*intersection_point);
+        if (intersection_point && !front_point_within_parking_lot) {
+          intersect_insertion_bound.insert(intersect_insertion_bound.end(), drivable_lane_bound.begin(), drivable_lane_bound.begin() + static_cast<int>(i));
+          // Insert elements starting from next_i to the end of parking_lot
+          intersect_insertion_bound.push_back(intersection_point_lanelet_point);
+          intersect_insertion_bound.insert(intersect_insertion_bound.end(), parking_lot.begin() + static_cast<int>(next_j), parking_lot.end());
+
+          // If the range wrapped around, insert the remaining elements from the beginning of parking_lot
+          if (next_j > 0) {
+            intersect_insertion_bound.insert(intersect_insertion_bound.end(), parking_lot.begin(), parking_lot.begin() + static_cast<int>(next_j));
+          }
+          return intersect_insertion_bound;
+        }
+        if (intersection_point && front_point_within_parking_lot){
+          intersect_insertion_bound.push_back(intersection_point_lanelet_point);
+          intersect_insertion_bound.insert(intersect_insertion_bound.end(), drivable_lane_bound.begin() + next_i, drivable_lane_bound.end());
+          return intersect_insertion_bound;
+        }
+      }
+    }
+    return {};
+  };
+
+  lanelet::ConstPoints3d new_bound_point;
   const auto lanelet_map_ptr = route_handler->getLaneletMapPtr();
   const auto all_parking_lot = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr);
-  std::vector<lanelet::Id> parking_lot_ids;
+  lanelet::Ids parking_lots_ids{};
+  for (const auto & drivable_lane : drivable_lanes) {
+    const auto & check_lane = is_left ? drivable_lane.left_lane : drivable_lane.right_lane;
+    lanelet::ConstPolygon3d linked_parking_lot;
+    const auto found_parking_lot =
+      lanelet::utils::query::getLinkedParkingLot(check_lane, all_parking_lot, &linked_parking_lot);
+
+    // if not parking lot, no drivable area extension needed, so just copy the sub-vector of original bounds
+    if (!found_parking_lot) {
+      const auto not_shared_bound_start_itr = std::find_if(original_bound.begin(), original_bound.end(), [&](const auto & p){
+        return p.id() == check_lane.leftBound().front().id();
+      });
+      const auto not_shared_bound_end_itr = std::find_if(
+        original_bound.begin(), original_bound.end(),
+        [&](const auto & p) { return p.id() == check_lane.leftBound3d().back().id(); });
+      new_bound_point.insert(
+        new_bound_point.end(), not_shared_bound_start_itr, not_shared_bound_end_itr);
+      continue;
+    }
 
-  lanelet::ConstPolygons3d linked_parking_lots;
+    const auto bound_exist = std::any_of(
+      parking_lots_ids.begin(), parking_lots_ids.end(),
+      [&](const lanelet::Id id) { return id == linked_parking_lot.id(); });
 
-  for (const auto & lanes : drivable_lanes) {
-    if (is_left) {
-      const auto & left_lane = lanes.left_lane;
+    // check if intersect point exist
+    const auto lane_bound = check_lane.leftBound();
 
-      lanelet::ConstPolygon3d linked_parking_lot;
-      const auto found_parking_lot =
-        lanelet::utils::query::getLinkedParkingLot(left_lane, all_parking_lot, &linked_parking_lot);
+    const auto shared_bound_start_itr = std::find_if(
+      original_bound.begin(), original_bound.end(),
+      [&](const auto & p) { return p.id() == check_lane.leftBound().front().id(); });
+    const auto shared_bound_end_itr = std::find_if(
+      original_bound.begin(), original_bound.end(),
+      [&](const auto & p) { return p.id() == check_lane.leftBound3d().back().id(); });
 
-      if (!found_parking_lot) {
-        continue;
-      }
+    lanelet::ConstPoints3d tmp_bound {};
+    tmp_bound.insert(tmp_bound.end(), shared_bound_start_itr, shared_bound_end_itr);
 
-      if (!parking_lot_ids.empty()) {
-      const auto not_registered = std::none_of(
-        parking_lot_ids.begin(), parking_lot_ids.end(),
-        [&](const auto id) { return id == linked_parking_lot.id(); });
+    const auto intersection_exist = found_intersection(tmp_bound, linked_parking_lot);
+    if(bound_exist && intersection_exist.empty()){
+      continue;
+    }
 
-      if (not_registered) {
-        linked_parking_lots.push_back(linked_parking_lot);
-        parking_lot_ids.push_back(linked_parking_lot.id());
-      }
-      } else {
-        parking_lot_ids.push_back(linked_parking_lot.id());
-        linked_parking_lots.push_back(linked_parking_lot);
-      }
+    if (!bound_exist && intersection_exist.empty()){
+      new_bound_point.insert(new_bound_point.end(), linked_parking_lot.begin(), linked_parking_lot.end());
     }
-  }
 
-  std::vector<lanelet::ConstPoint3d> new_bound_point;
+    if(!bound_exist && !intersection_exist.empty()){
+      new_bound_point.insert(new_bound_point.end(), intersection_exist.begin(), intersection_exist.end());
+      parking_lots_ids.push_back(linked_parking_lot.id());
+    }
 
+    if (bound_exist && !intersection_exist.empty()) {
+      new_bound_point.insert(new_bound_point.end(), intersection_exist.begin(), intersection_exist.end());
+    }
+  }
 
+  return new_bound_point;
 }
 
 void makeBoundLongitudinallyMonotonic(