Merge branch 'main' into feature/parallel_ndt_build

Author: anhnv3991

.github/workflows/build-and-test-differential.yaml

@@ -24,7 +24,6 @@ jobs:
         rosdistro:
           - humble
         container-suffix:
-          - ""
           - -cuda
         include:
           - rosdistro: humble

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/.pages

@@ -77,8 +77,8 @@ nav:
   - 'Additional Tools':
     - 'RTC Replayer': planning/rtc_replayer
     - 'Planning Debug Tools':
-      - ['About Planning Debug Tools'](https://github.com/autowarefoundation/autoware_tools/tree/main/planning_debug_tools)
-      - ['Stop Reason Visualizer'](https://github.com/autowarefoundation/autoware_tools/tree/main/planning_debug_tools/doc-stop-reason-visualizer)
+      - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools
+      - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md
     - 'Planning Test Utils': planning/planning_test_utils
     - 'Planning Topic Converter': planning/planning_topic_converter
     - 'Planning Validator': planning/planning_validator

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -208,6 +208,11 @@
           max_right_shift_length: 5.0
           max_left_shift_length: 5.0
           max_deviation_from_lane: 0.5                  # [m]
+          # approve the next shift line after reaching this percentage of the current shift line length.
+          # this parameter should be within range of 0.0 to 1.0.
+          # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear.
+          # this feature can be disabled by setting this parameter to 1.0. (in that case, avoidance module creates return shift as soon as possible.)
+          ratio_for_return_shift_approval: 0.5          # [-]
         # avoidance distance parameters
         longitudinal:
           min_prepare_time: 1.0                         # [s]

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -276,6 +276,9 @@ struct AvoidanceParameters
   // use for judge if the ego is shifting or not.
   double lateral_avoid_check_threshold{0.0};
 
+  // use for return shift approval.
+  double ratio_for_return_shift_approval{0.0};
+
   // For shift line generation process. The continuous shift length is quantized by this value.
   double quantize_filter_threshold{0.0};
 
@@ -539,6 +542,8 @@ struct AvoidancePlanningData
 
   bool valid{false};
 
+  bool ready{false};
+
   bool success{false};
 
   bool comfortable{false};

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -270,6 +270,33 @@ class AvoidanceHelper
     });
   }
 
+  bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const
+  {
+    if (std::abs(current_shift_length) < 1e-3) {
+      return true;
+    }
+
+    if (new_shift_lines.empty()) {
+      return true;
+    }
+
+    const auto front_shift_relative_length = new_shift_lines.front().getRelativeLength();
+
+    // same direction shift.
+    if (current_shift_length > 0.0 && front_shift_relative_length > 0.0) {
+      return true;
+    }
+
+    // same direction shift.
+    if (current_shift_length < 0.0 && front_shift_relative_length < 0.0) {
+      return true;
+    }
+
+    // keep waiting the other side shift approval until the ego reaches shift length threshold.
+    const auto ego_shift_ratio = (current_shift_length - getEgoShift()) / current_shift_length;
+    return ego_shift_ratio < parameters_->ratio_for_return_shift_approval + 1e-3;
+  }
+
   bool isShifted() const
   {
     return std::abs(getEgoShift()) > parameters_->lateral_avoid_check_threshold;

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -258,6 +258,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
     p.max_left_shift_length = getOrDeclareParameter<double>(*node, ns + "max_left_shift_length");
     p.max_deviation_from_lane =
       getOrDeclareParameter<double>(*node, ns + "max_deviation_from_lane");
+    p.ratio_for_return_shift_approval =
+      getOrDeclareParameter<double>(*node, ns + "ratio_for_return_shift_approval");
+    if (p.ratio_for_return_shift_approval < 0.0 || p.ratio_for_return_shift_approval > 1.0) {
+      throw std::domain_error(
+        "ratio_for_return_shift_approval should be within range of 0.0 to 1.0");
+    }
   }
 
   // avoidance maneuver (longitudinal)

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -133,7 +133,7 @@ bool AvoidanceModule::isExecutionRequested() const
 bool AvoidanceModule::isExecutionReady() const
 {
   DEBUG_PRINT("AVOIDANCE isExecutionReady");
-  return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid;
+  return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready;
 }
 
 bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const
@@ -513,6 +513,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
    */
   data.comfortable = helper_->isComfortable(data.new_shift_line);
   data.safe = isSafePath(data.candidate_path, debug);
+  data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength());
 }
 
 void AvoidanceModule::fillEgoStatus(

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);