Publish 6DOF and some useless stuff
Fix in state machine