Refactor code to conform to ros_control interface

[hc825b]

ROS and Gazebo already follow the ros_control to define interfaces (ROS topics) for both simulated and real hardware. Our current framework however use hard-coded ROS topic names for communication between middle-ware and hardware specific controllers.

http://gazebosim.org/tutorials/?tut=ros_control
http://wiki.ros.org/ros_control

A major drawback/difficulty may be that low level controllers have to be re-implemented with the ros_control interfaces in C++ instead of Python.