feat(intersection): ignore high curvature lane occlusion

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -52,6 +52,8 @@
         absence_traffic_light:
           creep_velocity: 1.388 # [m/s]
           maximum_peeking_distance: 6.0 # [m]
+        attention_lane_crop_curvature_threshold: 0.25
+        attention_lane_curvature_calculation_ds: 0.5
 
       enable_rtc:
         intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -164,6 +164,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       &debug_marker_array);
   }
 
+  if (debug_data_.occlusion_attention_area) {
+    appendMarkerArray(
+      createLaneletPolygonsMarkerArray(
+        debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917,
+        0.568, 0.596),
+      &debug_marker_array);
+  }
+
   if (debug_data_.adjacent_area) {
     appendMarkerArray(
       createLaneletPolygonsMarkerArray(

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -121,6 +121,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     node.declare_parameter<double>(ns + ".occlusion.absence_traffic_light.creep_velocity");
   ip.occlusion.absence_traffic_light.maximum_peeking_distance = node.declare_parameter<double>(
     ns + ".occlusion.absence_traffic_light.maximum_peeking_distance");
+  ip.occlusion.attention_lane_crop_curvature_threshold =
+    node.declare_parameter<double>(ns + ".occlusion.attention_lane_crop_curvature_threshold");
+  ip.occlusion.attention_lane_curvature_calculation_ds =
+    node.declare_parameter<double>(ns + ".occlusion.attention_lane_curvature_calculation_ds");
 }
 
 void IntersectionModuleManager::launchNewModules(

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -977,6 +977,7 @@
   const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention();
   const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area();
   debug_data_.attention_area = intersection_lanelets.attention_area();
+  debug_data_.occlusion_attention_area = occlusion_attention_area;
   debug_data_.adjacent_area = intersection_lanelets.adjacent_area();
 
   // get intersection area
@@ -1013,7 +1014,9 @@
   if (!occlusion_attention_divisions_) {
     occlusion_attention_divisions_ = util::generateDetectionLaneDivisions(
       occlusion_attention_lanelets, routing_graph_ptr,
-      planner_data_->occupancy_grid->info.resolution);
+      planner_data_->occupancy_grid->info.resolution,
+      planner_param_.occlusion.attention_lane_crop_curvature_threshold,
+      planner_param_.occlusion.attention_lane_curvature_calculation_ds);
   }
   const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value();
 
@@ -1303,7 +1306,7 @@
   const lanelet::ConstLanelets & adjacent_lanelets,
   const lanelet::CompoundPolygon3d & first_attention_area,
   const util::InterpolatedPathInfo & interpolated_path_info,
   const std::vector<util::DescritizedLane> & lane_divisions,
   const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> &
     blocking_attention_objects,
   const double occlusion_dist_thr)

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -113,6 +113,8 @@
         double creep_velocity;
         double maximum_peeking_distance;
       } absence_traffic_light;
+      double attention_lane_crop_curvature_threshold;
+      double attention_lane_curvature_calculation_ds;
     } occlusion;
   };
 
@@ -236,7 +238,7 @@
 
   // for occlusion detection
   const bool enable_occlusion_detection_;
   std::optional<std::vector<util::DescritizedLane>> occlusion_attention_divisions_;
   // OcclusionState prev_occlusion_state_ = OcclusionState::NONE;
   StateMachine collision_state_machine_;     //! for stable collision checking
   StateMachine before_creep_state_machine_;  //! for two phase stop
@@ -284,7 +286,7 @@
     const lanelet::ConstLanelets & adjacent_lanelets,
     const lanelet::CompoundPolygon3d & first_attention_area,
     const util::InterpolatedPathInfo & interpolated_path_info,
     const std::vector<util::DescritizedLane> & lane_divisions,
     const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> &
       parked_attention_objects,
     const double occlusion_dist_thr);

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -20,6 +20,7 @@
 #include <behavior_velocity_planner_common/utilization/path_utilization.hpp>
 #include <behavior_velocity_planner_common/utilization/trajectory_utils.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
+#include <interpolation/spline_interpolation_points_2d.hpp>
 #include <lanelet2_extension/regulatory_elements/road_marking.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
@@ -40,6 +41,21 @@
 #include <unordered_map>
 #include <vector>
 
+namespace tier4_autoware_utils
+{
+
+template <>
+inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p)
+{
+  geometry_msgs::msg::Point point;
+  point.x = p.x();
+  point.y = p.y();
+  point.z = p.z();
+  return point;
+}
+
+}  // namespace tier4_autoware_utils
+
 namespace behavior_velocity_planner
 {
 namespace bg = boost::geometry;
@@ -804,10 +820,26 @@
   return false;
 }
 
+double getHighestCurvature(const lanelet::ConstLineString3d & centerline)
+{
+  std::vector<lanelet::ConstPoint3d> points;
+  for (auto point = centerline.begin(); point != centerline.end(); point++) {
+    points.push_back(*point);
+  }
+
+  SplineInterpolationPoints2d interpolation(points);
+  const std::vector<double> curvatures = interpolation.getSplineInterpolatedCurvatures();
+  std::vector<double> curvatures_positive;
+  for (const auto & curvature : curvatures) {
+    curvatures_positive.push_back(std::fabs(curvature));
+  }
+  return *std::max_element(curvatures_positive.begin(), curvatures_positive.end());
+}
+
 std::vector<DescritizedLane> generateDetectionLaneDivisions(
   lanelet::ConstLanelets detection_lanelets_all,
-  [[maybe_unused]] const lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-  const double resolution)
+  const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution,
+  const double curvature_threshold, const double curvature_calculation_ds)
 {
   using lanelet::utils::getCenterlineWithOffset;
   using lanelet::utils::to2D;
@@ -819,6 +851,12 @@
     if (turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0) {
       continue;
     }
+    const auto fine_centerline =
+      lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds);
+    const double highest_curvature = getHighestCurvature(fine_centerline);
+    if (highest_curvature > curvature_threshold) {
+      continue;
+    }
     detection_lanelets.push_back(detection_lanelet);
   }
 
@@ -873,10 +911,10 @@
     auto & branch = branches[(ind2id[src])];
     int node_iter = ind2id[src];
     while (true) {
       const auto & dsts = adjacency[(id2ind[node_iter])];
       // NOTE: assuming detection lanelets have only one previous lanelet
       const auto next = std::find(dsts.begin(), dsts.end(), true);
       if (next == dsts.end()) {
         branch.push_back(node_iter);
         break;
       }

planning/behavior_velocity_intersection_module/src/util.hpp

@@ -108,7 +108,8 @@ bool isTrafficLightArrowActivated(
 
 std::vector<DescritizedLane> generateDetectionLaneDivisions(
   lanelet::ConstLanelets detection_lanelets,
-  const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution);
+  const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution,
+  const double curvature_threshold, const double curvature_calculation_ds);
 
 std::optional<InterpolatedPathInfo> generateInterpolatedPath(
   const int lane_id, const std::set<int> & associative_lane_ids,

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -39,6 +39,7 @@
   std::optional<geometry_msgs::msg::Pose> occlusion_first_stop_wall_pose{std::nullopt};
   std::optional<geometry_msgs::msg::Pose> pass_judge_wall_pose{std::nullopt};
   std::optional<std::vector<lanelet::CompoundPolygon3d>> attention_area{std::nullopt};
+  std::optional<std::vector<lanelet::CompoundPolygon3d>> occlusion_attention_area{std::nullopt};
   std::optional<geometry_msgs::msg::Polygon> intersection_area{std::nullopt};
   std::optional<lanelet::CompoundPolygon3d> ego_lane{std::nullopt};
   std::optional<std::vector<lanelet::CompoundPolygon3d>> adjacent_area{std::nullopt};
@@ -117,7 +118,7 @@
   std::optional<lanelet::CompoundPolygon3d> first_attention_area_ = std::nullopt;
 };
 
 struct DescritizedLane
 {
   int lane_id{0};
   // discrete fine lines from left to right
@@ -143,7 +144,7 @@
 struct PathLanelets
 {
   lanelet::ConstLanelets prev;
   // lanelet::Constlanelet entry2ego; this is included in `all` if exists
   lanelet::ConstLanelet
     ego_or_entry2exit;  // this is `assigned lane` part of the path(not from
                         // ego) if ego is before the intersection, otherwise from ego to exit