http://mbed.org/users/garyservin/code/ros_lib_indigo/#fd24f7ca9688