diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 445ecdb34d623..882150ffc1644 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -243,9 +243,11 @@ trajectory_follower::LateralOutput MpcLateralController::run( Trajectory predicted_traj; Float32MultiArrayStamped debug_values; - if ( - !m_is_ctrl_cmd_prev_initialized || - !input_data.current_operation_mode.is_autoware_control_enabled) { + 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; }