cap_to_obs_tf.py 文件源码

def rtk_orientation_to_numpy(msg):
    if isinstance(msg, Odometry):
        p = msg.pose.pose.orientation
        return np.array([p.x, p.y, p.z, p.w])
    elif isinstance(msg, PoseStamped) or isinstance(msg, NavSatFix):
        p = msg.pose.orientation
        return np.array([p.x, p.y, p.z, p.w])
    else:
        raise ValueError