feat(lane_departure_checker,start_planner): add check for path within lanes for bvspm (autowarefoundation#6366)

* WIP add new methods for lane departure checker

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add lanelet polygon check for lane departure

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* use new checkPathWillLeaveLane function

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* working solution, fix union bug

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* Add check fo backwards path

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* delete departure check lanes

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add lane departure check to geometric pullout

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* merge all union polygon

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* pre-commit changes

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* move the cheap/fast check first to possibly boost performance

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Co-authored-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp

@@ -18,6 +18,7 @@
 #include <boost/geometry/core/cs.hpp>
 #include <boost/geometry/geometries/geometries.hpp>
 #include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/geometries/register/ring.hpp>
 
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
@@ -98,5 +99,6 @@ BOOST_GEOMETRY_REGISTER_POINT_2D(                                       // NOLIN
   tier4_autoware_utils::Point2d, double, cs::cartesian, x(), y())       // NOLINT
 BOOST_GEOMETRY_REGISTER_POINT_3D(                                       // NOLINT
   tier4_autoware_utils::Point3d, double, cs::cartesian, x(), y(), z())  // NOLINT
+BOOST_GEOMETRY_REGISTER_RING(tier4_autoware_utils::LinearRing2d)        // NOLINT
 
 #endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -29,14 +29,20 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <boost/geometry/algorithms/envelope.hpp>
+#include <boost/geometry/algorithms/union.hpp>
 #include <boost/geometry/index/rtree.hpp>
-#include <boost/optional.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/geometry/LaneletMap.h>
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_core/geometry/Polygon.h>
 
 #include <map>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace lane_departure_checker
@@ -112,6 +118,19 @@ class LaneDepartureChecker
   bool checkPathWillLeaveLane(
     const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const;
 
+  std::vector<std::pair<double, lanelet::Lanelet>> getLaneletsFromPath(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
+
+  std::optional<lanelet::BasicPolygon2d> getFusedLaneletPolygonForPath(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
+
+  bool checkPathWillLeaveLane(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
+
+  PathWithLaneId cropPointsOutsideOfLanes(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path,
+    const size_t end_index);
+
   static bool isOutOfLane(
     const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint);
 

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -34,6 +34,7 @@ using motion_utils::calcArcLength;
 using tier4_autoware_utils::LinearRing2d;
 using tier4_autoware_utils::LineString2d;
 using tier4_autoware_utils::MultiPoint2d;
+using tier4_autoware_utils::MultiPolygon2d;
 using tier4_autoware_utils::Point2d;
 
 namespace
@@ -92,6 +93,7 @@ lanelet::ConstLanelets getCandidateLanelets(
 
   return candidate_lanelets;
 }
+
 }  // namespace
 
 namespace lane_departure_checker
@@ -298,6 +300,92 @@ bool LaneDepartureChecker::willLeaveLane(
   return false;
 }
 
+std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLaneletsFromPath(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
+{
+  // Get Footprint Hull basic polygon
+  std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
+  LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints);
+  auto to_basic_polygon = [](const LinearRing2d & footprint_hull) -> lanelet::BasicPolygon2d {
+    lanelet::BasicPolygon2d basic_polygon;
+    for (const auto & point : footprint_hull) {
+      Eigen::Vector2d p(point.x(), point.y());
+      basic_polygon.push_back(p);
+    }
+    return basic_polygon;
+  };
+  lanelet::BasicPolygon2d footprint_hull_basic_polygon = to_basic_polygon(footprint_hull);
+
+  // Find all lanelets that intersect the footprint hull
+  return lanelet::geometry::findWithin2d(
+    lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0);
+}
+
+std::optional<lanelet::BasicPolygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
+{
+  const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
+  // Fuse lanelets into a single BasicPolygon2d
+  auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional<lanelet::BasicPolygon2d> {
+    if (lanelets_distance_pair.empty()) return std::nullopt;
+    if (lanelets_distance_pair.size() == 1)
+      return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
+
+    lanelet::BasicPolygon2d merged_polygon =
+      lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
+    for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) {
+      const auto & route_lanelet = lanelets_distance_pair.at(i).second;
+      const auto & poly = route_lanelet.polygon2d().basicPolygon();
+
+      std::vector<lanelet::BasicPolygon2d> lanelet_union_temp;
+      boost::geometry::union_(poly, merged_polygon, lanelet_union_temp);
+
+      // Update merged_polygon by accumulating all merged results
+      merged_polygon.clear();
+      for (const auto & temp_poly : lanelet_union_temp) {
+        merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end());
+      }
+    }
+    if (merged_polygon.empty()) return std::nullopt;
+    return merged_polygon;
+  }();
+
+  return fused_lanelets;
+}
+
+bool LaneDepartureChecker::checkPathWillLeaveLane(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
+{
+  // check if the footprint is not fully contained within the fused lanelets polygon
+  const std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
+  const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
+  if (!fused_lanelets_polygon) return true;
+  return !std::all_of(
+    vehicle_footprints.begin(), vehicle_footprints.end(),
+    [&fused_lanelets_polygon](const auto & footprint) {
+      return boost::geometry::within(footprint, fused_lanelets_polygon.value());
+    });
+}
+
+PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index)
+{
+  PathWithLaneId temp_path;
+  const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
+  if (path.points.empty() || !fused_lanelets_polygon) return temp_path;
+  const auto vehicle_footprints = createVehicleFootprints(path);
+  size_t idx = 0;
+  std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) {
+    if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) {
+      temp_path.points.push_back(path.points.at(idx));
+    }
+    ++idx;
+  });
+  PathWithLaneId cropped_path = path;
+  cropped_path.points = temp_path.points;
+  return cropped_path;
+}
+
 bool LaneDepartureChecker::isOutOfLane(
   const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint)
 {
@@ -364,4 +452,5 @@ bool LaneDepartureChecker::willCrossBoundary(
   }
   return false;
 }
+
 }  // namespace lane_departure_checker

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp

@@ -18,6 +18,8 @@
 #include "behavior_path_planner_common/data_manager.hpp"
 #include "behavior_path_planner_common/parameters.hpp"
 
+#include <lane_departure_checker/lane_departure_checker.hpp>
+
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/point.hpp>
@@ -72,7 +74,8 @@ class GeometricParallelParking
     const bool left_side_parking);
   bool planPullOut(
     const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-    const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start);
+    const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start,
+    const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
   void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; }
   void setPlannerData(const std::shared_ptr<const PlannerData> & planner_data)
   {
@@ -115,7 +118,8 @@ class GeometricParallelParking
     const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
     const bool is_forward, const bool left_side_parking, const double end_pose_offset,
-    const double lane_departure_margin, const double arc_path_interval);
+    const double lane_departure_margin, const double arc_path_interval,
+    const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
   PathWithLaneId generateArcPath(
     const Pose & center, const double radius, const double start_yaw, double end_yaw,
     const double arc_path_interval, const bool is_left_turn, const bool is_forward);

planning/behavior_path_planner_common/package.xml

@@ -49,6 +49,7 @@
   <depend>freespace_planning_algorithms</depend>
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
+  <depend>lane_departure_checker</depend>
   <depend>lanelet2_extension</depend>
   <depend>magic_enum</depend>
   <depend>motion_utils</depend>

planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp

@@ -116,7 +116,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
                                               : parameters_.backward_parking_path_interval;
   auto arc_paths = planOneTrial(
     start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking,
-    end_pose_offset, lane_departure_margin, arc_path_interval);
+    end_pose_offset, lane_departure_margin, arc_path_interval, {});
   if (arc_paths.empty()) {
     return std::vector<PathWithLaneId>{};
   }
@@ -222,7 +222,8 @@ bool GeometricParallelParking::planPullOver(
 
 bool GeometricParallelParking::planPullOut(
   const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-  const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start)
+  const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start,
+  const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker)
 {
   constexpr bool is_forward = false;         // parking backward means pull_out forward
   constexpr double start_pose_offset = 0.0;  // start_pose is current_pose
@@ -242,7 +243,7 @@ bool GeometricParallelParking::planPullOut(
     auto arc_paths = planOneTrial(
       *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_start,
       start_pose_offset, parameters_.pull_out_lane_departure_margin,
-      parameters_.pull_out_path_interval);
+      parameters_.pull_out_path_interval, lane_departure_checker);
     if (arc_paths.empty()) {
       // not found path
       continue;
@@ -362,7 +363,8 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
   const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
   const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
   const bool is_forward, const bool left_side_parking, const double end_pose_offset,
-  const double lane_departure_margin, const double arc_path_interval)
+  const double lane_departure_margin, const double arc_path_interval,
+  const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker)
 {
   clearPaths();
 
@@ -496,6 +498,24 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
   setLaneIdsToPath(path_turn_first);
   setLaneIdsToPath(path_turn_second);
 
+  if (lane_departure_checker) {
+    const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();
+
+    const bool is_path_turn_first_outside_lanes =
+      lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, path_turn_first);
+
+    if (is_path_turn_first_outside_lanes) {
+      return std::vector<PathWithLaneId>{};
+    }
+
+    const bool is_path_turn_second_outside_lanes =
+      lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, path_turn_second);
+
+    if (is_path_turn_second_outside_lanes) {
+      return std::vector<PathWithLaneId>{};
+    }
+  }
+
   // generate arc path vector
   paths_.push_back(path_turn_first);
   paths_.push_back(path_turn_second);

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp

@@ -19,20 +19,27 @@
 #include "behavior_path_start_planner_module/pull_out_path.hpp"
 #include "behavior_path_start_planner_module/pull_out_planner_base.hpp"
 
+#include <lane_departure_checker/lane_departure_checker.hpp>
+
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
+#include <memory>
+
 namespace behavior_path_planner
 {
 class GeometricPullOut : public PullOutPlannerBase
 {
 public:
-  explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters);
+  explicit GeometricPullOut(
+    rclcpp::Node & node, const StartPlannerParameters & parameters,
+    const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
 
   PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; };
   std::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;
 
   GeometricParallelParking planner_;
   ParallelParkingParameters parallel_parking_parameters_;
+  std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker_;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -273,6 +273,7 @@ class StartPlannerModule : public SceneModuleInterface
     const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
   bool isSafePath() const;
   void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
+  lanelet::ConstLanelets createDepartureCheckLanes() const;
 
   // check if the goal is located behind the ego in the same route segment.
   bool isGoalBehindOfEgoInSameRouteSegment() const;

planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp

@@ -30,9 +30,12 @@ namespace behavior_path_planner
 {
 using start_planner_utils::getPullOutLanes;
 
-GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters)
+GeometricPullOut::GeometricPullOut(
+  rclcpp::Node & node, const StartPlannerParameters & parameters,
+  const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker)
 : PullOutPlannerBase{node, parameters},
-  parallel_parking_parameters_{parameters.parallel_parking_parameters}
+  parallel_parking_parameters_{parameters.parallel_parking_parameters},
+  lane_departure_checker_(lane_departure_checker)
 {
   planner_.setParameters(parallel_parking_parameters_);
 }
@@ -55,8 +58,8 @@ std::optional<PullOutPath> GeometricPullOut::plan(const Pose & start_pose, const
   planner_.setTurningRadius(
     planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle);
   planner_.setPlannerData(planner_data_);
-  const bool found_valid_path =
-    planner_.planPullOut(start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start);
+  const bool found_valid_path = planner_.planPullOut(
+    start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, lane_departure_checker_);
   if (!found_valid_path) {
     return {};
   }

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

@@ -65,9 +65,8 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
 
   // get safe path
   for (auto & pull_out_path : pull_out_paths) {
-    auto & shift_path =
-      pull_out_path.partial_paths.front();  // shift path is not separate but only one.
-
+    // shift path is not separate but only one.
+    auto & shift_path = pull_out_path.partial_paths.front();
     // check lane_departure with path between pull_out_start to pull_out_end
     PathWithLaneId path_shift_start_to_end{};
     {
@@ -80,51 +79,31 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
         shift_path.points.begin() + pull_out_end_idx + 1);
     }
 
-    // extract shoulder lanes from pull out lanes
-    lanelet::ConstLanelets shoulder_lanes;
-    std::copy_if(
-      pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes),
-      [&route_handler](const auto & pull_out_lane) {
-        return route_handler->isShoulderLanelet(pull_out_lane);
-      });
-    const auto drivable_lanes =
-      utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes);
-    const auto & dp = planner_data_->drivable_area_expansion_parameters;
-    const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets(
-      drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
-      dp.drivable_area_types_to_skip));
+
+    // check lane departure
+    // The method for lane departure checking verifies if the footprint of each point on the path is
+    // contained within a lanelet using `boost::geometry::within`, which incurs a high computational
+    // cost.
+    const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();
+    if (
+      parameters_.check_shift_path_lane_departure &&
+      lane_departure_checker_->checkPathWillLeaveLane(lanelet_map_ptr, path_shift_start_to_end)) {
+      continue;
+    }
 
     // crop backward path
     // removes points which are out of lanes up to the start pose.
     // this ensures that the backward_path stays within the drivable area when starting from a
     // narrow place.
+
     const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
       shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold,
       common_parameters.ego_nearest_yaw_threshold);
-    PathWithLaneId cropped_path{};
-    for (size_t i = 0; i < shift_path.points.size(); ++i) {
-      const Pose pose = shift_path.points.at(i).point.pose;
-      const auto transformed_vehicle_footprint =
-        transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose));
-      const bool is_out_of_lane =
-        LaneDepartureChecker::isOutOfLane(expanded_lanes, transformed_vehicle_footprint);
-      if (i <= start_segment_idx) {
-        if (!is_out_of_lane) {
-          cropped_path.points.push_back(shift_path.points.at(i));
-        }
-      } else {
-        cropped_path.points.push_back(shift_path.points.at(i));
-      }
-    }
-    shift_path.points = cropped_path.points;
 
-    // check lane departure
-    if (
-      parameters_.check_shift_path_lane_departure &&
-      lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_shift_start_to_end)) {
-      continue;
-    }
+    const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes(
+      lanelet_map_ptr, shift_path, start_segment_idx);
 
+    shift_path.points = cropped_path.points;
     shift_path.header = planner_data_->route_handler->getRouteHeader();
 
     return pull_out_path;

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -66,7 +66,8 @@ StartPlannerModule::StartPlannerModule(
       std::make_shared<ShiftPullOut>(node, *parameters, lane_departure_checker_));
   }
   if (parameters_->enable_geometric_pull_out) {
-    start_planners_.push_back(std::make_shared<GeometricPullOut>(node, *parameters));
+    start_planners_.push_back(
+      std::make_shared<GeometricPullOut>(node, *parameters, lane_departure_checker_));
   }
   if (start_planners_.empty()) {
     RCLCPP_ERROR(getLogger(), "Not found enabled planner");