Fix for the s-shaped 3d trajectory

[haider8645]

Text from original issue in gitlab:

_

The 3-D curves returned by Planner::plan may return very tiny S-curves. This happens (as far as we remember) when the z-change between two steps is much (no idea how much) bigger than the xy-change. Those small s-curves lead to stuttering when executed by the trajectory follower (i.e. it drives forward, stops, drives backward, stops, drives forward again (to follow the S).
A fix exists but that fix just sets z=0 for all trajectory steps. This fix works because the trajectory follower (at the time of writing) ignores the z-axsis anyway.
This bug needs to be implemented and fixed :-)

_
As IS:

To Be:

Solution:

The spline points are offset by the posWorld and start then projected on the actual plane of the trav patch.

[haider8645]

On inclines the projection along the plane normal results in a small offset to the goalPose. Green points results in red points are projection. This needs to be checked next:

[haider8645]

Intersecting a line through the trajectory point with the trav node plane solves the position offset issue