feat(intersection): disable stuck detection against private lane (#5910)

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

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -23,8 +23,7 @@
         stuck_vehicle_velocity_threshold: 0.833
         # enable_front_car_decel_prediction: false
         # assumed_front_car_decel: 1.0
-        timeout_private_area: 3.0
-        enable_private_area_stuck_disregard: false
+        disable_against_private_lane: true
 
       yield_stuck:
         turn_direction:

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -74,10 +74,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist");
   ip.stuck_vehicle.stuck_vehicle_velocity_threshold =
     getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold");
-  ip.stuck_vehicle.timeout_private_area =
-    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.disable_against_private_lane =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.disable_against_private_lane");
 
   ip.yield_stuck.turn_direction.left =
     getOrDeclareParameter<bool>(node, ns + ".yield_stuck.turn_direction.left");
@@ -201,7 +199,6 @@ void IntersectionModuleManager::launchNewModules(
     }
 
     const std::string location = ll.attributeOr("location", "else");
-    const bool is_private_area = (location.compare("private") == 0);
     const auto associative_ids =
       planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph);
     bool has_traffic_light = false;
@@ -213,7 +210,7 @@ void IntersectionModuleManager::launchNewModules(
     }
     const auto new_module = std::make_shared<IntersectionModule>(
       module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction,
-      has_traffic_light, enable_occlusion_detection, is_private_area, node_,
+      has_traffic_light, enable_occlusion_detection, node_,
       logger_.get_child("intersection_module"), clock_);
     generateUUID(module_id);
     /* set RTC status as non_occluded status initially */

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -97,8 +97,8 @@ IntersectionModule::IntersectionModule(
   [[maybe_unused]] std::shared_ptr<const PlannerData> planner_data,
   const PlannerParam & planner_param, const std::set<lanelet::Id> & associative_ids,
   const std::string & turn_direction, const bool has_traffic_light,
-  const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node,
-  const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)
+  const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger,
+  const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
   node_(node),
   lane_id_(lane_id),
@@ -107,7 +107,6 @@ IntersectionModule::IntersectionModule(
   has_traffic_light_(has_traffic_light),
   enable_occlusion_detection_(enable_occlusion_detection),
   occlusion_attention_divisions_(std::nullopt),
-  is_private_area_(is_private_area),
   occlusion_uuid_(tier4_autoware_utils::generateUUID())
 {
   velocity_factor_.init(PlanningBehavior::INTERSECTION);
@@ -1056,8 +1055,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   // 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);
+  const bool is_first_conflicting_lane_private =
+    (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0);
   if (stuck_detected) {
-    if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) {
+    if (!(is_first_conflicting_lane_private &&
+          planner_param_.stuck_vehicle.disable_against_private_lane)) {
     } else {
       std::optional<size_t> stopline_idx = std::nullopt;
       if (stuck_stopline_idx_opt) {

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -74,8 +74,7 @@ class IntersectionModule : public SceneModuleInterface
       bool use_stuck_stopline;
       double stuck_vehicle_detect_dist;
       double stuck_vehicle_velocity_threshold;
-      double timeout_private_area;
-      bool enable_private_area_stuck_disregard;
+      bool disable_against_private_lane;
     } stuck_vehicle;
 
     struct YieldStuck
@@ -263,8 +262,8 @@ class IntersectionModule : public SceneModuleInterface
     const int64_t module_id, const int64_t lane_id, std::shared_ptr<const PlannerData> planner_data,
     const PlannerParam & planner_param, const std::set<lanelet::Id> & associative_ids,
     const std::string & turn_direction, const bool has_traffic_light,
-    const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node,
-    const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock);
+    const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger,
+    const rclcpp::Clock::SharedPtr clock);
 
   /**
    * @brief plan go-stop velocity at traffic crossing with collision check between reference path
@@ -314,9 +313,6 @@ class IntersectionModule : public SceneModuleInterface
   // vehicles are very slow
   std::optional<rclcpp::Time> initial_green_light_observed_time_{std::nullopt};
 
-  // for stuck vehicle detection
-  const bool is_private_area_;
-
   // for RTC
   const UUID occlusion_uuid_;
   bool occlusion_safety_{true};