From 48d20f1456ef76b49a4c11eeaa58f2419ecfd9cc Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 6 Mar 2024 03:29:43 +0900 Subject: [PATCH] fix(mpc_lateral_controller): reset ctrl_cmd_prev during manual mode (#6541) * reset ctrl_cmd_prev during manual mode Signed-off-by: kyoichi-sugahara * fix manual driving condition Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../mpc_lateral_controller/src/mpc_lateral_controller.cpp | 6 +++++- 1 file changed, 5 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..882150ffc1644 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/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; }