feat(goal_planner): use neighboring lane of pull over lane to check g…

…oal footprint

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

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp

@@ -66,6 +66,14 @@ class GoalSearcher : public GoalSearcherBase
   BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
   BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
   bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const;
+  /** @brief Create a lanelet that represents the departure check area.
+   * @param [in] pull_over_lanes Lanelets that the vehicle will pull over to.
+   * @param [in] route_handler RouteHandler object.
+   * @return Lanelet that goal footprints should be inside.
+   */
+  lanelet::Lanelet createDepartureCehckLanelet(
+    const lanelet::ConstLanelets & pull_over_lanes,
+    const route_handler::RouteHandler & route_handler) const;
 
   LinearRing2d vehicle_footprint_{};
   bool left_side_parking_{true};

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

@@ -115,10 +115,7 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & p
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *route_handler, left_side_parking_, parameters_.backward_goal_search_length,
     parameters_.forward_goal_search_length);
-  auto lanes = utils::getExtendedCurrentLanes(
-    planner_data, backward_length, forward_length,
-    /*forward_only_in_route*/ false);
-  lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end());
+  const auto departure_check_lane = createDepartureCehckLanelet(pull_over_lanes, *route_handler);
 
   const auto goal_arc_coords =
     lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_);
@@ -175,7 +172,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & p
         break;
       }
 
-      if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) {
+      if (boost::geometry::intersects(
+            transformed_vehicle_footprint, departure_check_lane.polygon2d().basicPolygon())) {
         continue;
       }
 
@@ -527,4 +525,52 @@ GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes(
   return *closest_goal_candidate;
 }
 
+lanelet::Lanelet GoalSearcher::createDepartureCehckLanelet(
+  const lanelet::ConstLanelets & pull_over_lanes,
+  const route_handler::RouteHandler & route_handler) const
+{
+  const auto getBoundPoints =
+    [&](const lanelet::ConstLanelet & lane, const bool is_outer) -> lanelet::Points3d {
+    lanelet::Points3d points;
+    const auto & bound = left_side_parking_ ? (is_outer ? lane.leftBound() : lane.rightBound())
+                                            : (is_outer ? lane.rightBound() : lane.leftBound());
+    for (const auto & pt : bound) {
+      points.push_back(lanelet::Point3d(pt));
+    }
+    return points;
+  };
+
+  const auto getMostInnerLane =
+    [&](const lanelet::ConstLanelet & lane) -> std::optional<lanelet::ConstLanelet> {
+    auto neighboring_inner_lane = left_side_parking_ ? route_handler.getRightShoulderLanelet(lane)
+                                                     : route_handler.getLeftShoulderLanelet(lane);
+    if (!neighboring_inner_lane) {
+      neighboring_inner_lane = left_side_parking_ ? route_handler.getRightLanelet(lane)
+                                                  : route_handler.getLeftLanelet(lane);
+    }
+    return neighboring_inner_lane;
+  };
+
+  lanelet::Points3d outer_bound_points{};
+  lanelet::Points3d inner_bound_points{};
+  for (const auto & lane : pull_over_lanes) {
+    outer_bound_points = getBoundPoints(lane, true);
+    auto neighboring_inner_lane = getMostInnerLane(lane);
+    while (neighboring_inner_lane.has_value()) {
+      neighboring_inner_lane = getMostInnerLane(*neighboring_inner_lane);
+    }
+    if (neighboring_inner_lane.has_value()) {
+      inner_bound_points = getBoundPoints(*neighboring_inner_lane, false);
+    } else {
+      inner_bound_points = getBoundPoints(lane, false);
+    }
+  }
+
+  const auto outer_linestring = lanelet::LineString3d(lanelet::InvalId, outer_bound_points);
+  const auto inner_linestring = lanelet::LineString3d(lanelet::InvalId, inner_bound_points);
+  return lanelet::Lanelet(
+    lanelet::InvalId, left_side_parking_ ? outer_linestring : inner_linestring,
+    left_side_parking_ ? inner_linestring : outer_linestring);
+}
+
 }  // namespace autoware::behavior_path_planner