feat(goal_planner): allow shift pull over in parking lot outside lane (…

…autowarefoundation#7140)

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp

@@ -16,6 +16,7 @@
 #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_
 
 #include "behavior_path_goal_planner_module/goal_searcher_base.hpp"
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
 
 #include <lane_departure_checker/lane_departure_checker.hpp>
 
@@ -41,6 +42,7 @@ using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
+using Polygon2d = tier4_autoware_utils::Polygon2d;
 
 lanelet::ConstLanelets getPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
@@ -85,6 +87,10 @@ PathWithLaneId extendPath(
   const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
   const Pose & extend_pose);
 
+std::vector<Polygon2d> createPathFootPrints(
+  const PathWithLaneId & path, const double base_to_front, const double base_to_rear,
+  const double width);
+
 // debug
 MarkerArray createPullOverAreaMarkerArray(
   const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header,

planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp

@@ -258,17 +258,39 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
     road_lane_reference_path_to_shift_end.points.back().point.pose);
   pull_over_path.debug_poses.push_back(prev_module_path_terminal_pose);
 
-  // check if the parking path will leave lanes
-  const auto drivable_lanes =
-    utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes);
-  const auto & dp = planner_data_->drivable_area_expansion_parameters;
-  const auto expanded_lanes = utils::expandLanelets(
-    drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
-    dp.drivable_area_types_to_skip);
-  const auto combined_drivable = utils::combineDrivableLanes(
-    expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes);
-  if (lane_departure_checker_.checkPathWillLeaveLane(
-        utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath())) {
+  // check if the parking path will leave drivable area and lanes
+  const bool is_in_parking_lots = std::invoke([&]() -> bool {
+    const auto & p = planner_data_->parameters;
+    const auto parking_lot_polygons =
+      lanelet::utils::query::getAllParkingLots(planner_data_->route_handler->getLaneletMapPtr());
+    const auto path_footprints = goal_planner_utils::createPathFootPrints(
+      pull_over_path.getParkingPath(), p.base_link2front, p.base_link2rear, p.vehicle_width);
+    const auto is_footprint_in_any_polygon = [&parking_lot_polygons](const auto & footprint) {
+      return std::any_of(
+        parking_lot_polygons.begin(), parking_lot_polygons.end(),
+        [&footprint](const auto & polygon) {
+          return lanelet::geometry::within(footprint, lanelet::utils::to2D(polygon).basicPolygon());
+        });
+    };
+    return std::all_of(
+      path_footprints.begin(), path_footprints.end(),
+      [&is_footprint_in_any_polygon](const auto & footprint) {
+        return is_footprint_in_any_polygon(footprint);
+      });
+  });
+  const bool is_in_lanes = std::invoke([&]() -> bool {
+    const auto drivable_lanes =
+      utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes);
+    const auto & dp = planner_data_->drivable_area_expansion_parameters;
+    const auto expanded_lanes = utils::expandLanelets(
+      drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
+      dp.drivable_area_types_to_skip);
+    const auto combined_drivable = utils::combineDrivableLanes(
+      expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes);
+    return !lane_departure_checker_.checkPathWillLeaveLane(
+      utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath());
+  });
+  if (!is_in_parking_lots && !is_in_lanes) {
     return {};
   }
 

planning/behavior_path_goal_planner_module/src/util.cpp

@@ -433,4 +433,17 @@ PathWithLaneId extendPath(
   return extendPath(target_path, reference_path, extend_distance);
 }
 
+std::vector<Polygon2d> createPathFootPrints(
+  const PathWithLaneId & path, const double base_to_front, const double base_to_rear,
+  const double width)
+{
+  std::vector<Polygon2d> footprints;
+  for (const auto & point : path.points) {
+    const auto & pose = point.point.pose;
+    footprints.push_back(
+      tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width));
+  }
+  return footprints;
+}
+
 }  // namespace behavior_path_planner::goal_planner_utils