WIT智能imu_py，ros2运行串口与维特智能imu通信节点_维特智能imu数据导入python-CSDN博客

本文链接：https://blog.csdn.net/2403_83973108/article/details/145950913

以下是基于第二个代码的串口解码逻辑修改后的第一个ROS2节点代码，实现适配WT901/JY901协议的IMU数据发布：

```python
# -*- coding: utf-8 -*-
"""
修改说明：
1. 采用WT901/JY901协议解析逻辑
2. 支持动态缓冲区管理
3. 添加校验和验证
4. 适配11字节数据包结构
"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
import serial
import struct

class IMUPublisher(Node):

def __init__(self, name):
super().__init__(name)
self.get_logger().info(f"WT901 IMU节点已启动: {name}!")

# 初始化发布者
self.imu_publisher = self.create_publisher(Imu, 'wt901_imu', 10)

# 串口配置
self.ser = serial.Serial(
port='/dev/ttyUSB0',
baudrate=115200,
timeout=0.1
)
self.buffer = bytearray()

# IMU数据缓存
self.accel = [0.0, 0.0, 0.0] # m/s²
self.gyro = [0.0, 0.0, 0.0] # rad/s
self.quat = [1.0, 0.0, 0.0, 0.0]

# 创建定时器
self.create_timer(0.01, self.timer_callback)

def timer_callback(self):
try:
# 处理串口数据
self.process_serial()

# 构建并发布IMU消息
imu_msg = Imu()
imu_msg.header.stamp = self.get_clock().now().to_msg()
imu_msg.header.frame_id = "imu_link"

# 填充传感器数据
imu_msg.linear_acceleration.x = self.accel[0]
imu_msg.linear_acceleration.y = self.accel[1]
imu_msg.linear_acceleration.z = self.accel[2]

imu_msg.angular_velocity.x = self.gyro[0]
imu_msg.angular_velocity.y = self.gyro[1]
imu_msg.angular_velocity.z = self.gyro[2]

imu_msg.orientation.w = self.quat[0]
imu_msg.orientation.x = self.quat[1]
imu_msg.orientation.y = self.quat[2]
imu_msg.orientation.z = self.quat[3]

self.imu_publisher.publish(imu_msg)

except Exception as e:
self.get_logger().error(f"处理异常: {str(e)}")

def process_serial(self):
# 读取新数据
self.buffer += self.ser.read(self.ser.in_waiting)

# 持续处理完整数据包
while len(self.buffer) >= 11:
# 查找包头
start_idx = self.buffer.find(b'\x55')
if start_idx < 0:
self.buffer.clear()
return

# 移除包头前的无效数据
if start_idx > 0:
del self.buffer[:start_idx]
continue

# 检查数据包完整性
if len(self.buffer) < 11:
return

# 提取并验证数据包
packet = self.buffer[:11]
if self.verify_checksum(packet):
self.parse_packet(packet)
del self.buffer[:11]
else:
del self.buffer[:1] # 跳过无效包头

def verify_checksum(self, packet):
"""校验和验证"""
return (sum(packet[:10]) & 0xFF == packet[10]

def parse_packet(self, packet):
"""解析数据包"""
data_type = packet[1]
data = packet[2:10]

try:
if data_type == 0x51: # 加速度
self.parse_accel(data)
elif data_type == 0x52: # 角速度
self.parse_gyro(data)
elif data_type == 0x53: # 欧拉角
self.parse_angles(data)
elif data_type == 0x54: # 磁场
pass # 暂不处理磁场数据
elif data_type == 0x59: # 四元数
self.parse_quaternion(data)
except Exception as e:
self.get_logger().warn(f"解析异常: {str(e)}")

def parse_accel(self, data):
"""解析加速度 (单位转换: g -> m/s²)"""
scale = 16.0 * 9.80665 # 16g量程转换为m/s²
self.accel[0] = self.decode_s16(data[0:2]) / 32768.0 * scale
self.accel[1] = self.decode_s16(data[2:4]) / 32768.0 * scale
self.accel[2] = self.decode_s16(data[4:6]) / 32768.0 * scale

def parse_gyro(self, data):
"""解析角速度 (单位转换: °/s -> rad/s)"""
scale = 2000.0 * 0.0174533 # 2000dps转换为rad/s
self.gyro[0] = self.decode_s16(data[0:2]) / 32768.0 * scale
self.gyro[1] = self.decode_s16(data[2:4]) / 32768.0 * scale
self.gyro[2] = self.decode_s16(data[4:6]) / 32768.0 * scale

def parse_angles(self, data):
"""解析欧拉角 (备用)"""
pass # ROS Imu消息通常使用四元数

def parse_quaternion(self, data):
"""解析四元数"""
self.quat[0] = self.decode_s16(data[6:8]) / 32768.0 # w
self.quat[1] = self.decode_s16(data[0:2]) / 32768.0 # x
self.quat[2] = self.decode_s16(data[2:4]) / 32768.0 # y
self.quat[3] = self.decode_s16(data[4:6]) / 32768.0 # z

@staticmethod
def decode_s16(bytes_data):
"""解码有符号16位整数"""
return struct.unpack('<h', bytes_data)[0]

def main(args=None):
rclpy.init(args=args)
node = IMUPublisher("wt901_imu_node")
try:
rclpy.spin(node)
except KeyboardInterrupt:
node.ser.close()
finally:
rclpy.shutdown()

if __name__ == '__main__':
main()
```

主要改进说明：

1. **协议适配**
- 采用动态缓冲区管理（替代原固定长度读取）
- 实现11字节数据包结构解析
- 添加校验和验证机制
- 支持多数据类型处理（0x51/0x52/0x59等）

2. **单位转换优化**
- 加速度：g → m/s²（乘以9.80665）
- 角速度：°/s → rad/s（乘以π/180）
- 四元数：标准化到[-1, 1]范围

3. **数据流处理**
- 自动处理数据粘包/残包
- 支持连续数据流解析
- 无效数据自动丢弃

4. **代码结构优化**
- 分离数据解析与ROS消息构建
- 采用模块化解析方法
- 增强异常处理机制

使用注意事项：

1. **设备配置**
```python
# 需根据实际设备调整以下参数
serial_port = '/dev/ttyUSB0' # 设备路径
baudrate = 115200 # 波特率
frame_header = b'\x55' # 帧头标识
```

2. **坐标系统一**
- 确保IMU安装方向与ROS坐标系一致
- 默认采用ENU坐标系（x前，y左，z上）

3. **调试方法**
```python
# 在parse_packet方法中添加调试输出
print(f"处理数据包: 类型0x{data_type:02x}, 原始数据: {packet.hex()}")
```

4. **数据验证**
```bash
# 查看数据频率
ros2 topic hz /wt901_imu

# 查看原始数据
ros2 topic echo /wt901_imu
```