Angle calculation of closestPointOnSegment() should be changed

[cesar-lopez-mar]

Currently, closestPointOnSegment uses the angle of the start pose (or the one defined from start to end) to the returned Pose

A better idea is to interpolate the angle from start to end (when those angles are given). This will avoid "step" discontinuities of the returned Pose angle between segments, which I saw recently in one of our projects.

[rokusottervanger]

If this angle is used in feedforward action calculation, that would mean the robot starts steering before it reaches the pose that prescribes a rotation. Hence, it will "cut the corner". Is this desired behavior?

[cesar-lopez-mar]

Instead of "cutting corners", we avoid overshooting. In practice the robot will not move in straight lines connecting poses, it will do in curves, so it will never follow corners. The idea with the FF is to start steering such that by the time the robot reaches the next pose, it already has the right orientation.

[cesar-lopez-mar]

I gave it another thought and actually after the angle interpolation the FF action will be the same and also with discontinuities. This because the way it is computed, using a Pure-pursuit strategy (See https://www.hindawi.com/journals/ijvt/2016/2306818/ section 2.2). Along that segment the steering to reach the next pose will remain the same after interpolation.

Still the interpolation will have a positive impact by avoiding discontinuities in the calculated carrot (the PGP) when using track_base_link. So this issue remain valid, and perhaps we can open another one for having smoother FF.

[rokusottervanger]

Interesting... You're right. To get smoother FF, you'd need to use more information. You would also need to use points/poses further ahead. Is that an issue in any running project(s)?

About smoothing the PGP: maybe we should start by writing a test. This way, we can first see what the current behavior is, and it forces us to think about appropriate requirements before we start to add such an improvement feature.

[cesar-lopez-mar]

I did notice some "step-like" behavior in the steering of one of our vehicles, in the mean time I solved it by generating a path with higher resolution, but perhaps the path source is not always to modify so an interpolation feature might be useful.
In any case, yes let's start with a test. We should take this to an internal discussion.