feat(obstacle_cruise_planner): improve stop and cruise behavior for c…

…ut-in & out

Signed-off-by: Berkay Karaman <[email protected]>

autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml

@@ -21,23 +21,34 @@
 
       nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
-      min_behavior_stop_margin: 3.0 # [m]
+      min_behavior_stop_margin: 6.0 # [m]
       stop_on_curve:
         enable_approaching: false
         additional_safe_distance_margin: 3.0 # [m]
         min_safe_distance_margin: 3.0 # [m]
       suppress_sudden_obstacle_stop: false
 
       stop_obstacle_type:
-        unknown: true
-        car: true
-        truck: true
-        bus: true
-        trailer: true
-        motorcycle: true
-        bicycle: true
-        pedestrian: true
         pointcloud: false
+        inside:
+          unknown: true
+          car: true
+          truck: true
+          bus: true
+          trailer: true
+          motorcycle: true
+          bicycle: true
+          pedestrian: true
+
+        outside:
+          unknown: false
+          car: true
+          truck: true
+          bus: true
+          trailer: true
+          motorcycle: true
+          bicycle: true
+          pedestrian: true
 
       cruise_obstacle_type:
         inside:
@@ -57,7 +68,7 @@
           bus: true
           trailer: true
           motorcycle: true
-          bicycle: false
+          bicycle: true
           pedestrian: false
 
       slow_down_obstacle_type:
@@ -88,26 +99,31 @@
       stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle
 
       # hysteresis for cruise and stop
-      obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
-      obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
+      obstacle_velocity_threshold_from_cruise_to_stop : 1.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
+      obstacle_velocity_threshold_from_stop_to_cruise : 1.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
 
       # if crossing vehicle is determined as target obstacles or not
       crossing_obstacle:
-        obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s]
-        obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop
+        obstacle_velocity_threshold : 1.0 # velocity threshold for crossing obstacle for cruise or stop [m/s]
+        obstacle_traj_angle_threshold : 0.523599 # [rad] = 30 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop
 
       stop:
         max_lat_margin: 0.3 # lateral margin between the obstacles and ego's footprint
         max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint
+        outside_obstacle:
+          max_lateral_time_margin: 4.0 # time threshold of lateral margin between the obstacles and ego's footprint [s]
+          num_of_predicted_paths: 3 # number of the highest confidence predicted path to check collision between obstacle and ego
         crossing_obstacle:
-          collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]
+          collision_time_margin : 2.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]
 
       cruise:
         max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width
         outside_obstacle:
-          obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
-          ego_obstacle_overlap_time_threshold : 2.0 #  time threshold to decide cut-in obstacle for cruise or stop [s]
-          max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego
+          obstacle_velocity_threshold : 1.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
+          ego_obstacle_overlap_time_threshold : 0.2 #  time threshold to decide cut-in obstacle for cruise or stop [s]
+          max_prediction_time_for_collision_check : 10.0 # prediction time to check collision between obstacle and ego
+          max_lateral_time_margin: 5.0 # time threshold of lateral margin between obstacle and trajectory band with ego's width [s]
+          num_of_predicted_paths: 3 # number of the highest confidence predicted path to check collision between obstacle and ego
         yield:
           enable_yield: true
           lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding