cap_to_obs_tf.py 文件源码

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