# imu_node

**Repository Path**: wuyanfei2025/imu_node

## Basic Information

- **Project Name**: imu_node
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2025-06-18
- **Last Updated**: 2025-06-19

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README



imu_node 节点代码：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion, Vector3
import serial
import re
from math import radians

class DMPImuPublisher(Node):
    def __init__(self):
        super().__init__('dmp_imu_publisher')
        self.publisher = self.create_publisher(Imu, '/imu', 10)
        
        # 参数配置
        self.declare_parameter('serial_port', '/dev/ttyUSB0')
        self.declare_parameter('baud_rate', 115200)
        self.declare_parameter('frame_id', 'imu_link')
        
        # 初始化串口
        self.serial_init()
        
        # 数据缓存
        self.current_data = {
            'quat': [1.0, 0.0, 0.0, 0.0],
            'aworld': [0.0, 0.0, 0.0],
            'ypr': [0.0, 0.0, 0.0]
        }
        
        # 定时器（20Hz）
        self.timer = self.create_timer(0.05, self.process_data)

    def serial_init(self):
        """初始化串口连接"""
        port = self.get_parameter('serial_port').value
        baud = self.get_parameter('baud_rate').value
        
        try:
            self.ser = serial.Serial(
                port=port,
                baudrate=baud,
                timeout=0.1  # 短超时避免阻塞
            )
            self.get_logger().info(f"成功连接串口: {port} @ {baud}bps")
            # 清空初始缓冲区
            self.ser.reset_input_buffer()
        except Exception as e:
            self.get_logger().error(f"串口连接失败: {str(e)}")
            raise

    def parse_line(self, line):
        """解析单行数据"""
        line = line.strip()
        if not line:
            return None
            
        # 使用正则表达式匹配数据行
        if match := re.match(r'^quat\s+([-\d.]+)\s+([-\d.]+)\s+([-\d.]+)\s+([-\d.]+)', line):
            return ('quat', list(map(float, match.groups())))
        elif match := re.match(r'^aworld\s+([-\d.]+)\s+([-\d.]+)\s+([-\d.]+)', line):
            return ('aworld', list(map(float, match.groups())))
        elif match := re.match(r'^ypr\s+([-\d.]+)\s+([-\d.]+)\s+([-\d.]+)', line):
            return ('ypr', list(map(float, match.groups())))
        return None

    def read_serial(self):
        """读取并解析串口数据"""
        try:
            while self.ser.in_waiting > 0:
                line = self.ser.readline().decode('utf-8', errors='ignore')
                if parsed := self.parse_line(line):
                    key, values = parsed
                    self.current_data[key] = values
        except Exception as e:
            self.get_logger().error(f"串口读取错误: {str(e)}", throttle_duration_sec=5)

    def process_data(self):
        """处理并发布IMU数据"""
        self.read_serial()
        
        msg = Imu()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = self.get_parameter('frame_id').value
        
        # 设置四元数方向 (w,x,y,z)
        q = self.current_data['quat']
        msg.orientation = Quaternion(x=q[1], y=q[2], z=q[3], w=q[0])
        
        # 设置线加速度 (m/s²)
        accel = self.current_data['aworld']
        msg.linear_acceleration = Vector3(
            x=accel[0],
            y=accel[1],
            z=accel[2] - 9.8  # 减去重力加速度（假设Z轴向上）
        )
        
        # 设置角速度 (rad/s)
        ypr = self.current_data['ypr']
        msg.angular_velocity = Vector3(
            x=radians(ypr[0]),  # 转换为弧度
            y=radians(ypr[1]),
            z=radians(ypr[2])
        )
        
        # 发布消息
        self.publisher.publish(msg)
        self.get_logger().debug(f"发布数据: quat={q}, accel={accel}, ypr={ypr}", 
                              throttle_duration_sec=1)

def main(args=None):
    rclpy.init(args=args)
    node = DMPImuPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.ser.close()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()