传感器节点及地平线数据算法节点

IMU节点

为了稳定120hz，头盔的imu决定只读取4元数来获取姿态

gps节点

gps频率不会太快，所以设置了只有10hz

# 发布者：满足 NavSatFix 标准格式
self.nav_pub = self.create_publisher(NavSatFix, 'gnss/fix', 10)
# 发布地速（m/s）
self.vel_pub = self.create_publisher(TwistStamped, 'gnss/vel', 10)

读取了经纬度、高度、地度等二进制的数据并转换,

msg = type('UBXPOS', (), {})()
msg.latitude = lat / 1e7
msg.longitude = lon / 1e7
msg.altitude = height / 1000.0  # mm to m
msg.fixType = fixType
# 提取速度信息：velN, velE, velD, gSpeed（单位：mm/s），转换为 m/s
try:
velN, velE, velD, gSpeed = struct.unpack_from('<iiii', payload, 48)
msg.velN = velN / 1000.0
msg.velE = velE / 1000.0
msg.velD = velD / 1000.0
msg.gSpeed = gSpeed / 1000.0

气压计节点

建立节点时除了点问题，之前能读取到，运行节点时报错，还需要再改一下

算法节点

暂时之加入了地平线和俯梯度的相关数据，有俯仰角，横滚角和偏航角 先是订阅imu节点的数据

self.sub = self.create_subscription(Imu, 'exo/imu/data', self.imu_callback, 10)

之后把4元数通过公式换算

sinr_cosp = 2.0 * (w * x + y * z)
    cosr_cosp = 1.0 - 2.0 * (x * x + y * y)
    roll = math.atan2(sinr_cosp, cosr_cosp)

    sinp = 2.0 * (w * y - z * x)
    if abs(sinp) >= 1:
        pitch = math.copysign(math.pi / 2.0, sinp)
    else:
        pitch = math.asin(sinp)

    siny_cosp = 2.0 * (w * z + x * y)
    cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
    yaw = math.atan2(siny_cosp, cosy_cosp)

---

本文章使用limfx的vscode插件快速发布