Updated to work with gazebo