Replaced own hackwork with build in gazebo functionality