align the MPC steering angle when the car is controlled manually.

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

Author: HansOersted

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp

@@ -443,7 +443,7 @@ class MPC
   bool calculateMPC(
     const SteeringReport & current_steer, const Odometry & current_kinematics,
     AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory,
-    Float32MultiArrayStamped & diagnostic);
+    Float32MultiArrayStamped & diagnostic, const bool is_under_control = true);
 
   /**
    * @brief Set the reference trajectory to be followed.

control/mpc_lateral_controller/src/mpc.cpp

@@ -38,7 +38,7 @@ MPC::MPC(rclcpp::Node & node)
 bool MPC::calculateMPC(
   const SteeringReport & current_steer, const Odometry & current_kinematics,
   AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory,
-  Float32MultiArrayStamped & diagnostic)
+  Float32MultiArrayStamped & diagnostic, const bool is_under_control)
 {
   // since the reference trajectory does not take into account the current velocity of the ego
   // vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics.
@@ -103,6 +103,10 @@ bool MPC::calculateMPC(
   // save previous input for the mpc rate limit
   m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev;
   m_raw_steer_cmd_prev = Uex(0);
+  if (!is_under_control) {
+    m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev;
+    m_raw_steer_cmd_prev = mpc_data.steer;
+  }
 
   /* calculate predicted trajectory */
   predicted_trajectory =

control/mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -253,7 +253,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   }
 
   const bool is_mpc_solved = m_mpc->calculateMPC(
-    m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values);
+    m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values, is_under_control);
 
   // reset previous MPC result
   // Note: When a large deviation from the trajectory occurs, the optimization stops and