前言

在上一篇文章https://blog.csdn.net/2302_80779357/article/details/160084862?spm=1001.2014.3001.5501https://blog.csdn.net/2302_80779357/article/details/160084862?spm=1001.2014.3001.5501中,我们实现了通过RS485 Modbus轮询,在一个ROS 2节点中高效读取多个IMU的加速度、角速度和欧拉角数据。

此时,很多人可能会萌生一个自然的想法:“既然我有了3个IMU的姿态数据,如果我把它们分别绑在大臂、小臂和手背上,是不是就能在Rviz里复刻我的手臂动作了?”

如果你直接把IMU的姿态数据发给ROS的TF(Transform)树,你会发现Rviz里的手臂很快就扭成了一团“麻花”。本文将带你填平从“传感器数据”到“串联运动学可视化”之间的两个巨坑,并附上完整的Python驱动代码。


建立工作空间与目录结构 

在开始写代码之前,请确保你的 ROS2 工作空间结构是正确的。很多新手报错都是因为文件放错了位置。

打开终端,创建一个名为 wit_ros2_imu_ws 的工作空间,并创建一个名为 wit_ros2_imu 的 Python 包。 如果你已经建好了,请对照下方的目录树,检查你的文件是否都在对应的位置上:

wit_ros2_imu_ws/            # 工作空间根目录
├── src/                    # 源代码目录
│   └── wit_ros2_imu/       # ROS2包目录
│       ├── package.xml     # 包元数据(定义依赖项、版本等)
│       ├── setup.py        # Python包安装配置(定义可执行入口点)
│       ├── setup.cfg       # 配置文件(可选)
│       ├── resource/       # 资源文件目录
│       │   └── wit_ros2_imu # 包索引文件
│       ├── urdf/           # 🌟 新建:用于存放机械臂的 3D 模型文件
│       │   └── arm.urdf    # 机械臂模型文件
│       └── wit_ros2_imu/   # Python包主目录(存放你的核心代码)
│           ├── __init__.py # Python包初始化文件
│           ├── wit_ros2_imu.py       # IMU驱动及TF广播节点(核心)
│           └── imu_to_joint_state.py # 关节状态发布节点 (如有)
├── build/                  # 构建输出目录(colcon build 自动生成)
├── install/                # 安装目录(colcon build 自动生成)
└── log/                    # 日志目录(colcon build 自动生成)

构建机械臂的编写URDF

在让机械臂动起来之前,我们得先在ROS里告诉它机械臂长什么样。我们将用URDF(统一机器人描述格式)画几个圆柱体来代表手臂。

在你的ROS2工作空间下,新建一个文件夹存放URDF

<?xml version="1.0"?>
<robot name="human_arm">

  <!-- 底座 -->
  <link name="base_link"/>

 
  <!-- 上臂 -->
  <link name="upper_arm">
    <visual>
      <geometry><box size="0.08 0.05 0.3"/></geometry>
      <material name="red"><color rgba="1 0 0 1"/></material>
    </visual>
  </link>

  <!-- 前臂 -->
  <link name="forearm">
    <visual>
      <geometry><box size="0.08 0.05 0.27"/></geometry>
      <material name="green"><color rgba="0 1 0 1"/></material>
    </visual>
  </link>

  <!-- 手腕/手掌 -->
  <link name="wrist">
    <visual>
      <geometry><box size="0.08 0.05 0.12"/></geometry>
      <material name="blue"><color rgba="0 0 1 1"/></material>
    </visual>
  </link>

 
  <!-- 底座 → 上臂 -->
  <joint name="joint_shoulder" type="fixed">
    <parent link="base_link"/>
    <child link="upper_arm"/>
    <origin xyz="0 0 0"/>
  </joint>

  <!-- 上臂 → 前臂 (肘关节) -->
  <joint name="joint_elbow" type="fixed">
    <parent link="upper_arm"/>
    <child link="forearm"/>
    <origin xyz="0 0 0.3"/>
  </joint>

  <!-- 前臂 → 手腕 (腕关节) -->
  <joint name="joint_wrist" type="fixed">
    <parent link="forearm"/>
    <child link="wrist"/>
    <origin xyz="0 0 0.27"/>
  </joint>


  <link name="imu_51"/>
  <link name="imu_52"/>
  <link name="imu_53"/>

  <!-- IMU51 → 上臂 -->
  <joint name="imu_upper" type="fixed">
    <parent link="upper_arm"/>
    <child link="imu_51"/>
    <origin xyz="0 0 0.15"rpy="0 0 0"/>
  </joint>

  <!-- IMU52 → 前臂 -->
  <joint name="imu_forearm" type="fixed">
    <parent link="forearm"/>
    <child link="imu_52"/>
    <origin xyz="0 0 0.135"rpy="0 0 0"/>
  </joint>

  <!-- IMU53 → 手腕 -->
  <joint name="imu_wrist" type="fixed">
    <parent link="wrist"/>
    <child link="imu_53"/>
    <origin xyz="0 0 0.06"rpy="0 0 0"/>
  </joint>

</robot>

核心难点:为什么手臂会扭成“麻花”?

在使用穿戴式IMU驱动虚拟模型时,我们面临两个本质矛盾:

1. 物理安装与虚拟连杆的“轴向冲突”

  • 虚拟模型(URDF)的规矩: 在传统的机器人学中,连杆(Link)的延伸方向通常被定义为 Z轴。

  • 物理传感器的现实: 为了佩戴方便和按键朝向,当我们把IMU绑在手臂上时,沿着手臂延伸方向的往往是IMU的 Y轴。

  • 如果你不进行坐标系映射,当你抬起手臂时,Rviz里的模型可能会向侧面“劈叉”。

2. 绝对姿态与相对姿态的“叠加冲突”(重点)

  • IMU输出的是“绝对姿态”: 所有的IMU都是基于地球重力和地磁场进行解算的,它输出的四元数是相对于“世界坐标系”的绝对旋转。

  • ROS TF树需要的是“相对姿态”: TF树是一个父子层级结构(base_link -> upper_arm -> forearm)。如果你把大臂在世界里的绝对姿态发给TF,又把小臂的绝对姿态发给TF,ROS会认为小臂是在已经旋转过的大臂基础上,再叠加一次绝对旋转!这会导致转角被放大一倍,最终模型彻底错乱。


四元数数学解算

为了解决上述问题,我们需要在代码中引入四元数(Quaternion)的乘法和求逆运算。

第一步:校准安装误差(局部旋转)

我们需要将IMU的数据坐标系“拧”到与机器臂连杆一致。假设我们需要让IMU的Y轴映射到连杆的Z轴,这相当于让传感器局部坐标系绕X轴旋转了-90度。

使用四元数右乘来实现局部坐标系的转换:

$q_{link\_abs} = q_{raw} \times q_{offset}$

此时的 $q_{link\_abs}$就是该节手臂在世界坐标系下的绝对正确姿态。

        # 1. 原始世界坐标系绝对姿态
        q_raw = self.eulerToQuaternion(angle[0], angle[1], angle[2])
        
        # 2. 局部轴向映射:把 IMU 的 Y 轴转为连杆的 Z 轴
        # 绕局部 X 轴旋转 +90 度
        theta = -math.pi / 2
        q_offset = [
            math.sin(theta/2) * 1.0,  # x
            0.0,                      # y
            0.0,                      # z
            math.cos(theta/2)         # w
        ]
        # q_link_abs 是当前连杆在【世界坐标系】下的绝对正确姿态
        q_link_abs = self.quaternion_multiply(q_raw, q_offset)

第二步:剥离父节点运动(求取相对姿态)

对于串联机械臂,子连杆在TF树中的姿态,应该仅仅是它当前关节的弯曲程度。

我们需要用子节点的绝对姿态,减去父节点的绝对姿态。在四元数运算中,这等同于“左乘父节点四元数的逆”:

$q_{rel} = q_{parent}^{-1} \times q_{child}$

算出$q_{rel}$后,再将其广播到TF树中,手臂就能完美联动了!

# 3. 计算该节手臂相对于父节点的相对姿态
if dev_id == 0x51:
    # 上臂,直接使用绝对姿态(因为它的父节点是 base_link,通常固定)
    q_rel = q_link_abs
    parent_frame = "base_link"
    child_frame = "upper_arm"
elif dev_id == 0x52:
    # 前臂,它的父节点是上臂
    q_parent_abs = self.abs_orientations.get(0x51, [0.0, 0.0, 0.0, 1.0])
    q_parent_inv = self.quaternion_inverse(q_parent_abs)
    q_rel = self.quaternion_multiply(q_parent_inv, q_link_abs)
    parent_frame = "upper_arm"
    child_frame = "forearm"
elif dev_id == 0x53:
    # 手腕,它的父节点是前臂
    q_parent_abs = self.abs_orientations.get(0x52, [0.0, 0.0, 0.0, 1.0])
    q_parent_inv = self.quaternion_inverse(q_parent_abs)
    q_rel = self.quaternion_multiply(q_parent_inv, q_link_abs)
    parent_frame = "forearm"
    child_frame = "wrist"
else:
    return
...
# 4. 广播该节手臂相对于父节点的姿态到 TF 树
    t = TransformStamped()
    t.header.stamp = msg.header.stamp
    t.header.frame_id = parent_frame
    t.child_frame_id = child_frame
    t.transform.translation.x = 0.0
    t.transform.translation.y = 0.0
    t.transform.translation.z = 0.3 if dev_id != 0x53 else 0.27
    t.transform.rotation.x = q_rel[0]
    t.transform.rotation.y = q_rel[1]
    t.transform.rotation.z = q_rel[2]
    t.transform.rotation.w = q_rel[3]
    self.tf_broadcaster.sendTransform(t)

完整代码实现

在上一篇轮询代码的基础上,我们增加了 abs_orientations 字典来缓存每个IMU的绝对姿态,并重写了 publishImu 函数。

import math
import serial
import struct
import numpy as np
import rclpy
from enum import Enum
from rclpy.node import Node
from sensor_msgs.msg import Imu
from imu_msg.msg import ImuData
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster


class protocolType(Enum):
    TTL_STD = 1
    TTL_HIGH = 2
    CAN_STD = 3
    CAN_HIGH = 4
    RS485_STD = 5
    RS485_HIGH = 6


class imuDriverNode(Node):
    def __init__(self):
        super().__init__("imuDriverNode")

        self.declare_parameter("port", "/dev/imu_usb")
        self.declare_parameter("baudrate", 115200)
        self.declare_parameter("protocol", "RS485_STD")
        self.declare_parameter("modbusIDs", [0x51, 0x52, 0x53])
        # 声明节点的参数,包括串口端口、波特率、通信协议和Modbus ID列表。

        self.port = self.get_parameter("port").value
        self.baudrate = self.get_parameter("baudrate").value
        self.modbusIDs = self.get_parameter("modbusIDs").value
        protocolStr = self.get_parameter("protocol").value
        self.protocol = protocolType[protocolStr]
        # 获取节点的参数值。

        self.serialPort = serial.Serial(
            self.port, self.baudrate, timeout=0.001, write_timeout=0
        )
        self.get_logger().info(f"Serial opened: {self.port} @ {self.baudrate}")
        # 打开串口并初始化。

        self.tf_broadcaster = TransformBroadcaster(self)
        # 初始化TF2广播器,用于发布TF变换。
        self.modbusAddrList = [0x34, 0x37, 0x3A, 0x3D]
        self.modbusIndex = 0
        # 初始化Modbus地址索引为0。
        self.imuIndex = 0
        # 初始化IMU索引为0。
        self.waitingResponse = False
        self.lastSendTime = 0
        # 初始化等待响应标志为False,上次发送时间戳为0。
        self.currentRequestAddr = None
        # 初始化当前请求地址为None。
        self.rxBuffer = bytearray()
        # 初始化接收缓冲区为空字节数组。
        self.imuData = {}
        # 初始化IMU数据字典,用于存储每个IMU的加速度、陀螺仪和角度数据。
        self.imuPublishers = {}
        # 初始化IMU发布者字典,用于发布每个IMU的IMU数据和RPY数据。
        self.abs_orientations = {}
        # 初始化绝对姿态字典,用于存储每个IMU的绝对四元数姿态。
        for mid in self.modbusIDs:
            self.imuData[mid] = {
                'acc': np.zeros(3),
                'gyro': np.zeros(3),
                'angle': np.zeros(3),
                'mag': np.zeros(3),
            }
            # 初始化每个IMU的加速度、陀螺仪和角度数据为0。
            self.abs_orientations[mid] = [0.0, 0.0, 0.0, 1.0]  # 初始化为单位四元数
            suffix = f"imu_{mid:02x}"
            self.imuPublishers[mid] = {
                'imu': self.create_publisher(Imu, f"imu/data/{suffix}", 10),
                'rpy': self.create_publisher(ImuData, f"imu/rpy/{suffix}", 10),
            }

        self.create_timer(0.001, self.timerCallback)#每1ms调用一次timerCallback
        self.create_timer(0.1, self.printMsg)#每100ms调用一次printMsg
        # 创建定时器,用于打印IMU数据。
    def timerCallback(self):#定时器回调函数,用于发送Modbus读取命令。
        if self.protocol in (protocolType.RS485_STD, protocolType.RS485_HIGH):
            # 如果是RS485协议,才发送Modbus读取命令。
            now = self.get_clock().now().nanoseconds
            # 获取当前时间戳,单位为纳秒。
            if not self.waitingResponse:
                # 如果没有等待响应,才发送Modbus读取命令。
                current_id = self.modbusIDs[self.imuIndex]
                # 获取当前IMU的Modbus ID。
                addr = self.modbusAddrList[self.modbusIndex]
                # 获取当前Modbus地址。
                self.currentRequestAddr = addr
                # 更新当前请求地址为当前Modbus地址。

                if self.protocol == protocolType.RS485_HIGH and addr == 0x3D:
                    cmd = self.buildModbusReadCmd(current_id, addr, 6)
                    # 构建6字节的Modbus读取命令,用于读取加速度、陀螺仪和角度数据。
                else:
                    cmd = self.buildModbusReadCmd(current_id, addr, 3)
                    # 构建3字节的Modbus读取命令,用于读取加速度、陀螺仪和磁力数据。

                self.serialPort.write(cmd)
                self.waitingResponse = True
                self.lastSendTime = now
                # 更新上次发送时间戳为当前时间戳。

            elif now - self.lastSendTime > 50000000:
                # 如果上次发送时间戳超过50ms,说明没有收到响应。
                self.waitingResponse = False
                self.currentRequestAddr = None
                self.modbusIndex = (self.modbusIndex + 1) % len(self.modbusAddrList)
                if self.modbusIndex == 0:
                    self.imuIndex = (self.imuIndex + 1) % len(self.modbusIDs)
                    # 切换到下一个IMU。

        self.readSerial()
        self.parseBuffer()

    def readSerial(self):
        cnt = self.serialPort.in_waiting
        # 获取串口接收缓冲区的字节数。
        if cnt:
            self.rxBuffer.extend(self.serialPort.read(cnt))
        # 从串口接收缓冲区读取数据并扩展到接收缓冲区。

    # 解析接收缓冲区中的数据。
    def parseBuffer(self):
        if self.protocol not in (protocolType.RS485_STD, protocolType.RS485_HIGH):
            return

        while len(self.rxBuffer) >= 5:
            dev_id = self.rxBuffer[0]
            # 获取接收缓冲区的第一个字节,即设备bus ID。
            if dev_id not in self.modbusIDs:
                self.rxBuffer.pop(0)
                continue
            if self.rxBuffer[1] != 0x03:
                self.rxBuffer.pop(0)
                continue
                # 如果不是Modbus读取命令,跳过。
            byteCnt = self.rxBuffer[2]
            frameLen = 3 + byteCnt + 2
            if len(self.rxBuffer) < frameLen:
                return
                # 如果接收缓冲区中的数据不足,跳过。
            frame = self.rxBuffer[:frameLen]
            recv_crc = (frame[-2] << 8) | frame[-1]
            calc_crc = self.modbusCRC(frame[:-2])
            calc_crc = ((calc_crc & 0xFF) << 8) | (calc_crc >> 8)
            # 计算Modbus校验和。

            if recv_crc != calc_crc:
                self.rxBuffer.pop(0)
                continue
                # 如果校验和错误,跳过。
            del self.rxBuffer[:frameLen]
            self.handleModbusFrame(frame, dev_id)
            # 处理Modbus数据帧。
    
    def handleModbusFrame(self, frame, dev_id):
        byteCnt = frame[2]
        data = frame[3:3+byteCnt]
        addr = self.currentRequestAddr
        imu = self.imuData[dev_id]
        # 获取当前IMU的IMU数据字典。

        if self.protocol == protocolType.RS485_STD:
            if len(data) == 6:
                v = struct.unpack(">hhh", data)
                if addr == 0x34: imu['acc'] = np.array(v)
                if addr == 0x37: imu['gyro'] = np.array(v)
                if addr == 0x3A: imu['mag'] = np.array(v)
                if addr == 0x3D: imu['angle'] = np.array(v)
                # 如果是角度数据,直接赋值。
        else:
            if addr == 0x3D and len(data) == 12:
                # 如果是角度数据,直接赋值。
                def p32(o):
                    raw = (data[o] << 24) | (data[o+1] << 16) | (data[o+2] << 8) | data[o+3]
                    return raw if raw < 0x80000000 else raw - 0x100000000
                    # 处理32位有符号整数,将负数转换为正数。
                imu['angle'] = np.array([p32(0), p32(4), p32(8)])
                # 赋值角度数据。
            elif len(data) == 6:
                # 如果是加速度、陀螺仪和磁力数据,直接赋值。
                v = struct.unpack(">hhh", data)
                if addr == 0x34: imu['acc'] = np.array(v)
                if addr == 0x37: imu['gyro'] = np.array(v)
                if addr == 0x3A: imu['mag'] = np.array(v)
                # 如果是加速度、陀螺仪和磁力数据,直接赋值。

        self.publishImu(dev_id)
        # 发布IMU数据。
        self.waitingResponse = False
        # 等待响应标志位设为False。
        self.modbusIndex = (self.modbusIndex + 1) % len(self.modbusAddrList)
        if self.modbusIndex == 0:
            # 切换到下一个IMU。
            self.imuIndex = (self.imuIndex + 1) % len(self.modbusIDs)

    def publishImu(self, dev_id):
        imu = self.imuData[dev_id]
        acc = imu['acc'] * 16 / 32768.0
        gyro = np.radians(imu['gyro'] * 2000 / 32768.0)
        mag = imu['mag']

        if self.protocol == protocolType.RS485_HIGH:
            angle = np.radians(imu['angle'] / 1000.0)
        else:
            angle = np.radians(imu['angle'] * 180.0 / 32768.0)

        # 1. 原始世界坐标系绝对姿态
        q_raw = self.eulerToQuaternion(angle[0], angle[1], angle[2])
        
        # 2. 局部轴向映射:把 IMU 的 Y 轴转为连杆的 Z 轴
        # 绕局部 X 轴旋转 +90 度
        theta = -math.pi / 2
        q_offset = [
            math.sin(theta/2) * 1.0,  # x
            0.0,                      # y
            0.0,                      # z
            math.cos(theta/2)         # w
        ]
        # q_link_abs 是当前连杆在【世界坐标系】下的绝对正确姿态
        q_link_abs = self.quaternion_multiply(q_raw, q_offset)
        
        # 缓存当前绝对姿态
        self.abs_orientations[dev_id] = q_link_abs

        # 3. 发布 IMU 话题消息 (IMU 数据通常保留绝对姿态)
        msg = Imu()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = f"imu_{dev_id:02x}_link"
        msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z = acc.tolist()
        msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z = gyro.tolist()
        msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w = q_link_abs

        rpy = ImuData()
        rpy.header = msg.header
        rpy.imu = msg
        rpy.roll, rpy.pitch, rpy.yaw = map(math.degrees, angle)

        self.imuPublishers[dev_id]['imu'].publish(msg)
        self.imuPublishers[dev_id]['rpy'].publish(rpy)

        # ====================== 4. TF 相对姿态解算广播 ======================
        t = TransformStamped()
        t.header.stamp = msg.header.stamp
        
        # Q_relative = Q_parent_inverse * Q_child
        if dev_id == 0x51:
            t.header.frame_id = "base_link"
            t.child_frame_id = "upper_arm"
            t.transform.translation.z = 0.3
            # base_link 通常不动,所以相对姿态等于自身的绝对姿态
            q_tf = q_link_abs 
            
        elif dev_id == 0x52:
            t.header.frame_id = "upper_arm"
            t.child_frame_id = "forearm"
            t.transform.translation.z = 0.3
            # 获取父节点(0x51)的绝对姿态求逆,再乘以当前绝对姿态,得到相对旋转
            q_parent_inv = self.quaternion_inverse(self.abs_orientations[0x51])
            q_tf = self.quaternion_multiply(q_parent_inv, q_link_abs)
            
        elif dev_id == 0x53:
            t.header.frame_id = "forearm"
            t.child_frame_id = "wrist"
            t.transform.translation.z = 0.27
            # 获取父节点(0x52)的绝对姿态求逆,再乘以当前绝对姿态,得到相对旋转
            q_parent_inv = self.quaternion_inverse(self.abs_orientations[0x52])
            q_tf = self.quaternion_multiply(q_parent_inv, q_link_abs)
            
        else:
            return

        # 其他位移置零
        t.transform.translation.x = 0.0
        t.transform.translation.y = 0.0
        
        # 将计算好的“相对姿态”赋值给 TF
        t.transform.rotation.x = q_tf[0]
        t.transform.rotation.y = q_tf[1]
        t.transform.rotation.z = q_tf[2]
        t.transform.rotation.w = q_tf[3]
        
        self.tf_broadcaster.sendTransform(t)
    def buildModbusReadCmd(self, mid, addr, length):
        f = bytearray([mid, 0x03, (addr >> 8) & 0xFF, addr & 0xFF, (length >> 8) & 0xFF, length & 0xFF])
        crc = self.modbusCRC(f)
        f.extend([crc & 0xFF, (crc >> 8) & 0xFF])
        return f

    @staticmethod
    def modbusCRC(data):
        crc = 0xFFFF
        for b in data:
            crc ^= b
            for _ in range(8):
                crc = (crc >> 1) ^ 0xA001 if crc & 1 else crc >> 1
        return crc

    @staticmethod
    def eulerToQuaternion(r, p, y):
        cr, cp, cy = math.cos(r/2), math.cos(p/2), math.cos(y/2)
        sr, sp, sy = math.sin(r/2), math.sin(p/2), math.sin(y/2)
        return [
            sr*cp*cy - cr*sp*sy,
            cr*sp*cy + sr*cp*sy,
            cr*cp*sy - sr*sp*cy,
            cr*cp*cy + sr*sp*sy
        ]

    @staticmethod
    def quaternion_multiply(q1, q2):
        x1, y1, z1, w1 = q1
        x2, y2, z2, w2 = q2
        x = w1*x2 + x1*w2 + y1*z2 - z1*y2
        y = w1*y2 - x1*z2 + y1*w2 + z1*x2
        z = w1*z2 + x1*y2 - y1*x2 + z1*w2
        w = w1*w2 - x1*x2 - y1*y2 - z1*z2
        return [x, y, z, w]
    
    @staticmethod
    def quaternion_inverse(q):
        """计算四元数的逆"""
        x, y, z, w = q
        norm = x*x + y*y + z*z + w*w
        return [-x/norm, -y/norm, -z/norm, w/norm]

    def printMsg(self):
        print("\033[H\033[J", end="")
        for mid in self.modbusIDs:
            imu = self.imuData[mid]
            acc = imu['acc'] * 16 / 32768.0
            gyro = imu['gyro'] * 2000 / 32768.0
            angle = imu['angle'] * (180.0 / 32768.0 if self.protocol == protocolType.RS485_STD else 0.1)
            print(f"=== IMU {hex(mid)} ===")
            print(f"Accel: x={acc[0]:+8.3f} | y={acc[1]:+8.3f} | z={acc[2]:+8.3f} m/s²")
            print(f"Gyro:  x={gyro[0]:+8.3f} | y={gyro[1]:+8.3f} | z={gyro[2]:+8.3f} °/s")
            print(f"Angle: x={angle[0]:+8.3f} | y={angle[1]:+8.3f} | z={angle[2]:+8.3f} °")
            print(f"Mag:   x={imu['mag'][0]:+8.0f} | y={imu['mag'][1]:+8.0f} | z={imu['mag'][2]:+8.0f}\n")

    def shutdown(self):
        if self.serialPort and self.serialPort.is_open:
            self.serialPort.close()
            self.get_logger().info("Serial port closed")


def main():
    rclpy.init()
    node = imuDriverNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.shutdown()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

Rviz 测试与效果

  1. 编译并运行节点。

    colcon build
    source insatll/setup.bash
    ros2 run wit_ros2_imu wit_ros2_imu 
  2. 新开一个终端

    ros2 run robot_state_publisher robot_state_publisher arm.urdf
  3. 新开一个终端,打开 rviz2,将 Fixed Frame 设置为 base_link。

    ros2 run rviz2 rviz2

  4. 添加 TF 插件,勾选显示 Names 和 Axes。

  5. 现在,当你挥动贴满IMU的手臂时,你应该能看到 upper_arm、forearm 和 wrist 三个连杆像真正的骨骼一样,首尾相连并顺滑地跟随你的动作旋转了!

总结

将真实世界的传感器引入ROS的世界时,一定要牢记坐标系的一致性以及TF树的层级相对性。一旦想通了这层数学逻辑,不管是动捕服、多节机械臂还是蛇形机器人,万变不离其宗。

希望能帮到正在折腾IMU和机器人的你!有问题欢迎在评论区交流。

Logo

DAMO开发者矩阵,由阿里巴巴达摩院和中国互联网协会联合发起,致力于探讨最前沿的技术趋势与应用成果,搭建高质量的交流与分享平台,推动技术创新与产业应用链接,围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐