Add support for standard ROS PoseStamped message