Fix frame ids and publish a PoseStamped