Implement testing

[rafaelrojasmiliani]

1. Unit test of interoperability functions.
   • basis_msg_to_basis and basis_to_basis_msg can be inverted (applying one after the other gives the same)
   • gspline_msg_to_gspline and gspline_to_msg can be inverted (applying one after the other gives the same)
   • gspline_to_joint_gspline_msg and gspline_to_joint_trajectory_msg can be inverted
   • function_to_joint_trajectory_msg, gspline_msg_to_joint_trajectory_msg, joint_gspline_msg_to_joint_trajectory_msg, function_to_follow_joint_trajectory_goal, gspline_msg_to_follow_joint_trajectory_goal, joint_gspline_msg_to_follow_joint_trajectory_goal` test that the values coincide with the function position, derivative, acceleration
   • follow_joint_trajectory_feedback_to_follow_joint_gspline_feedback, follow_joint_gspline_goal_to_follow_joint_trajectory_goal check that can be inverted
   • follow_joint_gspline_result_to_follow_joint_trajectory_result follow_joint_trajectory_result_to_follow_joint_gspline_result can be inverted
   • waypoint_matrix, interval_length_vector give correct result
   • interpolate_joint_trajectory
   • minimum_sobolev_semi_norm, minimum_sobolev_semi_norm_joint_trajectory, minimum_jerk_trajectory
2. Integration tests
   • Basis message
   • GSpline message
   • Control messages and actions