Skip to content

Commit 8f49eac

Browse files
committed
use the flag input_data.current_operation_mode.is_autoware_control_enabled
Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>
1 parent c90d02d commit 8f49eac

File tree

1 file changed

+1
-5
lines changed

1 file changed

+1
-5
lines changed

control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp

+1-5
Original file line numberDiff line numberDiff line change
@@ -252,10 +252,6 @@ trajectory_follower::LateralOutput MpcLateralController::run(
252252
m_is_ctrl_cmd_prev_initialized = true;
253253
}
254254

255-
const bool is_driving_manually = input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::LOCAL ||
256-
input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::REMOTE ||
257-
input_data.current_operation_mode.mode == autoware_adapi_v1_msgs::msg::OperationModeState::STOP;
258-
259255
const bool is_mpc_solved = m_mpc->calculateMPC(
260256
m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values);
261257

@@ -264,7 +260,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
264260
// the vehicle will return to the path by re-planning the trajectory or external operation.
265261
// After the recovery, the previous value of the optimization may deviate greatly from
266262
// the actual steer angle, and it may make the optimization result unstable.
267-
if (!is_mpc_solved||is_driving_manually) {
263+
if (!is_mpc_solved||!input_data.current_operation_mode.is_autoware_control_enabled) {
268264
m_mpc->resetPrevResult(m_current_steering);
269265
} else {
270266
setSteeringToHistory(ctrl_cmd);

0 commit comments

Comments
 (0)