Add guard to dt negative case and break the controller for safety

[alcantara09]

Hi,

This PR adds a guard to the case when the time step on the diff_drive_controller is computed as negative due to some error coming from the hardware interface, as in #561

At my company we are experiencing the same backward movement behavior but less consistent and less predicable. This safe guards the controller to this type of error, what can be common given we are also experiencing the same issue.

[matthew-reynolds]

We should guard on period being negative, which corresponds to cmd_dt rather than dt, Could you also duplicate this over to ackermann_steering_controller and four_wheel_steering_controller? Those controllers all use the period in their SpeedLimiters, which is where this particularly nasty behaviour is coming from.

The other controllers only use the period for updating PIDs, which will still have some odd effects but shouldn't be nearly as big of a deal.

I will add, though, that if you're seeing this issue then the real solution is to fix your ControllerManager::Update() to use a monotonic time source. Adding warnings here is a bit of a fall back and diagnostic, it shouldn't be relied on to provide good behaviour. Passing in a negative period isn't something the framework is designed to handle, since it makes no sense.

[alcantara09]

We should guard on period being negative, which corresponds to cmd_dt rather than dt, Could you also duplicate this over to ackermann_steering_controller and four_wheel_steering_controller? Those controllers all use the period in their SpeedLimiters, which is where this particularly nasty behaviour is coming from.

The other controllers only use the period for updating PIDs, which will still have some odd effects but shouldn't be nearly as big of a deal.

Ok, I will do the following changes, no problem.

I will add, though, that if you're seeing this issue then the real solution is to fix your ControllerManager::Update() to use a monotonic time source. Adding warnings here is a bit of a fall back and diagnostic, it shouldn't be relied on to provide good behaviour. Passing in a negative period isn't something the framework is designed to handle, since it makes no sense.

You are absolutely correct, we are debugging our update to correct the problem. But given we were not the only ones to hit this issue, I thought it would be useful to have the guard

[alcantara09]

Hi,

I did the asked changes.

However I have one question still. I was getting a lot of negative values for dt. What is the meaning of this? should we add a std::abs to check the timeout with a positive value?

[alcantara09]

Hi,

I was just testing the entire chain and I just realized that setting curr_cmd to zero doesn't guard against the bug because it will be still inserted on the limiter.

I see two options then:

1. We can set curr_cmd to zero and skip the limiter step
2. We can set curr_cmd to zero and set cmd_dt to a positive value (The question then would be which value)

An alternative would be to throw an exception

What is your opinion on that?

(The unit tests are running normally locally, I do not understand why it failed on the pipeline)

[matthew-reynolds]

I think we probably want to just set the cmd_dt to 0. That should force the acceleration and jerk to be 0 - In other words, maintain the same vel as the prev update. Not perfect, but I think that's the safest solution.

Skipping the limiter step opens up a hole for a dangerous command to slip through, which I don't think is acceptable.

[alcantara09]

Fair enough, I did the changes

[matthew-reynolds]

Since we don't have tests to verify this behaviour, please test the changes locally to ensure the PR has the desired effect. Your last commit doesn't appear to satisfy the intended behaviour.

Please also retarget this PR to the noetic-devel branch. It can then be backported from there to Melodic.

[bmagyar]

Any update on this @alcantara09 ? I'd like to merge a working solution if you have one. The current errors printed are not bad to start but if you could set the values to 0.0 on top of reporting the error that'd make things a little safer.

[alcantara09]

Hi @bmagyar and @matthew-reynolds,

I am forcing cmd_dt to zero when we have a negative period.
This forces all jerk and acceleration to be zero and then we are forwarding last cmd as current cmd, as it was agreed as expected behavior. Only on the jerk calculation, v = dv0 + da, that the contributtion of the previous velocities still can alter the current cmd. What I do not believe should be set to zero.

I believe the code is in conformity with the issues raised by you, otherwise could you elaborate a little more the aspect that I am still missing so I can change?