Master sync for velocity controller (autowarefoundation#297)

* update velocity_controller_param.yaml for refactoring velocity_controller (autowarefoundation#238)

* update velocity_controller_param.yaml (autowarefoundation#247)

Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>

control_launch/config/velocity_controller/velocity_controller.param.yaml

@@ -1,36 +1,56 @@
 /**:
   ros__parameters:
-    # closest waypoint threshold
-    closest_waypoint_distance_threshold: 3.0
-    closest_waypoint_angle_threshold: 0.7854
-
-    # stop state
-    stop_state_velocity: 0.0
-    stop_state_acc: -3.4
-    stop_state_entry_ego_speed: 0.2
-    stop_state_entry_target_speed: 0.1
-    stop_state_keep_stopping_dist: 0.5
-
     # delay compensation
     delay_compensation_time: 0.17
 
-    # emergency stop by this controller
-    emergency_stop_acc: -5.0
-    emergency_stop_jerk: -3.0
+    # slope compensation
+    enable_slope_compensation: true
+
+    # state transition
+    drive_state_stop_dist: 0.5
+    stopping_state_stop_dist: 0.5
+    stopped_state_entry_vel: 0.01
+    stopped_state_entry_acc: 0.1
+    emergency_state_overshoot_stop_dist: 1.5
+    emergency_state_traj_trans_dev: 3.0
+    emergency_state_traj_rot_dev: 0.7854
+
+    # drive state
+    kp: 1.0
+    ki: 0.1
+    kd: 0
+    max_out: 1.0
+    min_out: -1.0
+    max_p_effort: 1.0
+    min_p_effort: -1.0
+    max_i_effort: 0.3
+    min_i_effort: -0.3
+    max_d_effort: 0
+    min_d_effort: 0
+    lpf_vel_error_gain: 0.9
+    current_velocity_threshold_pid_integration: 0.5
+
+    # smooth stop state
+    smooth_stop_max_strong_acc: -0.5
+    smooth_stop_min_strong_acc: -0.8
+    smooth_stop_weak_acc: -0.3
+    smooth_stop_weak_stop_acc: -0.8
+    smooth_stop_strong_stop_acc: -3.4
+    smooth_stop_max_fast_vel: 0.5
+    smooth_stop_min_running_vel: 0.01
+    smooth_stop_min_running_acc: 0.01
+    smooth_stop_weak_stop_time: 0.8
+    smooth_stop_weak_stop_dist: -0.3
+    smooth_stop_strong_stop_dist: -0.5
 
-    # smooth stop
-    smooth_stop:
-      stop_dist: 0.5
-      exit_ego_speed: 1.0
-      entry_ego_speed: 0.8
-      exit_target_speed: 1.0
-      entry_target_speed: 0.2
-      weak_brake_time: 1.0
-      weak_brake_acc: -1.0
-      increasing_brake_time: 1.0
-      increasing_brake_gradient: -0.1
-      stop_brake_time: 1.0
-      stop_brake_acc: -3.4
+    # stopped state
+    stopped_vel: 0.0
+    stopped_acc: -3.4
+
+    # emergency state
+    emergency_vel: 0.0
+    emergency_acc: -5.0
+    emergency_jerk: -3.0
 
     # acceleration limit
     max_acc: 3.0
@@ -40,25 +60,8 @@
     max_jerk: 2.0
     min_jerk: -5.0
 
-    # slope compensation
-    enable_slope_compensation: true
+    # pitch
     use_trajectory_for_pitch_calculation: false
+    lpf_pitch_gain: 0.95
     max_pitch_rad: 0.1
     min_pitch_rad: -0.1
-    lpf_pitch_gain: 0.95
-
-    # velocity feedback
-    pid_controller:
-      kp: 1.0
-      ki: 0.1
-      kd: 0.0
-      max_out: 1.0
-      min_out: -1.0
-      max_p_effort: 1.0
-      min_p_effort: -1.0
-      max_i_effort: 0.3
-      min_i_effort: -0.3
-      max_d_effort: 0.0
-      min_d_effort: 0.0
-      current_velocity_threshold_pid_integration: 0.5
-      lpf_velocity_error_gain: 0.9