fix(trajectory_follower_nodes): mpc_follower does not send proper converged data under low steering rate limit

[brkay54]

Signed-off-by: Berkay Karaman [email protected]

Description

Closes: #1543

In current Autoware.universe, mpc_follower sends is_steer_converged data "true" to longitudinal controller without reaching the desired steering angle value.

The problem is that when reinitialize the vehicle under low steering angle rate limit, solver can not solve the problem and sends the previous command to vehicle rather than the target value.
Like before reinitialize previous cmd = 30.0 degree,
After we reinitialize vehicle, previous_cmd still 30.0 degree but target_value is maybe 0.0 degree ideally but because steering_angle_rate_limit so small, mpc can not solve the problem and sends 30.0 degree again and vehicle starts moving (because before re initialization, current steering was near to 30.0 degree.)

To solve this, I set mpc's steering_rate_limit large as possible if the vehicle in stop state (otherwise it will be what we declared), so It send the (expected) target steering without steering_rate_limit constraint.

Also I changed the logic of the isSteerConverged(). Like @kosuke55 said here, mpc_follower will wait for the (prev_cmd == current_cmd) to be sure output will be the desired angle. Because mpc sends prev_cmd if mpc can not solve the problem, we are also checking is_mpc_calculated.

Related links

Tests performed

Notes for reviewers

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[brkay54]

@kosuke55 Sorry to bother you, would you try this? Do you think this implementation can solve our problem?

[kosuke55]

It's better to select to whether to control steer when stopping.
m_keep_steer_control_until_converged is the option, and we would like to keep it.

[kosuke55]

It's ok for draft, but we want change to use

enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY };

and move it to longitudinal_contrller_base
https://github.com/autowarefoundation/autoware.universe/blob/main/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp#L101-L111

[kosuke55]

@brkay54
thanks very much for your PR!

Also I changed the logic of the isSteerConverged(). Like @kosuke55 said #1543 (comment), mpc_follower will wait for the (prev_cmd == current_cmd) to be sure output will be the desired angle. Because mpc sends prev_cmd if mpc can not solve the problem, we are also checking is_mpc_calculated.

In this part, I meant like

  const double steer_cmd_diff = std::abs(cmd.steering_tire_angle - prev_cmd.steering_tire_angle);
  const doulbe steer_cmd_rate = steer_cmd_diff / m_mpc.m_ctrl_period;
  const bool is_cmd_changing = steer_cmd_rate > m_steer_rate_lim_rps / 2;  // need to consider which thresh is reasonable

I thought it would be good to check the value of is_cmd_changing in isSteerConverged().

The problem is that when reinitialize the vehicle under low steering angle rate limit, solver can not solve the problem and sends the previous command to vehicle rather than the target value.

Did you check the value of is_mpc_solved and did you mean that it sends previous commands when it is false?
I think that before using is_mpc_solved isStoppedState() is checked with previous_cmd and current_steer_angle and if stopped it sends previous commands. So is_mpc_solved does not seem related to convergence status.

[kosuke55]

@brkay54
And sorry to bother you, some PRs related to mpc will be merged so this PR will be conflicted. After they are merged could you please resolve the conflict and clean up(keep only necessary diff) your PR?:pray:

• feat(trajectory_follower): pub steer converged marker #2441
• refactor(trajectory_follower_nodes): refactor the controller framework #2432

[brkay54]

@kosuke55

Did you check the value of is_mpc_solved and did you mean that it sends previous commands when it is false?

Yes, if is_mpc_solved = false, controller calls getStopControlCommand() function which returns previous control command. If it returns the prev_cmd, isCmdChanging() function returns false and vehicle may start to move.

I think that before using is_mpc_solved isStoppedState() is checked with previous_cmd and current_steer_angle and if stopped it sends previous commands. So is_mpc_solved does not seem related to convergence status.

I changed the structure, it does not send prev_cmd anymore when in stopped state. (Because we do not want to prev_cmd in stopped state, we need desired steering cmd.) If the vehicle in stopped state, it just resets the input buffer with prev_cmd, does not send any control command. After it resets the input buffer, it creates new control command. (Btw I changed the order in run() function, please also consider this.)

[brkay54]

@kosuke55
FYI: I am gonna fix the CI&CD check problems, if this approach is fine for you.

Also this PR includes some structural changes of mpc_follower.
@maxime-clem @TakaHoribe @takayuki5168 you may want to take a look.

[kosuke55]

@brkay54
Sorry for the delay. I discussed with @TakaHoribe.
First of all, we think it is not a good idea for longitudinal and lateral to share the same stop state because it would create too many dependencies.
Also, if the essential problem is that mpc can not solve, we want to fix that. If it is still unsolvable, we eliminate the constraint condition. Can you please explain again how to reproduce when is_mpc_solved is false, or can we get a rosbag?

[brkay54]

@kosuke55
The problem is not whether mpc can solve or not. The problem is that controller does not wait until the mpc create proper control command. Let me explain:

For example;
steering_angle_rate_lim: 0.0872665 rad/sec (5.0 degree/sec)

Before re-initialize the vehicle:
steering_sent: 0.20 rad
actual_steering: 0.19 rad

When re-initialize the vehicle in straight road ->
our desired steering angle should be 0.0 .
actual_steering: 0.19 rad

if mpc can not solve -> lateral controller will create the previous control command which is 0.20. (it is near to actual steering so isSteerConverged() function would return true and vehicle starts to move.)

if mpc can solve -> Because the steering_angle_rate_lim = 0.0872665 rad/sec (5.0 degree/sec), lateral_controller will create control command which is near to previous steering command such as 0.195 rad. (Again it is near to actual steering so isSteerConverged() function would return true and vehicle starts to move.)

My proposal is:

• if longitudinal_controller is in stopped state make the steering_angle_rate constraint large to get proper steering angle. if it is in driving state make it default value.
• check the differences between the steering values which are output when mpc is solved. If difference < threshold and steering_cmd - actual_steering < converged_steer_rad, make isSteeringConverged = true.

@brkay54 Sorry for the delay. I discussed with @TakaHoribe. First of all, we think it is not a good idea for longitudinal and lateral to share the same stop state because it would create too many dependencies.

What do you mean when you said "it would create too many dependencies.".

With my proposal, we can still make clear the buffers with same Stopped state. But we need to use the stopped state of longitudinal controller to make the steering_angle_rate constraint large because we need to be sure to vehicle wont move.

Sorry for that, I do not have any rosbag, I am testing it in planning_simulator. You can reproduce the problem with setting the this param to lower value (I set it 5.0 degree/sec).

[kosuke55]

@brkay54 cc @TakaHoribe @mitsudome-r
Sorry for the delay and thank you for your detailed explanation.
Basically, I agreed with your proposals.

・ if longitudinal_controller is in stopped state make the steering_angle_rate constraint large to get proper steering angle. if it is in driving state make it default value.

This seems good as a workaround. I think it would be better to have a bool parameter(like no_stopped_steering_angle_rate_lim, disable_steer_rate_limit_when_stopped etc..)(by default false) and set the steering_angle_rate_lim to max only when it is enabled.
And it would also be better to set it just before using it in
https://github.com/autowarefoundation/autoware.universe/blob/main/control/mpc_lateral_controller/src/mpc.cpp/#L686-L691

const double steer_rate_lim = no_stopped_steering_angle_rate_lim && is_stopped ? numerical_limit<double>::max() : m_steer_rate_lim;

(And since the root cause seems that mpc can not solve under the constraint, so we will attempt to investigate further and fix in another PR.)

・ check the differences between the steering values which are output when mpc is solved. If difference < threshold and steering_cmd - actual_steering < converged_steer_rad, make isSteeringConverged = true.

This also seems good. And in case the difference is very small, it would be better to look at it for a few seconds for better performance. For example, instead of making a judgment based on a change of 0.20 -> 0.195 in one step, it is better to use 1-second difference and determine that it changes when the value changes 0.2 -> 0.1.

What do you mean when you said "it would create too many dependencies.".

Sorry for my lack of explanation.
One of autoware's policies is the modularity, and each module needs to work independently as possible. We would like to minimize the number of longitudinal controller information which is used in the lateral controller as much as possible. We want to determine the stopped state only in the lateral controller.

And isStoppedState() of mpc is originally the flag about whether to keep steer control, not about velocity state.
If the reason why you need to use stopped state of longitudinal controller in lateral controller is to prevent large difference commands with no constraints from being sent while driving because the lateral stop state which checks the steer angle does not match the longitudinal one, then how about check only velocity in mpc? As I mentioned above, how about const bool is_stopped = std::abs(v) < eps in mpc.cpp?

[mitsudome-r]

We had another call on this PR. In conclusion:

• We will allow larger steering rate limit when the vehicle is at stop (so that we have less case that mpc fails to solve)
• We create new issue to solve the issue with MPC not being able to solve with a given constraint as a long term solution
• We improve isSteerConverged() function to look longer period of time to detect if steering is properly converged or not.

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.

[mehmetdogru]

@brkay54 will update soon.

[brkay54]

@kosuke55 sorry for being late, I updated the PR, please check it.
Also, created another PR which is related is_steering_converged info: #3644

[codecov]

Codecov Report

Patch coverage: 36.36% and no project coverage change.

Comparison is base (9693e36) 13.82% compared to head (c2f53ea) 13.82%.

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #2454   +/-   ##
=======================================
  Coverage   13.82%   13.82%           
=======================================
  Files        1395     1395           
  Lines       98036    98064   +28     
  Branches    29147    29163   +16     
=======================================
+ Hits        13553    13562    +9     
- Misses      69826    69838   +12     
- Partials    14657    14664    +7     

Flag	Coverage Δ		*Carryforward flag
differential	46.45% <36.36%> (?)
total	13.82% <ø> (-0.01%)	⬇️	Carriedforward from 9693e36

*This pull request uses carry forward flags. Click here to find out more.

Impacted Files	Coverage Δ
..._lateral_controller/src/mpc_lateral_controller.cpp	34.74% <36.36%> (-0.25%)	⬇️

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

[mehmetdogru]

@kosuke55 friendly ping to review.

[kosuke55]

sorry to be late.

I checked psim and tier4 internal scenario test (1326/1372->1329/1372), and it worked fine.

[kosuke55]

LGTM

[kosuke55]

I send launcher PR autowarefoundation/autoware_launch#378
We need merge them at the same time.

[kosuke55]

@TakaHoribe
The code and behavior look good to me, if it is ok for you please approve this as a code owner.

[TakaHoribe]

LGTM. @brkay54 Thank you!