Fix gazebo odom callback