Tl;dr: Port Joint Trajectory Controller variants from the ROS1: joint_trajectory_controller.cpp

Hey,

So I have been testing this controller with my robot. When I set time_from_start: { sec: 5, nanosec: 0 } }, traversing the trajectory should take 5 seconds, which is true, but the velocity is incorrect. The controller correctly set's the positions, but because the controller does not write new velocity values to the command interface, the traversed trajectory is unsmooth; the joint moves too fast, so the joint movement stops before the new intermediate position is written.

Shouldn't the velocity be set using the command interface or did I misunderstood something?

I believe, that the velocity value should be set around here