From 37574af18b8e6048b74caf46dd2f1096f28dbd06 Mon Sep 17 00:00:00 2001
From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Date: Wed, 27 Oct 2021 18:27:10 +0900
Subject: [PATCH] Master sync for velocity controller (#297)

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

* update velocity_controller_param.yaml (#247)

Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../velocity_controller.param.yaml            | 95 ++++++++++---------
 1 file changed, 49 insertions(+), 46 deletions(-)

diff --git a/control_launch/config/velocity_controller/velocity_controller.param.yaml b/control_launch/config/velocity_controller/velocity_controller.param.yaml
index 66888225035d3..f28e5271dca2a 100644
--- a/control_launch/config/velocity_controller/velocity_controller.param.yaml
+++ b/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