fix doc

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

control/mpc_lateral_controller/CMakeLists.txt

@@ -33,4 +33,5 @@ if(BUILD_TESTING)
 endif()
 
 ament_auto_package(INSTALL_TO_SHARE
+  param
 )

control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml

@@ -0,0 +1,73 @@
+/**:
+  ros__parameters:
+    # -- system --
+    traj_resample_dist: 0.1 # path resampling interval [m]
+    use_steer_prediction: false # flag for using steer prediction (do not use steer measurement)
+    admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value
+    admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value
+
+    # -- path smoothing --
+    enable_path_smoothing: false # flag for path smoothing
+    path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing
+    curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num)
+    curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num)
+
+    # -- trajectory extending --
+    extend_trajectory_for_end_yaw_control: true  # flag of trajectory extending for terminal yaw control
+
+    # -- mpc optimization --
+    qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp)
+    mpc_prediction_horizon: 50 # prediction horizon step
+    mpc_prediction_dt: 0.1 # prediction horizon period [s]
+    mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q
+    mpc_weight_heading_error: 0.0 # heading error weight in matrix Q
+    mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q
+    mpc_weight_steering_input: 1.0 # steering error weight in matrix R
+    mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R
+    mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R
+    mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R
+    mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R
+    mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point
+    mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point
+    mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point
+    mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point
+    mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point
+    mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point
+    mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point
+    mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point
+    mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03)
+    mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability
+    mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability
+    mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero
+    mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration
+    mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing
+    mpc_min_prediction_length: 5.0 # minimum prediction length
+
+    # -- vehicle model --
+    vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics
+    input_delay: 0.24 # steering input delay time for delay compensation
+    vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s]
+    steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s]
+    acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss]
+    velocity_time_constant: 0.3 # velocity dynamics time constant  for trajectory velocity modification [s]
+
+    # -- lowpass filter for noise reduction --
+    steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz]
+    error_deriv_lpf_cutoff_hz: 5.0
+
+    # stop state: steering command is kept in the previous value in the stop state.
+    stop_state_entry_ego_speed: 0.001
+    stop_state_entry_target_speed: 0.001
+    converged_steer_rad: 0.1
+    keep_steer_control_until_converged: true
+    new_traj_duration_time: 1.0
+    new_traj_end_dist: 0.3
+
+    # vehicle parameters
+    mass_kg: 2400.0
+    mass_fl: 600.0
+    mass_fr: 600.0
+    mass_rl: 600.0
+    mass_rr: 600.0
+    cf: 155494.663
+    cr: 155494.663

control/pid_longitudinal_controller/CMakeLists.txt

@@ -24,5 +24,5 @@ if(BUILD_TESTING)
 endif()
 
 ament_auto_package(INSTALL_TO_SHARE
-  # config
-  )
+  param
+ )

control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml

@@ -0,0 +1,74 @@
+/**:
+  ros__parameters:
+    delay_compensation_time: 0.17
+
+    enable_smooth_stop: true
+    enable_overshoot_emergency: true
+    enable_large_tracking_error_emergency: true
+    enable_slope_compensation: false
+    enable_keep_stopped_until_steer_convergence: true
+
+    # state transition
+    drive_state_stop_dist: 0.5
+    drive_state_offset_stop_dist: 1.0
+    stopping_state_stop_dist: 0.49
+    stopped_state_entry_duration_time: 0.1
+    stopped_state_entry_vel: 0.1
+    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.7
+
+    # drive state
+    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
+    lpf_vel_error_gain: 0.9
+    current_vel_threshold_pid_integration: 0.5
+    enable_brake_keeping_before_stop: false
+    brake_keeping_acc: -0.2
+
+    # smooth stop state
+    smooth_stop_max_strong_acc: -0.5
+    smooth_stop_min_strong_acc: -1.0
+    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
+
+    # stopped state
+    stopped_vel: 0.0
+    stopped_acc: -3.4
+    stopped_jerk: -5.0
+
+    # emergency state
+    emergency_vel: 0.0
+    emergency_acc: -5.0
+    emergency_jerk: -3.0
+
+    # acceleration limit
+    max_acc: 3.0
+    min_acc: -5.0
+
+    # jerk limit
+    max_jerk: 2.0
+    min_jerk: -5.0
+
+    # pitch
+    use_trajectory_for_pitch_calculation: false
+    lpf_pitch_gain: 0.95
+    max_pitch_rad: 0.1
+    min_pitch_rad: -0.1

control/trajectory_follower/CMakeLists.txt

@@ -4,12 +4,6 @@ project(trajectory_follower)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-#ament_auto_add_library(${PROJECT_NAME} SHARED
-#  include/trajectory_follower/longitudinal_controller_base.hpp
-#  include/trajectory_follower/lateral_controller_base.hpp)
-#
-#ament_auto_package()
-
 add_library(${PROJECT_NAME} INTERFACE)
 target_include_directories(${PROJECT_NAME} INTERFACE
   "$<BUILD_SHARED:${CMAKE_CURRENT_SOURCE_DIR}/include>"

control/trajectory_follower/README.md

@@ -9,23 +9,21 @@ This is the design document for the `trajectory_follower` package.
     - Why did we implement this feature? -->
 
 This package provides the interface of longitudinal and lateral controllers used by the node of the `trajectory_follower_nodes` package.
-We can implement a detailed controller by deriving `LongitudinalControllerBase` and `LateralControllerBase`.
+We can implement a detailed controller by deriving the longitudinal and lateral base interfaces.
 
 ## Design
 
-![Controller](../../trajectory_follower_nodes/design/media/Controller.drawio.svg)
-
 There are lateral and longitudinal base interface classes and each algorithm inherits from this class to implement.
 The interface class has the following base functions.
 
 - `isReady()`: Check if the control is ready to compute.
-- `run()`: Compute control commands and return to [Trajectory Follower Nodes](../../trajectory_follower_nodes/README.md). This must be implemented by inherited algorithms.
+- `run()`: Compute control commands and return to [Trajectory Follower Nodes](../trajectory_follower_nodes/README.md). This must be implemented by inherited algorithms.
 - `sync()`: Input the result of running the other controller.
   - steer angle convergence
     - allow keeping stopped until steer is converged.
   - velocity convergence(currently not used)
 
-See [the Design of Trajectory Follower Nodes](../../trajectory_follower_nodes/README.md#Design) for how these functions work in the node.
+See [the Design of Trajectory Follower Nodes](../trajectory_follower_nodes/README.md#Design) for how these functions work in the node.
 
 ## Separated lateral (steering) and longitudinal (velocity) controls
 

control/trajectory_follower_nodes/README.md

@@ -8,12 +8,13 @@ Generate control commands to follow a given Trajectory.
 
 This is a node of the functionalities implemented in the controller class derived from [trajectory_follower](../../trajectory_follower/README.md#trajectory-follower) package. It has instances of those functionalities, gives them input data to perform calculations, and publishes control commands.
 
-By default, the controller instance from the `Controller` class as follows is used.
+By default, the controller instance with the `Controller` class as follows is used.
 
+```plantuml
 @startuml
-package trajectory*follower {
+package trajectory_follower {
 abstract class LateralControllerBase {
-longitudinal_sync_data*
+longitudinal_sync_data_
 
     virtual isReady(InputData)
     virtual run(InputData)
@@ -22,7 +23,7 @@ longitudinal_sync_data*
 
 }
 abstract class LongitudinalControllerBase {
-lateral*sync_data*
+lateral_sync_data_
 
     virtual isReady(InputData)
     virtual run(InputData)
@@ -63,10 +64,10 @@ run(InputData) override
 }
 }
 
-package trajectory*follower_nodes {
+package trajectory_follower_nodes {
 class Controller {
-longitudinal_controller*
-lateral*controller*
+longitudinal_controller_
+lateral_controller_
 onTimer()
 createInputData(): InputData
 }
@@ -76,21 +77,27 @@ MPCLateralController --|> LateralControllerBase
 PurePursuitLateralController --|> LateralControllerBase
 PIDLongitudinalController --|> LongitudinalControllerBase
 
-LateralSyncData --_LateralControllerBase
-LongitudinalSyncData --_ LongitudinalControllerBase
+LateralSyncData --> LongitudinalControllerBase
+LateralSyncData --> LateralControllerBase
+LongitudinalSyncData --> LongitudinalControllerBase
+LongitudinalSyncData --> LateralControllerBase
 InputData ..> LateralControllerBase
 InputData ..> LongitudinalControllerBase
 
 LateralControllerBase --o Controller
 LongitudinalControllerBase --o Controller
 InputData ..> Controller
 @enduml
+```
 
-The process flow is as follows.
+The process flow of `Controller` class is as follows.
 
 ```cpp
 // 1. create input data
 const auto input_data = createInputData(*get_clock());
+if (!input_data) {
+  return;
+}
 
 // 2. check if controllers are ready
 const bool is_lat_ready = lateral_controller_->isReady(*input_data);