revert obstacle_cruise_planner

Signed-off-by: Takayuki Murooka <[email protected]>

planning/autoware_obstacle_cruise_planner/CMakeLists.txt

@@ -7,7 +7,13 @@ autoware_package()
 find_package(Eigen3 REQUIRED)
 
 ament_auto_add_library(autoware_obstacle_cruise_planner_core SHARED
-  DIRECTORY src
+  src/node.cpp
+  src/utils.cpp
+  src/polygon_utils.cpp
+  src/optimization_based_planner/velocity_optimizer.cpp
+  src/optimization_based_planner/optimization_based_planner.cpp
+  src/pid_based_planner/pid_based_planner.cpp
+  src/planner_interface.cpp
 )
 
 rclcpp_components_register_node(autoware_obstacle_cruise_planner_core

planning/autoware_obstacle_cruise_planner/README.md

@@ -68,71 +68,6 @@ struct Obstacle
 };
 ```
 
-```plantuml
-@startuml
-skinparam monochrome true
-
-title Overall flowchart
-start
-
-partition filter_stop_obstacle_for_predicted_object {
-:is_stop_obstacle_type;
-
-partition filter_inside_stop_obstacle_for_predicted_object {
-:filter by lateral distance;
-
-:filter by label;
-
-:is_stop_obstacle_velocity;
-
-:check if the obstacle really collides with the trajectory;
-
-:is_crossing_transient_obstacle;
-}
-
-partition filter_outside_stop_obstacle_for_predicted_object {
-}
-
-:checkConsistency;
-note right
-consistent
-end note
-}
-
-partition filter_stop_obstacle_for_point_cloud {
-}
-
-:concat stop obstacles;
-note right
-concat stop obstacles by predicted objects and point cloud
-end note
-
-partition generate_stop_trajectory {
-:calculate dist to collide;
-
-:calc_desired_stop_margin;
-note right
-- whether the ego is cloes to the goal
-- whether the ego is on the curve
-- whether there is stop point by behavior's stop point
-end note
-
-:calc_candidate_zero_vel_dist;
-
-:hold_previous_stop_if_necessary;
-
-:insert_stop_point;
-
-:set_stop_planning_debug_info;
-}
-
-:publishDebugInfo;
-
-stop
-
-@enduml
-```
-
 ### Behavior determination against obstacles
 
 Obstacles for cruising, stopping and slowing down are selected in this order based on their pose and velocity.

planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -1,10 +1,86 @@
 /**:
   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 : 5.0 # This is also used as a stop margin [m]
+      terminal_safe_distance_margin : 3.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]
+      slow_down_min_acc: -1.0         # slow down min deceleration [m/ss]
+      slow_down_min_jerk: -1.0        # slow down min jerk [m/sss]
 
       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]
+      stop_on_curve:
+        enable_approaching: false # false
+        additional_safe_distance_margin: 3.0 # [m]
+        min_safe_distance_margin: 3.0 # [m]
+      suppress_sudden_obstacle_stop: false
+
+      stop_obstacle_type:
+        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:
+          unknown: true
+          car: true
+          truck: true
+          bus: true
+          trailer: true
+          motorcycle: true
+          bicycle: true
+          pedestrian: false
+
+        outside:
+          unknown: false
+          car: true
+          truck: true
+          bus: true
+          trailer: true
+          motorcycle: true
+          bicycle: false
+          pedestrian: false
+
+      slow_down_obstacle_type:
+        unknown: false
+        car: true
+        truck: true
+        bus: true
+        trailer: true
+        motorcycle: true
+        bicycle: true
+        pedestrian: true
+        pointcloud: false
 
     behavior_determination:
       pointcloud_search_radius: 5.0
@@ -20,9 +96,154 @@
       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 : 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]
+
+      # 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.3 # lateral margin between the obstacles except for unknown and ego's footprint
+        max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint
+        min_velocity_to_reach_collision_point: 2.0 # minimum velocity margin to calculate time to reach collision [m/s]
+        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
+          pedestrian_deceleration_rate: 0.5 # planner perceives pedestrians will stop with this rate to avoid unnecessary stops [m/ss]
+          bicycle_deceleration_rate: 0.5 # planner perceives bicycles will stop with this rate to avoid unnecessary stops [m/ss]
+        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.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
+          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
+          max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it
+          max_obstacles_collision_time: 10.0 # how far the blocking obstacle
+          stopped_obstacle_velocity_threshold: 0.5
+      slow_down:
+        max_lat_margin: 1.1  # lateral margin between obstacle and trajectory band with ego's width
+        lat_hysteresis_margin: 0.2
+
+        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"
+      default:
+        moving:
+          min_lat_margin: 0.2
+          max_lat_margin: 1.0
+          min_ego_velocity: 2.0
+          max_ego_velocity: 8.0
+        static:
+          min_lat_margin: 0.2
+          max_lat_margin: 1.0
+          min_ego_velocity: 4.0
+          max_ego_velocity: 8.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
+    stop:
+      type_specified_params:
+        labels: # For the listed types, the node try to read the following type specified values
+          - "default"
+          - "unknown"
+        # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined.
+          # limit_min_acc: common_param.yaml/limit.min_acc
+        unknown:
+          limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred.
+          sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop".
+          sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop".
+          abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit.