Added final missing vars to diagnostics
Speed is now -3200 to 3200, not -1 to 1
Switched to using JointTrajectory instead of Float32 for speed

Topic also changed to joint_trajectory within the public namespace