Middle of dev progress commit
Converted from controller_msgs to control_msgs
Merged master branch