First push of all source code. Has been catkinized, but not re-tested on the robot since being catkinized.