Removed duplicate functionality poseToMsg function