Merged changes from Atlas ROS plugins, cleaned up headers
Merged changes from Atlas ROS plugins, cleaned up headers