From 7d7b66f00949974235acf44140a10a2dc1134212 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 9 Mar 2024 19:34:01 +0900 Subject: [PATCH] fix(mpc_lateral_controller): precise backward driving Signed-off-by: Takayuki Murooka --- control/mpc_lateral_controller/src/mpc.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 8fa95f4b3b87b..373de2e0bd911 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -355,12 +355,15 @@ std::pair MPC::updateStateForDelayCompensation( MatrixXd Wd(DIM_X, 1); MatrixXd Cd(DIM_Y, DIM_X); + const double sign_vx = m_is_forward_shift ? 1 : -1; + MatrixXd x_curr = x0_orig; double mpc_curr_time = start_time; for (size_t i = 0; i < m_input_buffer.size(); ++i) { double k, v = 0.0; try { - k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time); + // NOTE: When driving backward, the curvature's sign should be reversed. + k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx; v = interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time); } catch (const std::exception & e) { RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what()); @@ -446,6 +449,7 @@ MPCMatrix MPC::generateMPCMatrix( const double ref_vx = reference_trajectory.vx.at(i); const double ref_vx_squared = ref_vx * ref_vx; + // NOTE: When driving backward, the curvature's sign should be reversed. const double ref_k = reference_trajectory.k.at(i) * sign_vx; const double ref_smooth_k = reference_trajectory.smooth_k.at(i) * sign_vx;