fix: suppress unintended permits at intersections

planning/behavior_velocity_intersection_module/README.md

@@ -8,7 +8,7 @@
 2. recognizing the occluded area in the intersection
 3. reacting to arrow signals of associated traffic lights
 
 The module is desinged to be agnositc to left-hand/right-hand traffic rules and works on crossroads, T-shape junctions, etc.
 
 ![topology](./docs/intersection-topology.drawio.svg)
 
@@ -26,7 +26,7 @@
 
 The `Attention Area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority.
 
 `Intersection Area`, which is supposed to be defined on the HDMap, is an area convering the entire intersection.
 
 ![attention_area](./docs/intersection-attention.drawio.svg)
 
@@ -38,7 +38,7 @@
 | ------------------------------ | --------------------------------------------------------------- | ------------------------------------------------ |
 | straight                       | Highest priority of all                                         | Priority over left/right lanes of the same group |
 | left(Left hand traffic)        | Priority over the other group and right lanes of the same group | Priority over right lanes of the same group      |
 | right(Left hand traffic)       | Priority only over the other group                              | priority only over the other gruop               |
 | left(Right hand traffic)       | Priority only over the other group                              | Priority only over the other group               |
 | right(Right hand traffic)      | Priority over the other group and left lanes of the same group  | priority over left lanes of the same group       |
 
@@ -62,7 +62,7 @@
 
 #### Stuck Vehicle Detection
 
-If there is any object in a certain distance (`stuck_vehicle_ignore_dist` and `stuck_vehicle_detect_dist`) before and after the exit of the intersection lane and the object velocity is less than a threshold (=`stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path
+If there is any object on the path in inside the intersection and at the exit of the intersection (up to `stuck_vehicle_detect_dist`) lane and its velocity is less than a threshold (`stuck_vehicle.stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path
 
 ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg)
 
@@ -107,8 +107,7 @@
 | `common.stop_overshoot_margin`                      | double | [m] margin for the overshoot from stopline                                                     |
 | `common.path_interpolation_ds`                      | double | [m] path interpolation interval                                                                |
 | `stuck_vehicle.stuck_vehicle_detect_dist`           | double | [m] length toward from the exit of intersection for stuck vehicle detection                    |
-| `stuck_vehicle.stuck_vehicle_ignore_dist`           | double | [m] length behind the exit of intersection for stuck vehicle detection                         |
-| `stuck_vehicle.stuck_vehicle_vel_thr`               | double | [m/s] velocity threhsold for stuck vehicle detection                                           |
+| `stuck_vehicle.stuck_vehicle_vel_thr`               | double | [m/s] velocity threshold for stuck vehicle detection                                           |
 | `collision_detection.state_transit_margin_time`     | double | [m] time margin to change state                                                                |
 | `collision_detection.min_predicted_path_confidence` | double | [-] minimum confidence value of predicted path to use for collision detection                  |
 | `collision_detection.collision_start_margin_time`   | double | [s] time margin for the beginning of collision with upcoming vehicle                           |

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -15,7 +15,6 @@
       stuck_vehicle:
         use_stuck_stopline: true # stopline generated before the first conflicting area
         stuck_vehicle_detect_dist: 3.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_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
         stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h
         # 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
@@ -48,6 +47,7 @@
         denoise_kernel: 1.0 # [m]
         possible_object_bbox: [1.0, 2.0] # [m x m]
         ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h
+        stop_release_margin_time: 1.0 # [s]
 
       enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
         intersection: true

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -40,7 +40,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
 {
   const std::string ns(getModuleName());
   auto & ip = intersection_param_;
-  const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
   ip.common.attention_area_margin =
     node.declare_parameter<double>(ns + ".common.attention_area_margin");
   ip.common.attention_area_length =
@@ -65,9 +64,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     node.declare_parameter<bool>(ns + ".stuck_vehicle.use_stuck_stopline");
   ip.stuck_vehicle.stuck_vehicle_detect_dist =
     node.declare_parameter<double>(ns + ".stuck_vehicle.stuck_vehicle_detect_dist");
-  ip.stuck_vehicle.stuck_vehicle_ignore_dist =
-    node.declare_parameter<double>(ns + ".stuck_vehicle.stuck_vehicle_ignore_dist") +
-    vehicle_info.max_longitudinal_offset_m;
   ip.stuck_vehicle.stuck_vehicle_vel_thr =
     node.declare_parameter<double>(ns + ".stuck_vehicle.stuck_vehicle_vel_thr");
   /*
@@ -117,6 +113,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     node.declare_parameter<std::vector<double>>(ns + ".occlusion.possible_object_bbox");
   ip.occlusion.ignore_parked_vehicle_speed_threshold =
     node.declare_parameter<double>(ns + ".occlusion.ignore_parked_vehicle_speed_threshold");
+  ip.occlusion.stop_release_margin_time =
+    node.declare_parameter<double>(ns + ".occlusion.stop_release_margin_time");
 }
 
 void IntersectionModuleManager::launchNewModules(