From c30d7a57f68315b9ddf59d0a55a98b8b8d4c1886 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Fri, 6 Oct 2023 16:17:29 +0900
Subject: [PATCH] hot-fix of
 https://github.com/autowarefoundation/autoware.universe/pull/5229

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../config/intersection.param.yaml            |  1 +
 .../src/manager.cpp                           |  2 ++
 .../src/scene_intersection.cpp                | 21 +++----------------
 .../src/scene_intersection.hpp                |  2 +-
 4 files changed, 7 insertions(+), 19 deletions(-)

diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
index e1ae01e581f86..da2284169888d 100644
--- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
+++ b/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
diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
index 2813105766c2c..929a3aba4539c 100644
--- a/planning/behavior_velocity_intersection_module/src/manager.cpp
+++ b/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");
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
index 4e0b91df8336d..9f23a48932e45 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
+++ b/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};
     }
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
index e75b22dbe828a..b9d8781e3f2e4 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
+++ b/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_;