hot-fix of autowarefoundation#5229

Signed-off-by: Yuki Takagi <[email protected]>

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -19,6 +19,7 @@
         # enable_front_car_decel_prediction: false # By default this feature is disabled
         # 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.
 
       collision_detection:
         state_transit_margin_time: 1.0

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -81,6 +81,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   */
   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.collision_detection.min_predicted_path_confidence =
     getOrDeclareParameter<double>(node, ns + ".collision_detection.min_predicted_path_confidence");

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -90,10 +90,6 @@ IntersectionModule::IntersectionModule(
     occlusion_stop_state_machine_.setMarginTime(planner_param_.occlusion.stop_release_margin_time);
     occlusion_stop_state_machine_.setState(StateMachine::State::GO);
   }
-  {
-    stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area);
-    stuck_private_area_timeout_.setState(StateMachine::State::STOP);
-  }
 
   decision_state_pub_ =
     node_.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
@@ -789,20 +785,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     checkStuckVehicle(planner_data_, ego_lane_with_next_lane, *path, intersection_stop_lines);
 
   if (stuck_detected) {
-    const double dist_stopline = motion_utils::calcSignedArcLength(
-      path->points, path->points.at(closest_idx).point.pose.position,
-      path->points.at(stuck_stop_line_idx).point.pose.position);
-    const bool approached_stop_line =
-      (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin);
-    const bool is_stopped = planner_data_->isVehicleStopped();
-    if (is_stopped && approached_stop_line) {
-      stuck_private_area_timeout_.setStateWithMarginTime(
-        StateMachine::State::GO, logger_.get_child("stuck_private_area_timeout"), *clock_);
-    }
-    const bool timeout =
-      (is_private_area_ && stuck_private_area_timeout_.getState() == StateMachine::State::GO);
-    if (!timeout) {
-      is_peeking_ = false;
+    if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) {
+      ;  // do nothing (in order to archive correspondence to awf-main)
+    } else {
       return IntersectionModule::StuckStop{
         stuck_stop_line_idx, !first_attention_area.has_value(), intersection_stop_lines};
     }

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -74,6 +74,7 @@ class IntersectionModule : public SceneModuleInterface
       bool enable_front_car_decel_prediction;  //! flag for using above feature
       */
       double timeout_private_area;
+      bool enable_private_area_stuck_disregard;
     } stuck_vehicle;
     struct CollisionDetection
     {
@@ -231,7 +232,6 @@ class IntersectionModule : public SceneModuleInterface
 
   // for stuck vehicle detection
   const bool is_private_area_;
-  StateMachine stuck_private_area_timeout_;
 
   // for RTC
   const UUID occlusion_uuid_;