def unpack_pose_msg(msg, stamped=False):
    """ Get position and orientation from a Pose(Stamped) message. """
    if stamped:
        p = msg.pose.position
        o = msg.pose.orientation
    else:
        p = msg.position
        o = msg.orientation

    return (p.x, p.y, p.z), (o.w, o.x, o.y, o.z)