chore(intersection): parameterize stuck vehicle detection turn_direct…

…ion (#5277) Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Oct 12, 2023 · f872838 · f872838
1 parent bc60bbf
commit f872838
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 2 deletions.
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -13,6 +13,10 @@
         path_interpolation_ds: 0.1 # [m]
         consider_wrong_direction_vehicle: false
       stuck_vehicle:
+        turn_direction:
+          left: true
+          right: true
+          straight: true
         use_stuck_stopline: true # stopline generated before the first conflicting area
         stuck_vehicle_detect_dist: 5.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length.
         stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h

diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -63,6 +63,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.common.consider_wrong_direction_vehicle =
     getOrDeclareParameter<bool>(node, ns + ".common.consider_wrong_direction_vehicle");
 
+  ip.stuck_vehicle.turn_direction.left =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.turn_direction.left");
+  ip.stuck_vehicle.turn_direction.right =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.turn_direction.right");
+  ip.stuck_vehicle.turn_direction.straight =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.turn_direction.straight");
   ip.stuck_vehicle.use_stuck_stopline =
     getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.use_stuck_stopline");
   ip.stuck_vehicle.stuck_vehicle_detect_dist =

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -1213,8 +1213,12 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
 bool IntersectionModule::checkStuckVehicle(
   const std::shared_ptr<const PlannerData> & planner_data, const util::PathLanelets & path_lanelets)
 {
-  // NOTE: No need to stop for stuck vehicle when the ego will turn left.
-  if (turn_direction_ == std::string("left")) {
+  const bool stuck_detection_direction = [&]() {
+    return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) ||
+           (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) ||
+           (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight);
+  }();
+  if (!stuck_detection_direction) {
     return false;
   }
 

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -61,6 +61,12 @@ class IntersectionModule : public SceneModuleInterface
     } common;
     struct StuckVehicle
     {
+      struct TurnDirection
+      {
+        bool left;
+        bool right;
+        bool straight;
+      } 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