ROS 2中发布和可视化传感器数据实战系列之四：kitti公开数据集IMU数据的发布及在RViz2中的显示

【作者注】：本文将介绍如何在ROS 2中将kitti公开数据集的IMU数据发布为sensor_msgs/IMU消息（文章附有完整的python源代码）。由于在ROS 2中RViz2默认情况下并不支持对IMU数据的可视化显示，因此需要自己添加专门用于显示sensor_msgs/Imu消息数据的插件。本文将通过添加Github网站上的imu_tools开源存储库中的rviz_imu_plugin插件软件包来将IMU数据中的线性加速度、角速度、方位等数据在RViz2中进行可视化显示。

走过路过，千万不要错过！！！撰文不易，广大知友若觉得本文有所帮助，还请多多点赞、收藏；如有不同意见或者看法，或者想要交流，请多多评论，或者联系作者，联系方式kennywangaigraphx@gmail.com，QQ-894625859。

另外作者在此郑重申明：本文为深圳季连科技有限公司（Aigraphx）kenny wang原创，知识产权归深圳季连所有，转载请一定要载明出处，否则深圳季连保留追究随意转载者法律责任的权利。

IMU全称Inertial Measurement Unit，即惯性测量单元，是测量物体三轴角速度和加速度的设备。一个IMU内通常装有三轴陀螺仪和三轴加速度计，分别用于测量物体在三维空间中的角速度和线性加速度。严格意义上的IMU只为用户提供三轴角速度及三轴加速度数据。

但有些模块会在IMU的基础上，以重力向量作为参考，用卡尔曼或者互补滤波等算法（即姿态解算算法）为用户提供有重力向量参考的俯仰角（pitch）、横滚角（roll）以及无参考标准的航向角（yaw），通常也称这类模块为6轴姿态模块，或者垂直参考单元（VRU，Vertical Reference Unit）。

还有的模块会在六轴IMU的基础上加装三轴磁力计，这种产品通常称为9轴模块，由于磁力计可以测量地磁场并通过磁场变化得出磁场北极方位的绝对值，因此通过内置姿态解算算法（卡尔曼或者互补滤波等算法）可以解算获得被测物体具有绝对参考的俯仰角、横滚角及航向角yaw（与地磁北极的夹角）的绝对全姿态，这类系统常用来为飞行器提供准确可靠的姿态与航行信息，因此通常被称为航姿参考系统（AHRS，Attitude and Heading Reference System）。广义上的IMU包括VRU和AHRS这两类系统。

IMU传感器目前也已成为自动驾驶、机器人和工业自动化等应用中重要且常用的传感器之一。由于IMU对相对和绝对位置的推演没有任何外部依赖，是一种类似于黑匣子的完备系统；IMU也不需要任何外部信号，可以被安装在汽车底盘等不外露的区域，可以对抗外来的电子或机械攻击；IMU对角速度和加速度的测量值之间本就具有一定的冗余性，再加上轮速计和方向盘转角等冗余信息，使其输出结果的置信度远高于其它传感器提供的绝对或相对定位结果。因此甚至有的观点认为，IMU是自动驾驶系统在定位领域的最后一道防线。而且，全球导航卫星系统（GNSS） + IMU是惯性导航系统（INS，Inertial Navigation System）的最常用的组合方案。由此可见，虽然IMU存在诸如测量精度会随时间漂移等缺陷，但其地位和重要性毋需质疑。

全球IMU主要生产商有Honeywell International、Analog Devices、EMCORE、TDK Corporation、Bosch（博世） Sensortec、Epson、STMicroelectronics等公司，国内也有少数几家IMU厂商，如深迪半导体、罕王电子、新纳传感、水木智芯、格纳微、华依科技等。由于我公司（深圳季连科技有限公司）客户已购置的部分激光雷达产品中内置了IMU（博世BMI088），且拟购置多款单独的IMU产品，拟安装应用于工业场景，主要用于进行工业自动化执行机构的姿态检测。由于目前尚未采购/安装IMU设备，因此需要先用开源的IMU数据集提前开展相关算法研究。为此，需将IMU数据发布为ROS 2话题消息（sensor_msgs/Imu）并在RViz2中进行可视化。

本文以kitti公开数据集中的IMU数据为例，IMU数据存放在oxts/data子目录下的各个.txt文件中，如下图所示：

首先来研究一下各个.txt文件中存储了哪些数据。用你喜欢的文本编辑器软件打开任意一个.txt文件，其内容应该如下图所示：

从该.txt文件中可以看到有30个数值，每个数值之间用一个空格隔开，但这30个数值代表什么含义呢？这就需要使用kitti数据集提供的元数据文件，即存放在oxts目录下的dataformat.txt文件才能知道这30个数值代表的含义。打开dataformat.txt文件，其内容应该如下所示：

也就是说，每个.txt文件中的30个数值分别表示IMU（其实应该是IMU + GPS或者惯性导航系统INS）数据的30个字段值，即空间位置（纬度lat、经度lon、海拔高度alt）、姿态（横滚角roll、俯仰角pitch、航向角yaw）、线速度（北向速度vn、东向速度ve、前向速度vf、左向速度vl、垂向速度vu）、线性加速度（x轴方向加速度ax、y轴方向加速度ay、z轴方向加速度az、前向加速度af、左向加速度al、垂向加速度au）、角速度（x轴方向角速度wx、y轴方向角速度wy、z轴方向角速度wz、前向角速度wf、左向角速度wl、垂向角速度wu）、位置精度pos_accuracy、速度精度vel_accuracy、导航状态navstat、GPS接收器追踪到的卫星数量numsats及其位置模式posmode、速度模式velmode和方位模式orimode。

但在ROS 2中发布IMU数据时并不需要而且也不能发布所有这30个字段值。具体可以发布哪些数据，需要通过ROS中的sensor_msgs/Imu消息类型接口定义来确定，其定义如下：

std_msgs/Header header

geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z

由此可见，在ROS 2中发布IMU数据到sensor_msgs/Imu消息类型的话题上时，只能发布方位、角速度和线性加速度这三类数据以及它们的协方差矩阵（3*3）。

掌握了上述信息后，我们要做的工作就是新建一个ROS 2软件包，在该软件包中编写IMU数据发布者节点代码，将kitti数据集中oxts/data子目录中各帧数据（对应一个.txt文件）中的方位、角速度和线性加速度数据读取到对应数组变量中，并将它们转换成Imu消息后进行发布。必要时还可以创建launch文件和.rviz配置文件，然后对该软件包进行编译，这样就可以用ROS 2将kitti数据集中的IMU数据发布到某个话题（topic）上，并在RViz2中进行可视化显示。有关如何创建、编辑和编译ROS 2软件包的具体方法，请参见本公司知乎专栏文章“ROS 2入门教程——2.2 创建您的第一个ROS 2软件包”和“ROS 2入门教程——2.4 编写一个简单的发布者和订阅者(Python)”。

下面是kenny wang编写的一个专用于kitti数据集IMU数据的ROS 2简单发布者节点的Python源代码：

import os
import pandas as pd
import math

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
from tf_transformations import quaternion_from_euler

IMU_COLUMN_NAMES = ['lat', 'lon', 'alt', 'roll', 'pitch', 'yaw', 'vn', 've', 'vf', 'vl', 'vu',\
                    'ax', 'ay', 'az', 'af', 'al', 'au', 'wx', 'wy', 'wz', 'wf', 'wl', 'wu', \
                    'posacc', 'velacc', 'navstat', 'numsats', 'posmode', 'velmode', 'orimode']

class KittiNode(Node):
    def __init__(self, name):
        super().__init__(name)
        self.get_logger().info("Initializing %s！" % name)
        self.imu_pub = self.create_publisher(Imu, 'kitti_imu', 10)
        timer_period = 0.1  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.velodyne_file_paths = \
            '/home/wangjg/datasets/kitti_raw_data/2011_09_26_drive_0093_sync/oxts/data/'
        self.file_number = len(self.velodyne_file_paths)
        self.frame = 0

    def timer_callback(self):
        imu = Imu()
        imu.header.frame_id = 'velodyne'
        imu.header.stamp = self.get_clock().now().to_msg()
 
        imu_data = read_imu(os.path.join(self.velodyne_file_paths, '%010d.txt' % self.frame))
        q = quaternion_from_euler(float(imu_data.roll),
                              float(imu_data.pitch),
                              float(imu_data.yaw))  # prevent the data from being overwritten
        imu.orientation.x = q[0]
        imu.orientation.y = q[1]
        imu.orientation.z = q[2]
        imu.orientation.w = q[3]
        imu.angular_velocity.x = float(imu_data.wf)
        imu.angular_velocity.y = float(imu_data.wl)
        imu.angular_velocity.z = float(imu_data.wu)
        imu.linear_acceleration.x = float(imu_data.af)
        imu.linear_acceleration.y = float(imu_data.al)
        imu.linear_acceleration.z = float(imu_data.au)
        self.imu_pub.publish(imu)

        self.get_logger().info("kitti imu data published %d " % self.frame)
        self.frame += 1
        if self.frame == self.file_number:
            self.frame = 0

def read_imu(path):
    df = pd.read_csv(path, header=None, sep=' ')
    df.columns = IMU_COLUMN_NAMES
    return df

def main(args=None):
    rclpy.init(args=args)  # 初始化rclpy
    node = KittiNode('kitti_imu_publisher')  # 实例化节点对象
    try:    # 保持节点运行，检测是否收到退出指令（Ctrl+C）
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("user keyboard interrupt !")
    node.destroy_node()  # 显式删除节点对象
    rclpy.shutdown()  # 关闭rclpy

编译软件包后，运行这个软件包的发布者节点，就可以将kitti数据集的IMU数据发布到ROS的话题/kitti_imu上，如下图所示：

此时运行rviz2，点击左下角的“Add”按钮，发现并不能添加正在发布中的/kitti_imu话题，如下图所示：

这是为什么呢？这是因为在ROS 2中RViz2并不能直接支持sensor_msgs/Imu消息数据的可视化显示，这也可以在其官方文档中发现这一点，如下图所示：

在上图中第一列列出了已经从ROS 1迁移过来的、可以显示的消息类型，包括用于发布相机图像的Image消息、发布激光雷达点云数据的PointCloud2消息、发布激光扫描数据的LaserScan消息、发布超声波或红外测距仪距离数据的Range消息等，但目前并不包含对发布IMU数据的Imu消息的支持。因此，为了在RViz2中显示IMU数据，需要自己另外添加专门用于支持Imu消息显示的rviz插件。

幸运的是，我们有万能的Github及其上面的众多开源资源库/存储库。在Github网站上输入IMU和ROS关键词进行搜索，可以找到很多相关的存储库，其中第一个CCNYRoboticsLab/imu_tools存储库中就有我们需要的插件rviz_imu_plugin以及基于Madgwick梯度下降姿态解算算法的姿态解算工具imu_filter_madgwick和基于互补滤波算法的姿态解算工具imu_complementary_filter。按照该存储库自述文档中介绍的方法进行二进制包安装或者从源代码编译安装（注意一定要选择与你机器上所安装ROS 2发行版相对应的分支，如foxy，galactic，或者humble，而且如果是用源代码编译安装的话，可以选择只编译我们需要的rviz_imu_plugin软件包），并设置好该插件软件包的环境后，再次运行RViz2后按下“Add”按钮，此时就可以将/kitti_imu话题发布的Imu消息数据进行可视化显示，如下图所示：

在对该IMU数据话题的固定坐标系（本实战中为velodyne）进行正确地配置后，就会在rviz2中可视化显示该IMU数据了。当然，为了方便，也可以参考本专栏本系列实战文章中的上一篇“RViz2可视化传感器数据系列教程之一：在RViz2中显示相机视频图像数据”，创建一个launch文件以同时运行启动这个软件包的点云数据发布者节点和rviz2，并用一个.rviz配置文件直接配置好rviz2，这样启动该launch文件就可以直接发布IMU数据，同时在rviz2中对该IMU数据进行可视化显示，在勾选Enable box、Enable axes和Enable acceleration复选框后，就可以显示出如下图所示的IMU数据。

参考资料：

github.com/CCNYRobotics

github.com/ros2/rviz