From 89b7b9c0362f49058f1ce9cf50000c04391bb0ad Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Mon, 4 Mar 2024 19:05:50 +0900 Subject: [PATCH 1/2] reset ctrl_cmd_prev during manual mode Signed-off-by: kyoichi-sugahara --- 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; } From 0516f5acf4233697dddc372708ce986606b8f949 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 5 Mar 2024 14:37:47 +0900 Subject: [PATCH 2/2] fix manual driving condition Signed-off-by: kyoichi-sugahara --- .../mpc_lateral_controller/src/mpc_lateral_controller.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) 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; }