fix(mpc_lateral_controller): align the MPC steering angle when the car is controlled manually. (autowarefoundation#7109)

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

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

* update the condition for is_driving_manually

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

* STOP mode included

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

* comment the is_driving_manually

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

* align the steering outside (after) the solver.

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

* use the flag input_data.current_operation_mode.is_autoware_control_enabled

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

* correct a typo

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

* correct the under control condition check

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

* undo the space delete

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

* unchange the unrelevant line

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

* pre-commit

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

---------

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>
Signed-off-by: Simon Eisenmann <simon.eisenmann@driveblocks.ai>

Author: HansOersted

control/autoware_mpc_lateral_controller/src/mpc.cpp

@@ -244,7 +244,7 @@ void MPC::setReferenceTrajectory(
 
 void MPC::resetPrevResult(const SteeringReport & current_steer)
 {
-  // Consider limit. The prev value larger than limitation brakes the optimization constraint and
+  // Consider limit. The prev value larger than limitation breaks the optimization constraint and
   // results in optimization failure.
   const float steer_lim_f = static_cast<float>(m_steer_lim);
   m_raw_steer_cmd_prev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f);

control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -287,7 +287,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   // the vehicle will return to the path by re-planning the trajectory or external operation.
   // After the recovery, the previous value of the optimization may deviate greatly from
   // the actual steer angle, and it may make the optimization result unstable.
-  if (!is_mpc_solved) {
+  if (!is_mpc_solved || !is_under_control) {
     m_mpc->resetPrevResult(m_current_steering);
   } else {
     setSteeringToHistory(ctrl_cmd);