Catkinized
Added message_generation in package.xml and Cmakelist; otherwise it doesn't catkin_make
Example file written in cpp to encode trajectories, some minor bugfixes