feat(intersection): ignore high curvature lane occlusion (autowarefou…

…ndation#5223)

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -57,6 +57,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

@@ -173,6 +173,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

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

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -989,6 +989,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   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
@@ -1027,7 +1028,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   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();
 

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -119,6 +119,8 @@ class IntersectionModule : public SceneModuleInterface
         double creep_velocity;
         double maximum_peeking_distance;
       } absence_traffic_light;
+      double attention_lane_crop_curvature_threshold;
+      double attention_lane_curvature_calculation_ds;
     } occlusion;
   };
 

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>
@@ -42,6 +43,22 @@
 #include <tuple>
 #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;
@@ -809,10 +826,26 @@ TrafficPrioritizedLevel getTrafficPrioritizedLevel(
   return TrafficPrioritizedLevel::NOT_PRIORITIZED;
 }
 
+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<DiscretizedLane> 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;
@@ -824,6 +857,12 @@ std::vector<DiscretizedLane> generateDetectionLaneDivisions(
     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);
   }
 

planning/behavior_velocity_intersection_module/src/util.hpp

@@ -109,7 +109,8 @@ TrafficPrioritizedLevel getTrafficPrioritizedLevel(
 
 std::vector<DiscretizedLane> 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 @@ struct DebugData
   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};