Holonomic robot: oscillation when strafing

[corot]

Looks like when we strafe we don't update the beginning of the trajectory, and so the look-ahead poses gets behind of the robot:

https://user-images.githubusercontent.com/322610/157814372-a72d3f1d-70ca-41e3-9316-3f0a491d49ab.mp4

When moving fw/bw the beginning of the trajectory always starts on current robot pose, so there's no problem:

https://user-images.githubusercontent.com/322610/157814078-4f7e7ac2-846e-4c98-8f43-0236b354ad14.mp4

A workaround that works for us is to set force_reinit_new_goal_dist to 0.0, so TEB is continuously recomputing the trajectory. Another, possibly better, is to reduce dt_hysteresis, cause the problem seems to be that we don't drop poses fast enough here

[corot]

After some research, I narrowed down the problem, but still don't have a solution. For some unknown reason, the dt of the TEB poses don't get updated when strafing, I guess because the optimization only considers velocity along x axis. I printed the total time for the trajectory and it remains constant! You can see on the video how the poses height (as a proxy of estimated reaching time) remains static when strafing, but not when moving straight or even when we have a small vx component:

https://user-images.githubusercontent.com/322610/158321796-1cfd072c-4d2d-4056-b130-5e57ed6fb273.mp4

Reducing dt_hysteresis avoids the issue because we keep dropping the traversed poses even without updating the the dt.

[corot]

@doisyg, remember, I invite u to yakiniku!

[corot]

I noticed that setting weight_max_vel_y to 0 also avoid the problem, but no idea why @RainerKuemmerle, does this tip gives you any clue about where the problem is? Clearly on EdgeVelocityHolonomic, but I cannot see anything wrong there (except maybe the minor detail addressed on #401, but that doesn't fix this issue)