tested working with gazebo and c6