Fixed publishing of poses