Add joint trajectory command, fixed #537

- add trajectory_msgs/JointTrajectory (/command) topic interface
- add test code to check /command topic interface