feat(intersection): check path margin for overshoot vehicles on red l…

…ight (autowarefoundation#5394)

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -55,6 +55,8 @@
           object_dist_to_stopline: 10.0 # [m]
         ignore_on_amber_traffic_light:
           object_expected_deceleration: 2.0 # [m/ss]
+        ignore_on_red_traffic_light:
+          object_margin_to_path: 2.0
 
       occlusion:
         enable: false

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -229,6 +229,12 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0),
     &debug_marker_array, now);
 
+  appendMarkerArray(
+    debug::createObjectsMarkerArray(
+      debug_data_.red_overshoot_ignore_targets, "red_overshoot_ignore_targets", module_id_, now,
+      0.0, 1.0, 0.0),
+    &debug_marker_array, now);
+
   appendMarkerArray(
     debug::createObjectsMarkerArray(
       debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2),

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -139,6 +139,9 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration =
     getOrDeclareParameter<double>(
       node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration");
+  ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path");
 
   ip.occlusion.enable = getOrDeclareParameter<bool>(node, ns + ".occlusion.enable");
   ip.occlusion.occlusion_attention_area_length =

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -30,6 +30,7 @@
 #include <boost/geometry/algorithms/intersection.hpp>
 
 #include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_core/geometry/Point.h>
 #include <lanelet2_core/geometry/Polygon.h>
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 #include <lanelet2_core/primitives/LineString.h>
@@ -1466,18 +1467,64 @@ bool IntersectionModule::checkCollision(
                          .object_expected_deceleration));
     return dist_to_stop_line > braking_distance;
   };
-
+  const auto isTolerableOvershoot = [&](const util::TargetObject & target_object) {
+    if (
+      !target_object.attention_lanelet || !target_object.dist_to_stop_line ||
+      !target_object.stop_line) {
+      return false;
+    }
+    const double dist_to_stop_line = target_object.dist_to_stop_line.value();
+    const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x;
+    const double braking_distance =
+      v * v /
+      (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light
+                         .object_expected_deceleration));
+    if (dist_to_stop_line > braking_distance) {
+      return false;
+    }
+    const auto stopline_front = target_object.stop_line.value().front();
+    const auto stopline_back = target_object.stop_line.value().back();
+    tier4_autoware_utils::LineString2d object_line;
+    object_line.emplace_back(
+      (stopline_front.x() + stopline_back.x()) / 2.0,
+      (stopline_front.y() + stopline_back.y()) / 2.0);
+    const auto stopline_mid = object_line.front();
+    const auto endpoint = target_object.attention_lanelet.value().centerline().back();
+    object_line.emplace_back(endpoint.x(), endpoint.y());
+    std::vector<tier4_autoware_utils::Point2d> intersections;
+    bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections);
+    if (intersections.empty()) {
+      return false;
+    }
+    const auto collision_point = intersections.front();
+    // distance from object expected stop position to collision point
+    const double stopline_to_object = -1.0 * dist_to_stop_line + braking_distance;
+    const double stopline_to_collision =
+      std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y());
+    const double object2collision = stopline_to_collision - stopline_to_object;
+    const double margin =
+      planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path;
+    return (object2collision > margin) || (object2collision < 0);
+  };
   // check collision between predicted_path and ego_area
   bool collision_detected = false;
   for (const auto & target_object : target_objects->all) {
     const auto & object = target_object.object;
     // If the vehicle is expected to stop before their stopline, ignore
+    const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object);
     if (
       traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED &&
-      expectedToStopBeforeStopLine(target_object)) {
+      expected_to_stop_before_stopline) {
       debug_data_.amber_ignore_targets.objects.push_back(object);
       continue;
     }
+    const bool is_tolerable_overshoot = isTolerableOvershoot(target_object);
+    if (
+      traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED &&
+      !expected_to_stop_before_stopline && is_tolerable_overshoot) {
+      debug_data_.red_overshoot_ignore_targets.objects.push_back(object);
+      continue;
+    }
     for (const auto & predicted_path : object.kinematics.predicted_paths) {
       if (
         predicted_path.confidence <

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -118,6 +118,10 @@ class IntersectionModule : public SceneModuleInterface
       {
         double object_expected_deceleration;
       } ignore_on_amber_traffic_light;
+      struct IgnoreOnRedTrafficLight
+      {
+        double object_margin_to_path;
+      } ignore_on_red_traffic_light;
     } collision_detection;
     struct Occlusion
     {

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -831,9 +831,16 @@ IntersectionLanelets getObjectiveLanelets(
     result.attention_stop_lines_.push_back(stop_line);
   }
   result.attention_non_preceding_ = std::move(detection_lanelets);
-  // TODO(Mamoru Sobue): find stop lines for attention_non_preceding_ if needed
   for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) {
-    result.attention_non_preceding_stop_lines_.push_back(std::nullopt);
+    std::optional<lanelet::ConstLineString3d> stop_line = std::nullopt;
+    const auto & ll = result.attention_non_preceding_.at(i);
+    const auto traffic_lights = ll.regulatoryElementsAs<lanelet::TrafficLight>();
+    for (const auto & traffic_light : traffic_lights) {
+      const auto stop_line_opt = traffic_light->stopLine();
+      if (!stop_line_opt) continue;
+      stop_line = stop_line_opt.get();
+    }
+    result.attention_non_preceding_stop_lines_.push_back(stop_line);
   }
   result.conflicting_ = std::move(conflicting_ex_ego_lanelets);
   result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids);

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -50,6 +50,7 @@ struct DebugData
   std::vector<geometry_msgs::msg::Polygon> candidate_collision_object_polygons;
   autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets;
   autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets;
+  autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets;
   autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets;
   autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets;
   std::vector<geometry_msgs::msg::Polygon> occlusion_polygons;