From 89b7b9c0362f49058f1ce9cf50000c04391bb0ad Mon Sep 17 00:00:00 2001
From: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Date: Mon, 4 Mar 2024 19:05:50 +0900
Subject: [PATCH] reset ctrl_cmd_prev during manual mode

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 control/mpc_lateral_controller/src/mpc_lateral_controller.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp
index 7eca1481ba921..445ecdb34d623 100644
--- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp
+++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp
@@ -243,7 +243,9 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   Trajectory predicted_traj;
   Float32MultiArrayStamped debug_values;
 
-  if (!m_is_ctrl_cmd_prev_initialized) {
+  if (
+    !m_is_ctrl_cmd_prev_initialized ||
+    !input_data.current_operation_mode.is_autoware_control_enabled) {
     m_ctrl_cmd_prev = getInitialControlCommand();
     m_is_ctrl_cmd_prev_initialized = true;
   }