fix(mpc_lateral_controller): reset ctrl_cmd_prev during manual mode (#…

…6541) * reset ctrl_cmd_prev during manual mode Signed-off-by: kyoichi-sugahara <[email protected]> * fix manual driving condition Signed-off-by: kyoichi-sugahara <[email protected]> --------- Signed-off-by: kyoichi-sugahara <[email protected]>
autowarefoundation · Mar 5, 2024 · 48d20f1 · 48d20f1
1 parent 7ef61a9
commit 48d20f1
Showing 1 changed file with 5 additions and 1 deletion.
diff --git 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;
   }