fix(mpc_lateral_controller): reset ctrl_cmd_prev during manual mode (#6541)

* reset ctrl_cmd_prev during manual mode

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* fix manual driving condition

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

Author: kyoichi-sugahara

control/mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -243,7 +243,11 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   Trajectory predicted_traj;
   Float32MultiArrayStamped debug_values;
 
-  if (!m_is_ctrl_cmd_prev_initialized) {
+  const bool is_under_control = input_data.current_operation_mode.is_autoware_control_enabled &&
+                                input_data.current_operation_mode.mode ==
+                                  autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS;
+
+  if (!m_is_ctrl_cmd_prev_initialized || !is_under_control) {
     m_ctrl_cmd_prev = getInitialControlCommand();
     m_is_ctrl_cmd_prev_initialized = true;
   }