Updated to work with Gazebo 1.9