feat(intersection)!: disable the exception behavior in the private ar…

…eas (#5229) * feat: make configurable to disable the exception treat of stuck obstacle in the private areas feat: change behavior in the private areas Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * delete the coment outed lines Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * change "enabled" to "enable" Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * fix setting Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * delete unused variables Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> --------- Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
autowarefoundation · Oct 6, 2023 · 950805f · 950805f
1 parent 6dfb197
commit 950805f
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 10 deletions.
diff --git 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
@@ -77,6 +77,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
@@ -96,10 +96,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);
-  }
   {
     temporal_stop_before_attention_state_machine_.setMarginTime(
       planner_param_.occlusion.before_creep_stop_time);
@@ -918,16 +914,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets);
   if (stuck_detected && stuck_stop_line_idx_opt) {
     auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value();
-    const bool stopped_at_stuck_line = stoppedForDuration(
-      stuck_stop_line_idx, planner_param_.stuck_vehicle.timeout_private_area,
-      stuck_private_area_timeout_);
-    const bool timeout = (is_private_area_ && stopped_at_stuck_line);
-    if (!timeout) {
+    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::StuckStop{
         closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt};
     }

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -70,6 +70,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
     {
@@ -255,7 +256,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_;