Now can output to gazebo, however robot movement is interesting