Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(mpc_lateral_controller): align the MPC steering angle when the car is controlled manually. #7109

Merged
merged 11 commits into from
Jun 20, 2024
2 changes: 1 addition & 1 deletion control/autoware_mpc_lateral_controller/src/mpc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -244,7 +244,7 @@ void MPC::setReferenceTrajectory(

void MPC::resetPrevResult(const SteeringReport & current_steer)
{
// Consider limit. The prev value larger than limitation brakes the optimization constraint and
// Consider limit. The prev value larger than limitation breaks the optimization constraint and
// results in optimization failure.
const float steer_lim_f = static_cast<float>(m_steer_lim);
m_raw_steer_cmd_prev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -260,7 +260,7 @@
// the vehicle will return to the path by re-planning the trajectory or external operation.
// After the recovery, the previous value of the optimization may deviate greatly from
// the actual steer angle, and it may make the optimization result unstable.
if (!is_mpc_solved) {
if (!is_mpc_solved||!is_under_control) {

Check warning on line 263 in control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

MpcLateralController::run increases in cyclomatic complexity from 12 to 13, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
m_mpc->resetPrevResult(m_current_steering);
} else {
setSteeringToHistory(ctrl_cmd);
Expand Down
Loading