docs(trajectory_follower, trajectory_follower_nodes): update docs

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

control/trajectory_follower_nodes/design/lateral_controller-design.md → control/trajectory_follower/design/mpc_lateral_controller-design.md

@@ -1,4 +1,4 @@
-# Lateral Controller
+# MPC Lateral Controller
 
 This is the design document for the lateral controller node
 in the `trajectory_follower_nodes` package.
@@ -22,29 +22,64 @@ The node uses an implementation of linear model predictive control (MPC) for acc
 The MPC uses a model of the vehicle to simulate the trajectory resulting from the control command.
 The optimization of the control command is formulated as a Quadratic Program (QP).
 
-These functionalities are implemented in the `trajectory_follower` package
-(see [trajectory_follower-mpc-design](../../trajectory_follower/design/trajectory_follower-mpc-design.md#mpc-trajectory-follower))
+Different vehicle models are implemented:
 
-### Assumptions / Known limits
+- kinematics : bicycle kinematics model with steering 1st-order delay.
+- kinematics_no_delay : bicycle kinematics model without steering delay.
+- dynamics : bicycle dynamics model considering slip angle.
+  The kinematics model is being used by default. Please see the reference [1] for more details.
+
+For the optimization, a Quadratic Programming (QP) solver is used with two options are currently implemented:
+
+- unconstraint_fast : use least square method to solve unconstraint QP with eigen.
+- [osqp](https://osqp.org/): run the [following ADMM](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) algorithm (for more details see the related papers at the [Citing OSQP](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) section):
+
+### Filtering
+
+Filtering is required for good noise reduction.
+A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is used for the yaw and lateral errors used as input of the MPC as well as for
+the output steering angle.
+Other filtering methods can be considered as long as the noise reduction performances are good
+enough.
+The moving average filter for example is not suited and can yield worse results than without any
+filtering.
+
+## Assumptions / Known limits
 
 <!-- Required -->
 
 The tracking is not accurate if the first point of the reference trajectory is at or in front of the current ego pose.
 
 - Issue to add points behind ego: <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1273>
 
-### Inputs / Outputs / API
+## Inputs / Outputs / API
 
 <!-- Required -->
 <!-- Things to consider:
     - How do you use the package / API? -->
 
-Inputs
+### Inputs
+
+Set the following from the [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md)
+
+- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow.
+- `nav_msgs/Odometry`: current odometry
+- `autoware_auto_vehicle_msgs/SteeringReport` current steering
 
-- `input/reference_trajectory` : reference trajectory to follow.
-- `input/current_kinematic_state`: current state of the vehicle (position, velocity, etc).
-  Output
-- `output/lateral_control_cmd`: generated lateral control command.
+### Outputs
+
+Return LateralOutput which contains the following to [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md)
+
+- `autoware_auto_control_msgs/AckermannLateralCommand`
+- LateralSyncData
+  - steer angle convergence
+
+### MPC class
+
+The `MPC` class (defined in `mpc.hpp`) provides the interface with the MPC algorithm.
+Once a vehicle model, a QP solver, and the reference trajectory to follow have been set
+(using `setVehicleModel()`, `setQPSolver()`, `setReferenceTrajectory()`), a lateral control command
+can be calculated by providing the current steer, velocity, and pose to function `calculateMPC()`.
 
 ### Parameter description
 
@@ -133,8 +168,16 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 - `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for stability.
 - `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` for stability.
 
+## References / External links
+
+<!-- Optional -->
+
+- [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking",
+  Robotics Institute, Carnegie Mellon University, February 2009.
+
 ## Related issues
 
 <!-- Required -->
 
 - <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057>
+- <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058>

control/trajectory_follower_nodes/design/longitudinal_controller-design.md → control/trajectory_follower/design/pid_longitudinal_controller-design.md

@@ -1,4 +1,4 @@
-# Longitudinal Controller
+# PID Longitudinal Controller
 
 ## Purpose / Use cases
 
@@ -9,56 +9,7 @@ It is assumed that the target acceleration calculated here will be properly real
 
 Note that the use of this module is not mandatory for Autoware if the vehicle supports the "target speed" interface.
 
-## Separated lateral (steering) and longitudinal (velocity) controls
-
-This longitudinal controller assumes that the roles of lateral and longitudinal control are separated as follows.
-
-- Lateral control computes a target steering to keep the vehicle on the trajectory, assuming perfect velocity tracking.
-- Longitudinal control computes a target velocity/acceleration to keep the vehicle velocity on the trajectory speed, assuming perfect trajectory tracking.
-
-Ideally, dealing with the lateral and longitudinal control as a single mixed problem can achieve high performance. In contrast, there are two reasons to provide velocity controller as a stand-alone function, described below.
-
-### Complex requirements for longitudinal motion
-
-The longitudinal vehicle behavior that humans expect is difficult to express in a single logic. For example, the expected behavior just before stopping differs depending on whether the ego-position is ahead/behind of the stop line, or whether the current speed is higher/lower than the target speed to achieve a human-like movement.
-
-In addition, some vehicles have difficulty measuring the ego-speed at extremely low speeds. In such cases, a configuration that can improve the functionality of the longitudinal control without affecting the lateral control is important.
-
-There are many characteristics and needs that are unique to longitudinal control. Designing them separately from the lateral control keeps the modules less coupled and improves maintainability.
-
-### Nonlinear coupling of lateral and longitudinal motion
-
-The lat-lon mixed control problem is very complex and uses nonlinear optimization to achieve high performance. Since it is difficult to guarantee the convergence of the nonlinear optimization, a simple control logic is also necessary for development.
-
-Also, the benefits of simultaneous longitudinal and lateral control are small if the vehicle doesn't move at high speed.
-
-## Design
-
-## Assumptions / Known limits
-
-1. Smoothed target velocity and its acceleration shall be set in the trajectory
-   1. The velocity command is not smoothed inside the controller (only noise may be removed).
-   2. For step-like target signal, tracking is performed as fast as possible.
-2. The vehicle velocity must be an appropriate value
-   1. The ego-velocity must be a signed-value corresponding to the forward/backward direction
-   2. The ego-velocity should be given with appropriate noise processing.
-   3. If there is a large amount of noise in the ego-velocity, the tracking performance will be significantly reduced.
-3. The output of this controller must be achieved by later modules (e.g. vehicle interface).
-   1. If the vehicle interface does not have the target velocity or acceleration interface (e.g., the vehicle only has a gas pedal and brake interface), an appropriate conversion must be done after this controller.
-
-## Inputs / Outputs / API
-
-### Output
-
-- control_cmd [`autoware_auto_msgs/LongitudinalCommand`] : command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration.
-- debug_values [`autoware_auto_msgs/Float32MultiArrayDiagnostic`] : debug values used for the control command generation (e.g. the contributions of each P-I-D terms).
-
-### Input
-
-- current_state [`autoware_auto_msgs/VehicleKinematicState`] : Current ego state including the current pose and velocity.
-- current_trajectory [`autoware_auto_msgs/Trajectory`] : Current target trajectory for the desired velocity on the each trajectory points.
-
-## Inner-workings / Algorithms
+## Design / Inner-workings / Algorithms
 
 ### States
 
@@ -141,7 +92,59 @@ Depending on the actuating principle of the vehicle, the mechanism that physical
 
 In this controller, the predicted ego-velocity and the target velocity after the delay time are calculated and used for the feedback to address the time delay problem.
 
-### Parameter description
+### Slope compensation
+
+Based on the slope information, a compensation term is added to the target acceleration.
+
+There are two sources of the slope information, which can be switched by a parameter.
+
+- Pitch of the estimated ego-pose (default)
+  - Calculates the current slope from the pitch angle of the estimated ego-pose
+  - Pros: Easily available
+  - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration.
+- Z coordinate on the trajectory
+  - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory
+  - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained
+  - Pros: Can be used in combination with delay compensation (not yet implemented)
+  - Cons: z-coordinates of high-precision map is needed.
+  - Cons: Does not support free space planning (for now)
+
+## Assumptions / Known limits
+
+1. Smoothed target velocity and its acceleration shall be set in the trajectory
+   1. The velocity command is not smoothed inside the controller (only noise may be removed).
+   2. For step-like target signal, tracking is performed as fast as possible.
+2. The vehicle velocity must be an appropriate value
+   1. The ego-velocity must be a signed-value corresponding to the forward/backward direction
+   2. The ego-velocity should be given with appropriate noise processing.
+   3. If there is a large amount of noise in the ego-velocity, the tracking performance will be significantly reduced.
+3. The output of this controller must be achieved by later modules (e.g. vehicle interface).
+   1. If the vehicle interface does not have the target velocity or acceleration interface (e.g., the vehicle only has a gas pedal and brake interface), an appropriate conversion must be done after this controller.
+
+## Inputs / Outputs / API
+
+### Input
+
+Set the following from the [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md)
+
+- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow.
+- `nav_msgs/Odometry`: current odometry
+
+### Output
+
+Return LongitudinalOutput which contains the following to [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md)
+
+- `autoware_auto_control_msgs/LongitudinalCommand`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration.
+- LongitudinalSyncData
+  - velocity convergence(currently not used)
+
+### PIDController class
+
+The `PIDController` class is straightforward to use.
+First, gains and limits must be set (using `setGains()` and `setLimits()`) for the proportional (P), integral (I), and derivative (D) components.
+Then, the velocity can be calculated by providing the current error and time step duration to the `calculate()` function.
+
+## Parameter description
 
 The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the
 AutonomouStuff Lexus RX 450h for under 40 km/h driving.
@@ -177,7 +180,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 | emergency_state_traj_trans_dev      | double | If the ego's position is `emergency_state_traj_tran_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m]                      | 3.0           |
 | emergency_state_traj_rot_dev        | double | If the ego's orientation is `emergency_state_traj_rot_dev` rad away from the nearest trajectory point orientation, the state will transit to EMERGENCY. [rad]        | 0.784         |
 
-#### DRIVE Parameter
+### DRIVE Parameter
 
 | Name                                  | Type   | Description                                                                                                                                                        | Default value |
 | :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
@@ -196,7 +199,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 | current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s]                                      | 0.5           |
 | brake_keeping_acc                     | double | If `enable_brake_keeping_before_stop` is true, a certain acceleration is kept during DRIVE state before the ego stops [m/s^2] See [Brake keeping](#brake-keeping). | 0.2           |
 
-#### STOPPING Parameter (smooth stop)
+### STOPPING Parameter (smooth stop)
 
 Smooth stop is enabled if `enable_smooth_stop` is true.
 In smooth stop, strong acceleration (`strong_acc`) will be output first to decrease the ego velocity.
@@ -218,15 +221,15 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig
 | smooth_stop_weak_stop_dist   | double | Weak acceleration will be output when the ego is `smooth_stop_weak_stop_dist`-meter before the stop point. [m]       | -0.3          |
 | smooth_stop_strong_stop_dist | double | Strong acceleration will be output when the ego is `smooth_stop_strong_stop_dist`-meter over the stop point. [m]     | -0.5          |
 
-#### STOPPED Parameter
+### STOPPED Parameter
 
 | Name         | Type   | Description                                  | Default value |
 | :----------- | :----- | :------------------------------------------- | :------------ |
 | stopped_vel  | double | target velocity in STOPPED state [m/s]       | 0.0           |
 | stopped_acc  | double | target acceleration in STOPPED state [m/s^2] | -3.4          |
 | stopped_jerk | double | target jerk in STOPPED state [m/s^3]         | -5.0          |
 
-#### EMERGENCY Parameter
+### EMERGENCY Parameter
 
 | Name           | Type   | Description                                       | Default value |
 | :------------- | :----- | :------------------------------------------------ | :------------ |

control/trajectory_follower/design/trajectory_follower-design.md

@@ -16,6 +16,40 @@ Mainly, it implements two algorithms:
 - PID control for the computation of velocity and acceleration commands.
   - [trajectory_follower-pid-design](trajectory_follower-pid-design.md)
 
+## 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.
+
+- `setInputData()`: Input the data subscribed in [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md). This must be implemented with the inherited algorithm and the used data must be selected.
+- `run()`: Compute control commands and return to [controller_node](../../trajectory_follower_nodes/design/). This must be implemented by inherited algorithms.
+- `syncData()`: Input the result of running the other controller.
+
+## Separated lateral (steering) and longitudinal (velocity) controls
+
+This longitudinal controller assumes that the roles of lateral and longitudinal control are separated as follows.
+
+- Lateral control computes a target steering to keep the vehicle on the trajectory, assuming perfect velocity tracking.
+- Longitudinal control computes a target velocity/acceleration to keep the vehicle velocity on the trajectory speed, assuming perfect trajectory tracking.
+
+Ideally, dealing with the lateral and longitudinal control as a single mixed problem can achieve high performance. In contrast, there are two reasons to provide velocity controller as a stand-alone function, described below.
+
+### Complex requirements for longitudinal motion
+
+The longitudinal vehicle behavior that humans expect is difficult to express in a single logic. For example, the expected behavior just before stopping differs depending on whether the ego-position is ahead/behind of the stop line, or whether the current speed is higher/lower than the target speed to achieve a human-like movement.
+
+In addition, some vehicles have difficulty measuring the ego-speed at extremely low speeds. In such cases, a configuration that can improve the functionality of the longitudinal control without affecting the lateral control is important.
+
+There are many characteristics and needs that are unique to longitudinal control. Designing them separately from the lateral control keeps the modules less coupled and improves maintainability.
+
+### Nonlinear coupling of lateral and longitudinal motion
+
+The lat-lon mixed control problem is very complex and uses nonlinear optimization to achieve high performance. Since it is difficult to guarantee the convergence of the nonlinear optimization, a simple control logic is also necessary for development.
+
+Also, the benefits of simultaneous longitudinal and lateral control are small if the vehicle doesn't move at high speed.
+
 ## Related issues
 
 <!-- Required -->