feat(obstacle_cruise): pseudo occlusion (#628)

Signed-off-by: Yuki Takagi <[email protected]>
Co-authored-by: Makoto Kurihara <[email protected]>

autoware_launch/config/planning/preset/x2_preset.yaml

@@ -94,9 +94,10 @@ launch:
 
   - arg:
       name: motion_stop_planner_type
-      default: obstacle_cruise_planner
+      default: obstacle_cruise_planner_with_pseudo_occlusion
       # option: obstacle_stop_planner
       #         obstacle_cruise_planner
+      #         obstacle_cruise_planner_with_pseudo_occlusion
       #         none
 
   - arg:

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

@@ -107,6 +107,10 @@
       slow_down:
         max_lat_margin: 0.8  # lateral margin between obstacle and trajectory band with ego's width
         lat_hysteresis_margin: 0.2
+        pseudo_occlusion:
+          enable_function: false # If true, the slow down function behave as pseudo_occlusion
+          max_obj_vel: 1.0 # Objects which is slow than this speed are focused by the pseudo_occlusion function
+          focus_intersections: [0] # Specify the polygon's ID of lanelet map. If a object is on this ID's area the function treat the object even if it's right-hand beside object. ID "0" is required to parse as ROS params.
 
         successive_num_to_entry_slow_down_condition: 5
         successive_num_to_exit_slow_down_condition: 5

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

@@ -0,0 +1,215 @@
+/**:
+  ros__parameters:
+    common:
+      planning_algorithm: "pid_base" # currently supported algorithm is "pid_base"
+
+      enable_debug_info: false
+      enable_calculation_time_info: false
+
+      enable_slow_down_planning: true
+
+      # longitudinal info
+      idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]
+      min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
+      min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
+      safe_distance_margin : 6.0 # This is also used as a stop margin [m]
+      terminal_safe_distance_margin : 6.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m]
+      hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s]
+      hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m]
+
+      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: 6.0 # [m]
+      stop_on_curve:
+        enable_approaching: true
+        additional_safe_distance_margin: 2.0 # [m]
+        min_safe_distance_margin: 3.5 # [m]
+      suppress_sudden_obstacle_stop: true
+
+      stop_obstacle_type:
+        unknown: false
+        car: false
+        truck: false
+        bus: false
+        trailer: false
+        motorcycle: false
+        bicycle: false
+        pedestrian: false
+
+      cruise_obstacle_type:
+        inside:
+          unknown: false
+          car: false
+          truck: false
+          bus: false
+          trailer: false
+          motorcycle: false
+          bicycle: false
+          pedestrian: false
+
+        outside:
+          unknown: false
+          car: false
+          truck: false
+          bus: false
+          trailer: false
+          motorcycle: false
+          bicycle: false
+          pedestrian: false
+
+      slow_down_obstacle_type:
+        unknown: false
+        car: true
+        truck: true
+        bus: true
+        trailer: true
+        motorcycle: false
+        bicycle: false
+        pedestrian: false
+
+    behavior_determination:
+      decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking
+
+      prediction_resampling_time_interval: 0.1
+      prediction_resampling_time_horizon: 10.0
+
+      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 : 4.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
+      obstacle_velocity_threshold_from_stop_to_cruise : 4.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
+
+      stop:
+        max_lat_margin: 0.0 # lateral margin between obstacle and trajectory band with ego's width
+        crossing_obstacle:
+          collision_time_margin : 4.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.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
+          ego_obstacle_overlap_time_threshold : 1.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
+
+      slow_down:
+        max_lat_margin: 1.5  # lateral margin between obstacle and trajectory band with ego's width
+        lat_hysteresis_margin: 0.2
+        pseudo_occlusion:
+          enable_function: true # If true, the slow down function behave as pseudo_occlusion
+          max_obj_vel: 1.0 # Objects which is slow than this speed are focused by the pseudo_occlusion function
+          focus_intersections: [0, 452] # Specify the polygon's ID of lanelet map. If a object is on this ID's area the function treat the object even if it's right-hand beside object. ID "0" is required to parse as ROS params.
+
+        successive_num_to_entry_slow_down_condition: 5
+        successive_num_to_exit_slow_down_condition: 5
+
+      # consider the current ego pose (it is not the nearest pose on the reference trajectory)
+      # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence"
+      # The both errors decrease with constant rates against the time.
+      consider_current_pose:
+        enable_to_consider_current_pose: true
+        time_to_convergence: 1.5 #[s]
+
+    cruise:
+      pid_based_planner:
+        use_velocity_limit_based_planner: true
+        error_function_type: quadratic # choose from linear, quadratic
+
+        velocity_limit_based_planner:
+            # PID gains to keep safe distance with the front vehicle
+            kp: 10.0
+            ki: 0.0
+            kd: 2.0
+
+            output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
+            vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
+
+            enable_jerk_limit_to_output_acc: false
+
+            disable_target_acceleration: true
+
+        velocity_insertion_based_planner:
+            kp_acc: 6.0
+            ki_acc: 0.0
+            kd_acc: 2.0
+
+            kp_jerk: 20.0
+            ki_jerk: 0.0
+            kd_jerk: 0.0
+
+            output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
+            output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
+
+            enable_jerk_limit_to_output_acc: true
+
+        min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
+        min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s]
+        time_to_evaluate_rss: 0.0
+
+        lpf_normalized_error_cruise_dist_gain: 0.2
+
+      optimization_based_planner:
+        dense_resampling_time_interval: 0.2
+        sparse_resampling_time_interval: 2.0
+        dense_time_horizon: 5.0
+        max_time_horizon: 25.0
+        velocity_margin: 0.2 #[m/s]
+
+        # Parameters for safe distance
+        t_dangerous: 0.5
+
+        # For initial Motion
+        replan_vel_deviation: 5.0         # velocity deviation to replan initial velocity [m/s]
+        engage_velocity: 0.25              # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
+        engage_acceleration: 0.1           # engage acceleration [m/ss] (use this acceleration when engagement)
+        engage_exit_ratio: 0.5             # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
+        stop_dist_to_prohibit_engage: 0.5  # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]
+
+        # Weights for optimization
+        max_s_weight: 100.0
+        max_v_weight: 1.0
+        over_s_safety_weight:  1000000.0
+        over_s_ideal_weight:  50.0
+        over_v_weight:  500000.0
+        over_a_weight:  5000.0
+        over_j_weight:  10000.0
+
+    slow_down:
+      # parameters to calculate slow down velocity by linear interpolation
+      labels:
+        - "default"
+        - "pedestrian"
+      default:
+        moving:
+          min_lat_margin: 0.7
+          max_lat_margin: 1.5
+          min_ego_velocity: 2.78
+          max_ego_velocity: 9.72
+        static:
+          min_lat_margin: 0.7
+          max_lat_margin: 1.5
+          min_ego_velocity: 2.78
+          max_ego_velocity: 9.72
+      pedestrian:
+        moving:
+          min_lat_margin: 0.5
+          max_lat_margin: 1.0
+          min_ego_velocity: 2.0
+          max_ego_velocity: 4.0
+        static:
+          min_lat_margin: 0.5
+          max_lat_margin: 1.0
+          min_ego_velocity: 2.0
+          max_ego_velocity: 4.0
+      moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving"
+      moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold
+
+      time_margin_on_target_velocity: 1.5 # [s]
+
+      lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity
+      lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path
+      lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start