Don't assume all joints have associated tolerances, and deal with
potentially out-of-order tolerance specifications (compared to order of
joints MotoROS2 uses internally).

Easier to test and potentially to reuse.

Not just when there are no JointTolerance instances.

Note: they are appended to the main ActionServer_FJT source file as the function(s) tested are local to that compilation unit (ie: static).

They're not needed outside this compilation unit.

Need space for at least the number of joint positions in the trajectory point.

Instead of the maximum possible number always.

Work-around doesn't seem needed anymore.

A debug log warning only right now. We could consider making it a fatal error and actually return it.

This is a temporary implementation, as it would be better to check the goal_tolerance field can be parsed when validating a new goal.

Co-authored-by: Ted Miller <[email protected]>

Co-authored-by: Ted Miller <[email protected]>