Merge pull request #829 from tier4/hotfix/intersection

fix: suppress unintended permits at intersections

planning/behavior_velocity_intersection_module/README.md

@@ -62,7 +62,7 @@ Objects that satisfy all of the following conditions are considered as target ob
 
 #### 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 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav
 | `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(