Example / testing code for ros_control