Revert "Fix frame ids and publish a PoseStamped"

This reverts commit f1d4ae341a265409ea01cee4fe782b0a072eb9e9.