feat(intersection): new feature of yield stuck detection (#5270)

* feat(intersection): new feature of yield stuck detection Signed-off-by: Takayuki Murooka <[email protected]> * update config Signed-off-by: Takayuki Murooka <[email protected]> * use turn_direction Signed-off-by: Takayuki Murooka <[email protected]> * update config Signed-off-by: Takayuki Murooka <[email protected]> * update Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
autowarefoundation · Oct 13, 2023 · 8b1f3c0 · 8b1f3c0
1 parent def9ffc
commit 8b1f3c0
Show file tree

Hide file tree

Showing 8 changed files with 181 additions and 4 deletions.
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -24,6 +24,12 @@
         # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning
         timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area
         enable_private_area_stuck_disregard: false #In the intersections which labeled as "private area", the obstacle vehicles judged as "stuck" are neglected if this param is set as true.
+        yield_stuck:
+          turn_direction:
+            left: true
+            right: false
+            straight: false
+          distance_thr: 1.0 # [m]
 
       collision_detection:
         state_transit_margin_time: 1.0
@@ -42,9 +48,9 @@
         use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module
         minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity
         yield_on_green_traffic_light:
-          distance_to_assigned_lanelet_start: 5.0
-          duration: 2.0
-          range: 30.0 # [m]
+          distance_to_assigned_lanelet_start: 10.0
+          duration: 3.0
+          range: 50.0 # [m]
 
       occlusion:
         enable: false

diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp
@@ -238,6 +238,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2),
     &debug_marker_array, now);
 
+  appendMarkerArray(
+    debug::createObjectsMarkerArray(
+      debug_data_.yield_stuck_targets, "stuck_targets", module_id_, now, 0.4, 0.99, 0.2),
+    &debug_marker_array, now);
+
   appendMarkerArray(
     debug::createObjectsMarkerArray(
       debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99,

diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -85,6 +85,14 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.timeout_private_area");
   ip.stuck_vehicle.enable_private_area_stuck_disregard =
     getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard");
+  ip.stuck_vehicle.yield_stuck_turn_direction.left =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.left");
+  ip.stuck_vehicle.yield_stuck_turn_direction.right =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.right");
+  ip.stuck_vehicle.yield_stuck_turn_direction.straight =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.straight");
+  ip.stuck_vehicle.yield_stuck_distance_thr =
+    getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.yield_stuck.distance_thr");
 
   ip.collision_detection.min_predicted_path_confidence =
     getOrDeclareParameter<double>(node, ns + ".collision_detection.min_predicted_path_confidence");

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -195,6 +195,21 @@ void prepareRTCByDecisionResult(
   return;
 }
 
+template <>
+void prepareRTCByDecisionResult(
+  const IntersectionModule::YieldStuckStop & result,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety,
+  double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance)
+{
+  RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "YieldStuckStop");
+  const auto closest_idx = result.closest_idx;
+  const auto stop_line_idx = result.stuck_stop_line_idx;
+  *default_safety = false;
+  *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx);
+  *occlusion_safety = true;
+  return;
+}
+
 template <>
 void prepareRTCByDecisionResult(
   const IntersectionModule::NonOccludedCollisionStop & result,
@@ -419,6 +434,38 @@ void reactRTCApprovalByDecisionResult(
   return;
 }
 
+template <>
+void reactRTCApprovalByDecisionResult(
+  const bool rtc_default_approved, const bool rtc_occlusion_approved,
+  const IntersectionModule::YieldStuckStop & decision_result,
+  [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param,
+  const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+  StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data)
+{
+  RCLCPP_DEBUG(
+    rclcpp::get_logger("reactRTCApprovalByDecisionResult"),
+    "YieldStuckStop, approval = (default: %d, occlusion: %d)", rtc_default_approved,
+    rtc_occlusion_approved);
+  const auto closest_idx = decision_result.closest_idx;
+  if (!rtc_default_approved) {
+    // use default_rtc uuid for stuck vehicle detection
+    const auto stop_line_idx = decision_result.stuck_stop_line_idx;
+    planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path);
+    debug_data->collision_stop_wall_pose =
+      planning_utils::getAheadPose(stop_line_idx, baselink2front, *path);
+    {
+      tier4_planning_msgs::msg::StopFactor stop_factor;
+      stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose;
+      stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets);
+      planning_utils::appendStopReason(stop_factor, stop_reason);
+      velocity_factor->set(
+        path->points, path->points.at(closest_idx).point.pose,
+        path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION);
+    }
+  }
+  return;
+}
+
 template <>
 void reactRTCApprovalByDecisionResult(
   const bool rtc_default_approved, const bool rtc_occlusion_approved,
@@ -780,6 +827,9 @@ static std::string formatDecisionResult(const IntersectionModule::DecisionResult
   if (std::holds_alternative<IntersectionModule::StuckStop>(decision_result)) {
     return "StuckStop";
   }
+  if (std::holds_alternative<IntersectionModule::YieldStuckStop>(decision_result)) {
+    return "YieldStuckStop";
+  }
   if (std::holds_alternative<IntersectionModule::NonOccludedCollisionStop>(decision_result)) {
     return "NonOccludedCollisionStop";
   }
@@ -985,6 +1035,23 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     }
   }
 
+  // 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_stop_line_idx_opt) {
+    auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value();
+    if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) {
+      if (
+        default_stop_line_idx_opt &&
+        fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) {
+        stuck_stop_line_idx = default_stop_line_idx_opt.value();
+      }
+    } else {
+      return IntersectionModule::YieldStuckStop{closest_idx, stuck_stop_line_idx};
+    }
+  }
+
   // if attention area is empty, collision/occlusion detection is impossible
   if (!first_attention_area_opt) {
     return IntersectionModule::Indecisive{"attention area is empty"};
@@ -1234,6 +1301,37 @@ bool IntersectionModule::checkStuckVehicle(
     &debug_data_);
 }
 
+bool IntersectionModule::checkYieldStuckVehicle(
+  const std::shared_ptr<const PlannerData> & planner_data, const util::PathLanelets & path_lanelets,
+  const std::optional<lanelet::CompoundPolygon3d> & first_attention_area)
+{
+  if (!first_attention_area) {
+    return false;
+  }
+
+  const bool yield_stuck_detection_direction = [&]() {
+    return (turn_direction_ == "left" &&
+            planner_param_.stuck_vehicle.yield_stuck_turn_direction.left) ||
+           (turn_direction_ == "right" &&
+            planner_param_.stuck_vehicle.yield_stuck_turn_direction.right) ||
+           (turn_direction_ == "straight" &&
+            planner_param_.stuck_vehicle.yield_stuck_turn_direction.straight);
+  }();
+  if (!yield_stuck_detection_direction) {
+    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(),
+    planner_param_.stuck_vehicle.stuck_vehicle_vel_thr,
+    planner_param_.stuck_vehicle.yield_stuck_distance_thr, &debug_data_);
+}
+
 autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects(
   const lanelet::ConstLanelets & attention_area_lanelets,
   const lanelet::ConstLanelets & adjacent_lanelets,

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -66,7 +66,8 @@ class IntersectionModule : public SceneModuleInterface
         bool left;
         bool right;
         bool straight;
-      } turn_direction;
+      };
+      TurnDirection turn_direction;
       bool use_stuck_stopline;  //! stopline generate before the intersection lanelet when is_stuck.
       double stuck_vehicle_detect_dist;  //! distance from end point to finish stuck vehicle check
       double stuck_vehicle_vel_thr;      //! Threshold of the speed to be recognized as stopped
@@ -77,6 +78,8 @@ class IntersectionModule : public SceneModuleInterface
       */
       double timeout_private_area;
       bool enable_private_area_stuck_disregard;
+      double yield_stuck_distance_thr;
+      TurnDirection yield_stuck_turn_direction;
     } stuck_vehicle;
     struct CollisionDetection
     {
@@ -147,6 +150,11 @@ class IntersectionModule : public SceneModuleInterface
     size_t stuck_stop_line_idx{0};
     std::optional<size_t> occlusion_stop_line_idx{std::nullopt};
   };
+  struct YieldStuckStop
+  {
+    size_t closest_idx{0};
+    size_t stuck_stop_line_idx{0};
+  };
   struct NonOccludedCollisionStop
   {
     size_t closest_idx{0};
@@ -206,6 +214,7 @@ class IntersectionModule : public SceneModuleInterface
   using DecisionResult = std::variant<
     Indecisive,                   // internal process error, or over the pass judge line
     StuckStop,                    // detected stuck vehicle
+    YieldStuckStop,               // detected yield stuck vehicle
     NonOccludedCollisionStop,     // detected collision while FOV is clear
     FirstWaitBeforeOcclusion,     // stop for a while before peeking to occlusion
     PeekingTowardOcclusion,       // peeking into occlusion while collision is not detected
@@ -288,6 +297,11 @@ class IntersectionModule : public SceneModuleInterface
     const std::shared_ptr<const PlannerData> & planner_data,
     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);
+
   autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects(
     const lanelet::ConstLanelets & attention_area_lanelets,
     const lanelet::ConstLanelets & adjacent_lanelets,

diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp
@@ -1108,6 +1108,45 @@ bool checkStuckVehicleInIntersection(
   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)
+{
+  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
+    }
+    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
+    }
+
+    const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object);
+
+    // check if the object is too close to the ego path
+    if (yield_stuck_distance_thr < bg::distance(ego_poly, obj_footprint)) {
+      continue;
+    }
+
+    // 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;
+    }
+  }
+  return false;
+}
+
 Polygon2d generateStuckVehicleDetectAreaPolygon(
   const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist)
 {

diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp
@@ -125,6 +125,12 @@ bool checkStuckVehicleInIntersection(
   const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr,
   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,
+  DebugData * debug_data);
+
 Polygon2d generateStuckVehicleDetectAreaPolygon(
   const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist);
 

diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/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 stuck_targets;
+  autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets;
   std::vector<geometry_msgs::msg::Polygon> occlusion_polygons;
   std::optional<std::pair<geometry_msgs::msg::Point, geometry_msgs::msg::Point>>
     nearest_occlusion_projection{std::nullopt};