Port over tests for joint_trajectory_controller from ROS1

[bmagyar]

Scope is up to you, a 500 line PR is a good chunk to review, feel free to open a better-scope follow-up ticket for what's left

https://github.com/ros-controls/ros_controllers/tree/melodic-devel/joint_trajectory_controller/test

[v-lopez]

Gonna be updating missing tests here.

init_joint_trajectory_test missing tests

• InitLogicCombine Combination not implemented yet.
• InitValuesCombine Combination not implemented yet.
• WrappingSpec Wrapping not implemented yet.
• IgnoreWrappingSpec Wrapping not implemented yet.
• GoalHandleTest Goal Handle update not implemented yet
• OtherTimeBase Othe time base not implemented yet

hold_trajectory_builder_test missing tests

All tests missing. HoldTrajectoryBuilder not implemented yet.

joint_partial_trajectory_controller_test missing tests

• invalidMessages
• topicReplacesTopicTraj actionReplacesActionTraj actionReplacesTopicTraj topicReplacesActionTraj Will only do one test for this
• ignoreOldTopicTraj ignoreOldActionTraj
• ignorePartiallyOldActionTraj
• executeParitalActionTrajInFuture Test implemented but commented, current_trajectory not implemented yet

joint_trajectory_controller_test missing tests

• stateTopicConsistency
• queryStateServiceConsistency
• invalidMessages
• topicSingleTraj
• actionSingleTraj
• jointReordering
• jointWraparound Wrapping not implemented yet
• jointWraparoundPiSingularity Wrapping not implemented yet
• topicReplacesTopicTraj actionReplacesActionTraj actionReplacesTopicTraj topicReplacesActionTraj Tested with partial joints
• emptyTopicCancelsTopicTraj emptyTopicCancelsActionTraj emptyActionCancelsTopicTraj
• emptyTopicCancelsActionTrajWithDelay emptyTopicCancelsActionTrajWithDelayStopZero delay funcitonality not implemented yet in test Hardware, not sure if needed.
• cancelActionGoal
• ignoreOldTopicTraj
• ignoreOldActionTraj
• ignoreSingleOldActionTraj
• ignorePartiallyOldActionTraj
• jointVelocityFeedForward
• pathToleranceViolation
• goalToleranceViolation

[v-lopez]

#75 Contains the invalidMessages and topicReplaces tests

Waiting on #68 to go through

[v-lopez]

@bmagyar I have a question regarding: https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp#L542

If I understand correctly that test, it does the following. Given 3 joints.

Send trajectory with several points: [(0, 0, 0), (1, 2, 3), (2, 4, 6)]
Wait until first waypoint is done, robot joints are at (1, 2, 3)

Send a partial trajectory only for the first joint [(-1), (-2)]. Where only the second point is in the future.

Expects that when the second trajectory time has passed, the joints are at: (-2, 4, 6). Since the first joint follows the partial trajectory, and the other joints follow the original trajectory.

Is this behavior correct? Shouldn't the second trajectory fully stop the execution of the first one, and then move only the partial joints, so the final state is (-2, 2, 3)?

I don't think neither of these behaviors are currently implemented, but I'd like to get a better understanding before implementing it.