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
评论列表
文章目录