Merge pull request autowarefoundation#761 from tier4/hotfix/pr4694

fix(intersection): modify ego polygon calculation

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -10,10 +10,9 @@
         intersection_max_accel: 0.5 # m/ss
         stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collision detection
         use_intersection_area: false
-        path_interpolation_ds: 0.2 # [m]
-
+        path_interpolation_ds: 0.1 # [m]
       stuck_vehicle:
-        use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck
+        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
@@ -25,24 +24,29 @@
         state_transit_margin_time: 1.0
         min_predicted_path_confidence: 0.05
         minimum_ego_predicted_velocity: 1.388 # [m/s]
-        collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
-        collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
+        normal:
+          collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
+          collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
+        relaxed:
+          collision_start_margin_time: 2.0
+          collision_end_margin_time: 0.0
         keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr
 
       occlusion:
         enable: false
         occlusion_attention_area_length: 70.0 # [m]
         enable_creeping: false # flag to use the creep velocity when reaching occlusion limit stop line
         occlusion_creep_velocity: 0.8333 # the creep velocity to occlusion limit stop line
-        peeking_offset: -0.1 # [m] offset for peeking into detection area
+        peeking_offset: -0.5 # [m] offset for peeking into detection area
         free_space_max: 43
         occupied_min: 58
         do_dp: true
-        before_creep_stop_time: 1.0 # [s]
+        before_creep_stop_time: 0.1 # [s]
         min_vehicle_brake_for_rss: -2.5 # [m/s^2]
         max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph
         denoise_kernel: 1.0 # [m]
-        pub_debug_grid: false
+        possible_object_bbox: [1.0, 2.0] # [m x m]
+        ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h
 
     merge_from_private:
       stop_line_margin: 3.0

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -72,16 +72,20 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.stuck_vehicle.timeout_private_area =
     node.declare_parameter<double>(ns + ".stuck_vehicle.timeout_private_area");
 
-  ip.collision_detection.state_transit_margin_time =
-    node.declare_parameter<double>(ns + ".collision_detection.state_transit_margin_time");
   ip.collision_detection.min_predicted_path_confidence =
     node.declare_parameter<double>(ns + ".collision_detection.min_predicted_path_confidence");
   ip.collision_detection.minimum_ego_predicted_velocity =
     node.declare_parameter<double>(ns + ".collision_detection.minimum_ego_predicted_velocity");
-  ip.collision_detection.collision_start_margin_time =
-    node.declare_parameter<double>(ns + ".collision_detection.collision_start_margin_time");
-  ip.collision_detection.collision_end_margin_time =
-    node.declare_parameter<double>(ns + ".collision_detection.collision_end_margin_time");
+  ip.collision_detection.state_transit_margin_time =
+    node.declare_parameter<double>(ns + ".collision_detection.state_transit_margin_time");
+  ip.collision_detection.normal.collision_start_margin_time =
+    node.declare_parameter<double>(ns + ".collision_detection.normal.collision_start_margin_time");
+  ip.collision_detection.normal.collision_end_margin_time =
+    node.declare_parameter<double>(ns + ".collision_detection.normal.collision_end_margin_time");
+  ip.collision_detection.relaxed.collision_start_margin_time =
+    node.declare_parameter<double>(ns + ".collision_detection.relaxed.collision_start_margin_time");
+  ip.collision_detection.relaxed.collision_end_margin_time =
+    node.declare_parameter<double>(ns + ".collision_detection.relaxed.collision_end_margin_time");
   ip.collision_detection.keep_detection_vel_thr =
     node.declare_parameter<double>(ns + ".collision_detection.keep_detection_vel_thr");
 
@@ -102,7 +106,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.occlusion.max_vehicle_velocity_for_rss =
     node.declare_parameter<double>(ns + ".occlusion.max_vehicle_velocity_for_rss");
   ip.occlusion.denoise_kernel = node.declare_parameter<double>(ns + ".occlusion.denoise_kernel");
-  ip.occlusion.pub_debug_grid = node.declare_parameter<bool>(ns + ".occlusion.pub_debug_grid");
+  ip.occlusion.possible_object_bbox =
+    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");
 }
 
 void IntersectionModuleManager::launchNewModules(