Position and orientation works with a /pose visualization in RVIZ, but the coordinates are not 100% correct yet.