fix(intersection): generate yield stuck detect area from multiple lanes

planning/behavior_velocity_intersection_module/README.md

@@ -184,6 +184,12 @@ If there is any object on the path inside the intersection and at the exit of th
 
 ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg)
 
+## Yield stuck vehicle detection
+
+If there is any stopped object on the attention lanelet between the intersection point with ego path and the position which is `yield_stuck.distance_threshold` before that position, the object is regarded as yielding to ego vehicle. In this case ego is given the right-of-way by the yielding object but this module inserts stopline to prevent entry into the intersection. This scene happens when the object is yielding against ego or the object is waiting before the crosswalk around the exit of the intersection.
+
+![yield_stuck_detection](./docs/yield-stuck.drawio.svg)
+
 ## Collision detection
 
 The following process is performed for the targets objects to determine whether ego can pass the intersection safely. If it is judged that ego cannot pass the intersection with enough margin, this module inserts a stopline on the path.

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -29,9 +29,9 @@
       yield_stuck:
         turn_direction:
           left: true
-          right: false
+          right: true
           straight: false
-        distance_threshold: 1.0
+        distance_threshold: 5.0
 
       collision_detection:
         consider_wrong_direction_vehicle: false

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -196,6 +196,14 @@
       &debug_marker_array, now);
   }
 
+  if (debug_data_.yield_stuck_detect_area) {
+    appendMarkerArray(
+      createLaneletPolygonsMarkerArray(
+        debug_data_.yield_stuck_detect_area.value(), "yield_stuck_detect_area", lane_id_, 0.6588235,
+        0.34509, 0.6588235),
+      &debug_marker_array);
+  }
+
   if (debug_data_.ego_lane) {
     appendMarkerArray(
       createLaneletPolygonsMarkerArray(

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -1056,74 +1056,104 @@
   // stuck vehicle detection is viable even if attention area is empty
   // so this needs to be checked before attention area validation
   const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets);
-  if (stuck_detected && stuck_stopline_idx_opt) {
-    auto stuck_stopline_idx = stuck_stopline_idx_opt.value();
+  if (stuck_detected) {
     if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) {
-      if (
-        default_stopline_idx_opt &&
-        fromEgoDist(stuck_stopline_idx) < -planner_param_.common.stopline_overshoot_margin) {
-        stuck_stopline_idx = default_stopline_idx_opt.value();
-      }
     } else {
-      return IntersectionModule::StuckStop{
-        closest_idx, stuck_stopline_idx, occlusion_peeking_stopline_idx_opt};
+      std::optional<size_t> stopline_idx = std::nullopt;
+      if (stuck_stopline_idx_opt) {
+        const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) <
+                                            -planner_param_.common.stopline_overshoot_margin;
+        if (!is_over_stuck_stopline) {
+          stopline_idx = stuck_stopline_idx_opt.value();
+        }
+      }
+      if (!stopline_idx) {
+        if (default_stopline_idx_opt && fromEgoDist(default_stopline_idx_opt.value()) >= 0.0) {
+          stopline_idx = default_stopline_idx_opt.value();
+        } else if (
+          first_attention_stopline_idx_opt &&
+          fromEgoDist(first_attention_stopline_idx_opt.value()) >= 0.0) {
+          stopline_idx = closest_idx;
+        }
+      }
+      if (stopline_idx) {
+        return IntersectionModule::StuckStop{
+          closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt};
+      }
     }
   }
 
-  // yield stuck vehicle detection is viable even if attention area is empty
-  // so this needs to be checked before attention area validation
-  const bool yield_stuck_detected = checkYieldStuckVehicle(
-    planner_data_, path_lanelets, intersection_lanelets.first_attention_area());
-  if (yield_stuck_detected && stuck_stopline_idx_opt) {
-    const auto stuck_stopline_idx = stuck_stopline_idx_opt.value();
-    return IntersectionModule::YieldStuckStop{closest_idx, stuck_stopline_idx};
-  }
-
   // if attention area is empty, collision/occlusion detection is impossible
   if (!first_attention_area_opt) {
     return IntersectionModule::Indecisive{"attention area is empty"};
   }
   const auto first_attention_area = first_attention_area_opt.value();
 
   // if attention area is not null but default stop line is not available, ego/backward-path has
   // already passed the stop line
   if (!default_stopline_idx_opt) {
     return IntersectionModule::Indecisive{"default stop line is null"};
   }
   // occlusion stop line is generated from the intersection of ego footprint along the path with the
   // attention area, so if this is null, eog has already passed the intersection
   if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) {
     return IntersectionModule::Indecisive{"occlusion stop line is null"};
   }
   const auto default_stopline_idx = default_stopline_idx_opt.value();
   const bool is_over_default_stopline =
     util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx);
   const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx;
   const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value();
   const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value();
 
   const auto & adjacent_lanelets = intersection_lanelets.adjacent();
   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();
 
   // check occlusion on detection lane
   if (!occlusion_attention_divisions_) {
     occlusion_attention_divisions_ = util::generateDetectionLaneDivisions(
       occlusion_attention_lanelets, routing_graph_ptr,
       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();
 
   // get intersection area
   const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr);
   // filter objects
   auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area);
 
+  const bool yield_stuck_detected = checkYieldStuckVehicle(
+    target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding_);
+  if (yield_stuck_detected) {
+    std::optional<size_t> stopline_idx = std::nullopt;
+    const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0;
+    const bool is_before_first_attention_stopline =
+      fromEgoDist(first_attention_stopline_idx) >= 0.0;
+    if (stuck_stopline_idx_opt) {
+      const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) <
+                                          -planner_param_.common.stopline_overshoot_margin;
+      if (!is_over_stuck_stopline) {
+        stopline_idx = stuck_stopline_idx_opt.value();
+      }
+    }
+    if (!stopline_idx) {
+      if (is_before_default_stopline) {
+        stopline_idx = default_stopline_idx;
+      } else if (is_before_first_attention_stopline) {
+        stopline_idx = closest_idx;
+      }
+    }
+    if (stopline_idx) {
+      return IntersectionModule::YieldStuckStop{closest_idx, stopline_idx.value()};
+    }
+  }
+
   const double is_amber_or_red =
     (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) ||
     (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED);
@@ -1376,13 +1406,10 @@
 }
 
 bool IntersectionModule::checkYieldStuckVehicle(
-  const std::shared_ptr<const PlannerData> & planner_data, const util::PathLanelets & path_lanelets,
-  const std::optional<lanelet::CompoundPolygon3d> & first_attention_area)
+  const util::TargetObjects & target_objects,
+  const util::InterpolatedPathInfo & interpolated_path_info,
+  const lanelet::ConstLanelets & attention_lanelets)
 {
-  if (!first_attention_area) {
-    return false;
-  }
-
   const bool yield_stuck_detection_direction = [&]() {
     return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) ||
            (turn_direction_ == "right" && planner_param_.yield_stuck.turn_direction.right) ||
@@ -1392,13 +1419,9 @@
     return false;
   }
 
-  const auto & objects_ptr = planner_data->predicted_objects;
-
-  const auto & ego_lane = path_lanelets.ego_or_entry2exit;
-  const auto ego_poly = ego_lane.polygon2d().basicPolygon();
-
   return util::checkYieldStuckVehicleInIntersection(
-    objects_ptr, ego_poly, first_attention_area.value(),
+    target_objects, interpolated_path_info, attention_lanelets, turn_direction_,
+    planner_data_->vehicle_info_.vehicle_width_m,
     planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold,
     planner_param_.yield_stuck.distance_threshold, &debug_data_);
 }

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -336,9 +336,9 @@ class IntersectionModule : public SceneModuleInterface
     const util::PathLanelets & path_lanelets);
 
   bool checkYieldStuckVehicle(
-    const std::shared_ptr<const PlannerData> & planner_data,
-    const util::PathLanelets & path_lanelets,
-    const std::optional<lanelet::CompoundPolygon3d> & first_attention_area);
+    const util::TargetObjects & target_objects,
+    const util::InterpolatedPathInfo & interpolated_path_info,
+    const lanelet::ConstLanelets & attention_lanelets);
 
   util::TargetObjects generateTargetObjects(
     const util::IntersectionLanelets & intersection_lanelets,

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -31,6 +31,7 @@
 #include <boost/geometry/algorithms/correct.hpp>
 #include <boost/geometry/algorithms/intersects.hpp>
 
+#include <lanelet2_core/geometry/LineString.h>
 #include <lanelet2_core/geometry/Polygon.h>
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 
@@ -298,7 +299,7 @@
     const auto path_footprint = tier4_autoware_utils::transformVector(
       local_footprint, tier4_autoware_utils::pose2transform(base_pose));
     if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) {
-      // TODO(Mamoru Sobue): maybe consideration of braking dist is necessary
+      // NOTE: maybe consideration of braking dist is necessary
       first_footprint_attention_centerline_ip_opt = i;
       break;
     }
@@ -1179,40 +1180,145 @@
   return false;
 }
 
-bool checkYieldStuckVehicleInIntersection(
-  const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
-  const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area,
-  const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, DebugData * debug_data)
+static lanelet::LineString3d getLineStringFromArcLength(
+  const lanelet::ConstLineString3d & linestring, const double s1, const double s2)
 {
-  const auto first_attention_area_2d = lanelet::utils::to2D(first_attention_area);
-  Polygon2d first_attention_area_poly;
-  for (const auto & p : first_attention_area_2d) {
-    first_attention_area_poly.outer().emplace_back(p.x(), p.y());
-  }
-
-  for (const auto & object : objects_ptr->objects) {
-    if (!isTargetStuckVehicleType(object)) {
-      continue;  // not target vehicle type
+  lanelet::Points3d points;
+  double accumulated_length = 0;
+  size_t start_index = linestring.size();
+  for (size_t i = 0; i < linestring.size() - 1; i++) {
+    const auto & p1 = linestring[i];
+    const auto & p2 = linestring[i + 1];
+    const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint());
+    if (accumulated_length + length > s1) {
+      start_index = i;
+      break;
     }
-    const auto obj_v_norm = std::hypot(
-      object.kinematics.initial_twist_with_covariance.twist.linear.x,
-      object.kinematics.initial_twist_with_covariance.twist.linear.y);
-    if (obj_v_norm > stuck_vehicle_vel_thr) {
-      continue;  // not stop vehicle
+    accumulated_length += length;
+  }
+  if (start_index < linestring.size() - 1) {
+    const auto & p1 = linestring[start_index];
+    const auto & p2 = linestring[start_index + 1];
+    const double residue = s1 - accumulated_length;
+    const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized();
+    const auto start_basic_point = p1.basicPoint() + residue * direction_vector;
+    const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point);
+    points.push_back(start_point);
+  }
+
+  accumulated_length = 0;
+  size_t end_index = linestring.size();
+  for (size_t i = 0; i < linestring.size() - 1; i++) {
+    const auto & p1 = linestring[i];
+    const auto & p2 = linestring[i + 1];
+    const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint());
+    if (accumulated_length + length > s2) {
+      end_index = i;
+      break;
     }
+    accumulated_length += length;
+  }
 
-    const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object);
+  for (size_t i = start_index + 1; i < end_index; i++) {
+    const auto p = lanelet::Point3d(linestring[i]);
+    points.push_back(p);
+  }
+  if (end_index < linestring.size() - 1) {
+    const auto & p1 = linestring[end_index];
+    const auto & p2 = linestring[end_index + 1];
+    const double residue = s2 - accumulated_length;
+    const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized();
+    const auto end_basic_point = p1.basicPoint() + residue * direction_vector;
+    const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point);
+    points.push_back(end_point);
+  }
+  return lanelet::LineString3d{lanelet::InvalId, points};
+}
 
-    // check if the object is too close to the ego path
-    if (yield_stuck_distance_thr < bg::distance(ego_poly, obj_footprint)) {
+static lanelet::ConstLanelet createLaneletFromArcLength(
+  const lanelet::ConstLanelet & lanelet, const double s1, const double s2)
+{
+  const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString());
+  // make sure that s1, and s2 are between [0, lane_length]
+  const auto s1_saturated = std::max(0.0, std::min(s1, total_length));
+  const auto s2_saturated = std::max(0.0, std::min(s2, total_length));
+
+  const auto ratio_s1 = s1_saturated / total_length;
+  const auto ratio_s2 = s2_saturated / total_length;
+
+  const auto s1_left =
+    static_cast<double>(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString()));
+  const auto s2_left =
+    static_cast<double>(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString()));
+  const auto s1_right =
+    static_cast<double>(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString()));
+  const auto s2_right =
+    static_cast<double>(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString()));
+
+  const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left);
+  const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right);
+
+  return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound);
+}
+
+bool checkYieldStuckVehicleInIntersection(
+  const util::TargetObjects & target_objects,
+  const util::InterpolatedPathInfo & interpolated_path_info,
+  const lanelet::ConstLanelets & attention_lanelets, const std::string & turn_direction,
+  const double width, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr,
+  DebugData * debug_data)
+{
+  LineString2d sparse_intersection_path;
+  const auto [start, end] = interpolated_path_info.lane_id_interval.value();
+  for (unsigned i = start; i < end; ++i) {
+    const auto & point = interpolated_path_info.path.points.at(i).point.pose.position;
+    const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation);
+    if (turn_direction == "right") {
+      const double right_x = point.x - width / 2 * std::sin(yaw);
+      const double right_y = point.y + width / 2 * std::cos(yaw);
+      sparse_intersection_path.emplace_back(right_x, right_y);
+    } else if (turn_direction == "left") {
+      const double left_x = point.x + width / 2 * std::sin(yaw);
+      const double left_y = point.y - width / 2 * std::cos(yaw);
+      sparse_intersection_path.emplace_back(left_x, left_y);
+    } else {
+      // straight
+      sparse_intersection_path.emplace_back(point.x, point.y);
+    }
+  }
+  lanelet::ConstLanelets yield_stuck_detect_lanelets;
+  for (const auto & attention_lanelet : attention_lanelets) {
+    const auto centerline = attention_lanelet.centerline2d().basicLineString();
+    std::vector<Point2d> intersects;
+    bg::intersection(sparse_intersection_path, centerline, intersects);
+    if (intersects.empty()) {
       continue;
     }
+    const auto intersect = intersects.front();
+    const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates(
+      centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y()));
+    const double yield_stuck_start =
+      std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr);
+    const double yield_stuck_end = intersect_arc_coords.length;
+    yield_stuck_detect_lanelets.push_back(
+      createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end));
+  }
+  debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets);
+  for (const auto & object : target_objects.all_attention_objects) {
+    const auto obj_v_norm = std::hypot(
+      object.object.kinematics.initial_twist_with_covariance.twist.linear.x,
+      object.object.kinematics.initial_twist_with_covariance.twist.linear.y);
 
-    // check if the footprint is in the stuck detect area
-    const bool is_in_stuck_area = bg::within(obj_footprint, first_attention_area_poly);
-    if (is_in_stuck_area && debug_data) {
-      debug_data->yield_stuck_targets.objects.push_back(object);
-      return true;
+    if (obj_v_norm > stuck_vehicle_vel_thr) {
+      continue;
+    }
+    for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) {
+      const bool is_in_lanelet = lanelet::utils::isInLanelet(
+        object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet);
+      if (is_in_lanelet) {
+        debug_data->yield_stuck_targets.objects.push_back(object.object);
+        return true;
+      }
     }
   }
   return false;
@@ -1527,6 +1633,7 @@
     const double yaw = tf2::getYaw(p.orientation);
     const double x = p.position.x;
     const double y = p.position.y;
+    // NOTE(Mamoru Sobue): maybe this is opposite
     const double left_x = x + width / 2 * std::sin(yaw);
     const double left_y = y - width / 2 * std::cos(yaw);
     const double right_x = x - width / 2 * std::sin(yaw);

planning/behavior_velocity_intersection_module/src/util.hpp

@@ -131,9 +131,10 @@ bool checkStuckVehicleInIntersection(
   DebugData * debug_data);
 
 bool checkYieldStuckVehicleInIntersection(
-  const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
-  const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area,
-  const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr,
+  const util::TargetObjects & target_objects,
+  const util::InterpolatedPathInfo & interpolated_path_info,
+  const lanelet::ConstLanelets & attention_lanelets, const std::string & turn_direction,
+  const double width, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr,
   DebugData * debug_data);
 
 Polygon2d generateStuckVehicleDetectAreaPolygon(

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -45,6 +45,7 @@ struct DebugData
   std::optional<lanelet::CompoundPolygon3d> ego_lane{std::nullopt};
   std::optional<std::vector<lanelet::CompoundPolygon3d>> adjacent_area{std::nullopt};
   std::optional<geometry_msgs::msg::Polygon> stuck_vehicle_detect_area{std::nullopt};
+  std::optional<std::vector<lanelet::CompoundPolygon3d>> yield_stuck_detect_area{std::nullopt};
   std::optional<geometry_msgs::msg::Polygon> candidate_collision_ego_lane_polygon{std::nullopt};
   std::vector<geometry_msgs::msg::Polygon> candidate_collision_object_polygons;
   autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets;