Note that the unit test also passes without
thechange. But the VelocitySmoothing's
local_time would get NAN. This would leads to
wrong trajectory calculations.
During a round corner the L1 distance calculation
was only done in 2D and the z-axis coordinate
was directly coming from the next waypoint.
This lead to an unpredictable altitude profile
between two waypoints. Sometimes almost all
altitude difference was already covered during
the turn instead of going diagonally.
Extract the functionality to plan smooth position motion trajectories into a
motion planning library, such that it can be used in other parts of the code as well.
Matthias Grob