Fixed pruning when rotation in place

[Eman7C7]

Currently the pruning of the TEB only considers the position when looking for the closest pose. This creates a problem in case the TEB contains several poses in the same position but with different orientation (i.e., robot rotating on the spot). This PR fixes this problem by checking the orientation in case the closest position is the first one.

[amakarow]

Hey, thank you for your contribution. It really makes sense to include the orientation into the pruning scheme. Just thinking about the following scenario: You have a noisy measurement with large orientation error and your closed-loop sampling time is much smaller than dt of the TEB, furthermore we are not rotating in place. Then position distance checker would return index = 0 and the orientation checker might give us a future pose. So we would accidentally remove the first portion of the TEB. It is probably better to combine both metrics into a single distance metric. But it might be non-trivial to choose a proper weight to trade off between position and orientation errors. Another approach could be to check for negligible linear velocity before checking the orientation error. What do you think?

[Eman7C7]

I don't quite get your comment: if the pose estimate is wrong (I assume that's what you mean with "noisy measurement") there is not much we can do anyway... (We cannot guarantee the linear pruning to work correctly either in case of wrong pose estimate.)
What am I missing?

[amakarow]

I mean, assume dt_ref = 0.3 but the actual closed-loop sampling time is 0.05, then you would likely get index == 0 for small speeds because the L2 norm could be minimal here.

If you now have a noisy orientation error, it could happen that the search for the angular difference returns index >> 0.

This seems to be more crucial than for position errors. But maybe this works also for the orientation as we are doing a local rather than a global search.

Have you tested this on a real robot?

[RainerKuemmerle]

The code runs on multiple instances of a differential drive robot and also on omnidirectional robots.
I think the main problem with the pruning is that if nothing is pruned, the robot will stop. This is what we observed when the TEB contained rotation on spot. If you prune a bit too much, the subsequent optimizations will take care.