From 8c804aa52ce9edd33fbca754eedebc9fc5af4fce 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 3d19104112a71..5f187630e45cd 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -239,7 +239,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; }