Action send_goal: add velocity parameter handling

[c00kiepreferences]

What does this PR do?

Draft PR adding handling of the velocity parameter of ros2 action send_goal. Due to a big amount of ifology present in handling edge-cases of manipulation, this PR does not work properly for now: that's why i changed its status to draft.

How it worked previously

In previous implementation, after receiving a goal, joints trajectory component would order each joint and articulation to go straight to the goal position, without taking velocity parameter into consideration at all. This would result in joints and articulations speed depending only on force limit, damping, stiffness vaules etc, without any option to adjust the speed by user.

How it works with this PR

In my proposed implementation, joints trajectory component splits the path of the joint into smaller parts, ordering joints to go to the next checkpoint each tick. This way, it is guaranteed for the joints to move with the requested velocity at max. Keep in mind that actual velocity of the joint will not always reflect requested one: joints have their inertia and need to gain speed, they can't exceed their max velocity, etc.

Edge case handling

Changing the implementation requires caution when handling some edge cases, especially when send_goal action is used in a non-standard way:

• Ordering joint to move out of bounds: both articulation and joint movement planning takes limits into account and the planned path will never exceed them, with one exception listed below.
• Limited joints with limits 180°, -180°: when this type of constreint is met, for articulations, it is assumed that the only restriction in its movement is making a full circe by going straight from 180° to -180°. As for the joints, the matter gets more complicated since option of checking their motion type is not provided: joints with such limits will behave as if they are not limited at all.
• Goal positions exceeding [-180°, 180°]: joint ordered to move to a position outside of those bounds will make one full circle at max. Ordering a joint to go to position 4$\cdot\pi$ while it is postioned at 0° will not result in it making 2 full circles.

Changes in usage implied by this PR

Adding velocity parameter handling is useful, but keep in mind that with great power comes great responsibility. As velocity can be both positive and negative, before ordering a joint to move to position x, you'll have to check if its current position is greater or lower and set the velocity to positive or negative integer accordingly. Otherwise, the joint will move in the opposite direction, till it reaches the goal from the other side, reaches its limit or meets the time limit of execution. Keep in mind that this is not a change that serves just for simplifying that implementation of this PR: using negative velocities is crucial for MoveIt to work.

How was this PR tested?

This PR does not work yet and a deeper dive into the edge-case handling is necessary. I recommend caution when merging, even after correcting and testing this PR as it severely changes the usage of the send_goal action and any small mistake could cause joints and articulations to behave in the least expected ways.