毫米波雷达SLAM：利用TI IWR1443+ROS+NVIDIA JTX+TurtleBot2实现（上）

IWR1443ROS驱动安装&工作原理解析

服务器和必备的各种IDE已经准备完毕（RE0）。从这一章（RE1）开始利用IWR1443毫米波雷达配合JTX1和turtelBot2复现TI公司的SLAM建图实验。本文先介绍一下如何编译和使用IWR1443雷达驱动以及简要介绍其工作原理。

驱动安装

0、前期准备

需要提前说明的是，要完成毫米波雷达建图实验，我们至少需要两台能够运行Ubuntu的设备，并且至少有一台需要是移动设备。本次实验采用NVIDIA公司的JTX1开发板作为Ubuntu的移动设备，安装在TurtleBot2上，用于接受雷达信息并控制机器人。另外一台Ubuntu设备是RE0当中配置的服务器。

而现在要安装的驱动虽然不是必须的，但最好是在两台设备上都进行安装，因为我们经常需要调试雷达的参数并查看雷达的成像效果。在两台设备上都安装的话比较便于调试。

1、下载源码

有关的源码下载自TI Resource Explorer网站（http://dev.ti.com/tirex/#/）。
进入之后在左侧文件浏览器当中选择：mmWave Sensor->Industrial Toolbox
在右侧的内容浏览器的顶部最右侧有下载按钮，点击即可下载所有关于TI mmWave雷达的源码。

2、准备雷达

TI IWR1443EVM是一块板载雷达和DSP的开发板，其本身內建一个开箱的程序。但是如果我们曾使用其做过其他的实验（在其DSP当中烧录了其他程序），或者更改过其运行参数，就没办法直接使用TI提供的驱动代码。方便起见我们需要重新向DSP上烧录官方提供的OUT-OF-BOX程序。

但是我们其实是可以更改雷达的驱动代码，使其适应不同的DSP程序，或者是发射波形配置的

注意在烧录程序的时候需要将SOP2跳键帽扣上：

而加载配置和使用的时候要将跳键帽拔出。
烧录完成之后拔开跳键帽。

3、编译源码

1、在合适的地方新建一个文件夹“<name>_ws”作为这个驱动包的工作空间，当然名字可以随便起。

mkdir <name>_ws

2、在工作空间下新建一个src文件夹作为源码目录。这个src文件夹的文件名必须是src，这是ROS编译器catkin_make所要求的。

cd <name>_ws
mkdir src

3、在src文件夹下将压缩包当中的ti_mmwave_rospkg文件夹解压出来。

4、需要编译源码我们还差一个组件，这个组件不是ROS的基础组件，无法使用rosdep获取，所以我们需要从源码编译。在src目录下git，或者直接在github上面下载解压均可。

git clone https://github.com/wjwwood/serial.git

我常常因为网络不好git不下来，建议直接去github下载

git完毕后我们不需要去调整他，serial跟ti_mmwave_rospkg平行放置即可。

5、编译
回到工作空间目录下

catkin_make

到这里就编译完成，接下来测试一下源码。

4、测试驱动

测试驱动之前先要给用于接收雷达数据的和发送配置文件的两个USB串口获取权限。在没有更改串口名称的情况下，TI的毫米波雷达使用的都是ttyACM1和ttyACM0，但是有很多其他的设备有时也会起这个名字，要注意。如果有重名的话需要改一下。
每次测试驱动前，或是重新插拔都需要：

sudo chmod a+rw /dev/ttyACM0
sudo chmod a+rw /dev/ttyACM1

否则会出现错误：

DataUARTHandler Read Thread: Failed to open Data serial port with error: IO Exception (13): Permission denied, file /home/mug/WORK/RDrive_ws/src/serial/src/impl/unix.cc, line 151.

且rviz界面没有显示。
在获得串口权限和source之后，运行：

roslaunch ti_mmwave_rospkg rviz_1443_3d.launch

可以看到雷达点云。

驱动解析

一、文件结构

TI提供的最主要驱动代码文件夹是“ti_mmwave_rospkg”当然要顺利运行还需要另一个文件夹“serial”但是因为我们不需要改动后者，所以解析的内容旨在分析“ti_mmwave_rospkg”源码文件夹。

在“ti_mmwave_rospkg”源码文件夹下共有6个文件夹，其作对应的做用如下

• cfg ：存放默认雷达配置文件，每个启动项的配置文件与应用同名。
• docs ：存放雷达驱动的数据手册和使用说明。
• include ：存放头文件，定义了数据格式等。
• launch ：存放启动项文件。
• src ：存放include文件夹对应的源文件。
• srv : 存放服务格式文件.srv。

其中cfg文件如何去解析，launch启动项文件如何去编写等内容不在TI驱动的主要考虑范围，下文主要解析src、include和src文件夹。

二、服务

服务用于将配置文件载入雷达flash当中，需要启动的节点除了mmWaveQuickConfiguickConfig外还需要mmWaveCommSrv用于接收配置文件。启动launch文件的时候会自动启动mmWaveCommSrv。

服务传递的信息格式也很简单，发收分别通过string型的字符串comm和resp完成。对应的头文件和源文件实现也比较常规。唯一值得注意的地方是在编程时头文件内需要包含服务格式“mmWaveCLI”对应的头文件，这个文件是在编译过程当中自动生成的，因此需要在CMakeLists中写好。

三、节点

先介绍一下PC处理数据的流程，该过程分为三步：

• 利用“serial”库从串口获取数据。
• 利用PC内的两块缓存区，缓存1获取数据A时（主要是找到魔数，并把它去掉），缓存2对之前获取到的数据B进行重新排序。这个过程是同时发生的。
• 当A完成解包，B完成排序后，缓存1把A给缓存2处理，自己再读入新的数据。（乒乓缓存）

四、数据解包

读取数据的过程很简单，检测一个长度为8字节的固定开头即可（magic word）。我们比较关心的是每一帧数据是如何被ROS理解的。这里每种型号的EVM发送的数据格式都是类似的，只有一些小细节不太一样。比如，1443的包头字长是40字，而其他型号有44个字，这是因为14系列缺少一个关于子帧数的信息。但是这些细节不影响我们分析，因此我们在这里将其忽略。详细的解包要求见下表：

index	context
0x00~0x04	version
0x05~0x08	total packet length
0x09~0x12	EVM platform
0x13~0x16	frame number
0x17~0x20	cpu cycles time
0x21~0x24	objects number
0x21~0x24	TLVs number
0x25~0x28	sub frame number

该表是数据包包头的内容解析，较为的简单，如果一个数据包的包头解析正确，且total packet length正确，就可以对该包的剩余部分，即检测数据部分进行解析。这里还需要额外提到的还有一点，关于TLV（Tag Length Value）。TLV是一种节点间的通信协议，通信前需要确定协议的形式和长度，这些在TLVs number中已经包含了。

检测数据会被存入RScan当中，并作为一个消息在之后发布。（随后要用到多个雷达协同工作时话题名称需要重新更改、编译）RScan可以看作一个一维向量，长度是探测到的物体个数。

    DataUARTHandler_pub = nodeHandle->advertise< sensor_msgs::PointCloud2 >("RScan", 100);

对RScan进行内存分配后，计算所有目标点可能出现的空间范围，其中用到的最大俯仰角和方向角是一般被设为90度。

    maxAllowedElevationAngleDeg = 90; // Use max angle if none specified
    maxAllowedAzimuthAngleDeg = 90; // Use max angle if none specified

RScan是pointcloud2型的消息，因此需要将对应的雷达数据转化为点云。转化的方式根据EVM当中烧录的SDK文件不同而有所变化。在3.x之后的版本当中获取检测数据和转换为点云的方式更为简单，因此我们以3.x之后的版本为例来进行解析。

首先先声明存储雷达数据的类实体：

    mmwData.numObjOut = mmwData.header.numDetectedObj;

之后对雷达数据当中的每个目标点做处理，分别拷贝每个检测目标的坐标（xyz单位m）以及速度信息（单位m/s），像这样：

    memcpy( &mmwData.newObjOut.x, &currentBufp->at(currentDatap), sizeof(mmwData.newObjOut.x));//获取x坐标信息
    currentDatap += ( sizeof(mmwData.newObjOut.x) );//移动数据指针到y坐标信息首位

接下来需要把这些坐标信息转化为ROS能够接受的RScan格式，这其中涉及到一个坐标轴的转化问题。ROS默认的x轴平行于EVM数据默认的y轴；其y轴平行于EMV的x轴、且指向相反；两者z轴重叠。

     RScan->points[i].x = mmwData.newObjOut.y;   // ROS standard coordinate system X-axis is forward which is the mmWave sensor Y-axis
     RScan->points[i].y = -mmwData.newObjOut.x;  // ROS standard coordinate system Y-axis is left which is the mmWave sensor -(X-axis)
     RScan->points[i].z = mmwData.newObjOut.z;   // ROS standard coordinate system Z-axis is up which is the same as mmWave sensor Z-axis

3.x以后的版本当中将幅度值存在了整个检测目标数据之后读取，方式类似坐标，获取后直接传回RScan即可。但是诸如检测目标的速度信息，以及噪声等信息，虽然驱动当中写了相关的代码可以在指针指向该数据时做一些处理，但最终都没有赋值给RScan，也就是没有传入到ROS系统当中，因此不再赘述。