在ros中使用 RPLIDAR_A1 激光雷达 8000点/秒 的配置方法

下午拿到一个思岚科技的RPLIDAR_A1，具体版本型号是A1M8-R5。

直接在 ros 环境下搞起，配置与使用非常简单，但是仔细阅读了源码却发现一些需要注意的地方，在这里罗列下来。

1、安装

非常简单，只需要一条指令。

sudo apt-get install ros-YOUR_ROS_DISTRO-rplidar-ros

然后就可以利用 rospack find 指令查看是否安装成功。

2、rplidar-ros源码链接

利用指令安装后没有源码，可以github查看源码。如果有需要可以下载下来， 当做普通 ros package 来进行更改重新编译。

https://github.com/Slamtec/rplidar_ros

3、升级固件到8K Boost模式

在官网的下载栏，有一个8K Boost模式的醒目字样，于是果断下载固件进行了升级。

http://www.slamtec.com/cn/Support#rplidar-a-series

下载链接是一个exe文件，直接双击运行；连接雷达，选择对应串口后一路点击“下一步”就行。

4、ROS下更改工作模式

默认的 rplidar.launch 文件中，没有对应参数，需要添加一项

<param name="scan_mode"  type="string"  value="Boost">

运行后可以看到提示信息中有工作模式和8K的字样。

5、注意事项

源码中有些地方写的不是很恰当，这里先贴出来代码再逐一分析。

        start_scan_time = ros::Time::now();
        op_result = drv->grabScanDataHq(nodes, count);
        end_scan_time = ros::Time::now();
        scan_duration = (end_scan_time - start_scan_time).toSec();

这里先通过 start_scan_time 记录下系统时间戳，然后 drv->grabScanDataHq(nodes, count) 获取到了一个周期中数量为 count 的逐点激光测量数值保存到 nodes 数组中。之后这个时间戳会被打到 /scan 的消息里，因此消息的时间戳是激光扫描开始的时间，如果需要做去畸变的算法，必须注意这个时间戳。

激光的默认坐标系是如下定义方式，扫描从0°到360°，按顺时针的扫描顺序逐点将距离、角度值、信号强度等信息写在 nodes 数组里。

紧跟下面的代码就是对扫描的角度进行判断和处理，使得所有角度都在0~360之间。

        if (op_result == RESULT_OK) {
            op_result = drv->ascendScanData(nodes, count);
            float angle_min = DEG2RAD(0.0f);
            float angle_max = DEG2RAD(359.0f);
            if (op_result == RESULT_OK) {

默认的 launch 文件里 angle_compensate 的参数设定为 true，下面的代码就写的很有问题了。

                if (angle_compensate) {
                    //const int angle_compensate_multiple = 1;
                    const int angle_compensate_nodes_count = 360*angle_compensate_multiple;
                    int angle_compensate_offset = 0;
                    rplidar_response_measurement_node_hq_t angle_compensate_nodes[angle_compensate_nodes_count];
                    memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_hq_t));

                    int i = 0, j = 0;
                    for( ; i < count; i++ ) {
                        if (nodes[i].dist_mm_q2 != 0) {
                            float angle = getAngle(nodes[i]);
                            int angle_value = (int)(angle * angle_compensate_multiple);
                            if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value;
                            for (j = 0; j < angle_compensate_multiple; j++) {

                                int angle_compensate_nodes_index = angle_value-angle_compensate_offset+j;
                                if(angle_compensate_nodes_index >= angle_compensate_nodes_count)
                                    angle_compensate_nodes_index = angle_compensate_nodes_count-1;
                                angle_compensate_nodes[angle_compensate_nodes_index] = nodes[i];
                            }
                        }
                    }
  
                    publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count,
                             start_scan_time, scan_duration, inverted,
                             angle_min, angle_max, max_distance,
                             frame_id);
                }

这里的代码强行将0~360°内扫描的总数 count 的测距值填充到了 360*angle_compensate_multiple 大小的数组中。在 i 循环内直接采用了取整的方式来获得索引

 int angle_value = (int)(angle * angle_compensate_multiple);

(int) 操作取整就是直接截取了实数的整数部分，既没有“四舍五入”，也没有线性插值，非常粗暴，不严谨；而后在内层的 j 循环中就更加可笑，无论循环几次，所有的数值都被赋值了相同的 nodes[i]，这样等于人为造成了激光扫描线的锯齿状。

所以，如果用这个雷达来做激光SLAM的话，请务必将  launch 文件里 angle_compensate 的参数设定为 false。

经过测试，在 angle_compensate 为 false 时，扫描数据为1046，约合0.3°的数据间隔；rostopic 的频率是7.7Hz，可以达到 8000点/秒 的数据量，与官网描述相符。

6、ROS消息 /scan 的坐标定义

在 pulish_scan() 函数中，有如下语句

    bool reversed = (angle_max > angle_min);
    if ( reversed ) {
      scan_msg.angle_min =  M_PI - angle_max;
      scan_msg.angle_max =  M_PI - angle_min;
    } else {
      scan_msg.angle_min =  M_PI - angle_min;
      scan_msg.angle_max =  M_PI - angle_max;
    }

由于原本激光数据是从0~360°，逐点记录的，这里的 if 语句被执行，原本的X轴被旋转了 180°，也就是指向电机的方向；Z方向发生翻转，从顺时针变为逆时针；Y轴按照右手法则来确定。如下所示：

同时，测量数据的序号也必须翻转。代码如下，其中 else 语句被执行：

    bool reverse_data = (!inverted && reversed) || (inverted && !reversed);
    if (!reverse_data) {
        for (size_t i = 0; i < node_count; i++) {
            float read_value = (float) nodes[i].dist_mm_q2/4.0f/1000;
            if (read_value == 0.0)
                scan_msg.ranges[i] = std::numeric_limits<float>::infinity();
            else
                scan_msg.ranges[i] = read_value;
            scan_msg.intensities[i] = (float) (nodes[i].quality >> 2);
        }
    } else {
        for (size_t i = 0; i < node_count; i++) {
            float read_value = (float)nodes[i].dist_mm_q2/4.0f/1000;
            if (read_value == 0.0)
                scan_msg.ranges[node_count-1-i] = std::numeric_limits<float>::infinity();
            else
                scan_msg.ranges[node_count-1-i] = read_value;
            scan_msg.intensities[node_count-1-i] = (float) (nodes[i].quality >> 2);
        }
    }

所以，最先扫描的点序号最大，最后扫描的点序号最小。这与常识不太相符。

总结：

为了让 RPLIDAR_A1 在 ros 中达到8000点/秒，必须如下配置。

①刷 8K Boost 固件

②在 launch 文件中添加参数 <param name="scan_mode"  type="string"  value="Boost">

③在 launch 文件中将参数 angle_compensate 设定为 false

④注意：ros 消息的时间戳是激光扫描的开始时间；序号翻转，与激光器自转顺序相反。

补充：

最近看 cartographer 的代码，其中对 /scan 数据的预处理，将时间戳信息和点云信息一起做了保存，因此又去查看了 sensor_msgs/LaserScan 的定义。

在 header 部分明确说明，时间戳 timestamp 是第一个激光扫描线的时间；time_increment 的值可以用来对激光进行修正。

而在 rplidar_ros 的代码里，时间戳取了 start_scan_time，这是不对的。

        start_scan_time = ros::Time::now();
        op_result = drv->grabScanDataHq(nodes, count);
        end_scan_time = ros::Time::now();

从 sdk 的源码中可以清晰地看到， grabScanDataHq() 函数进入后先进入阻塞 _dataEvt.wait(timeout)；等待数据接收子线程获取到了完整的一个扫描周期后 _dataEvt.set() 解除阻塞，才能进行数据的转存。（实际上从 sdk 源码中可以看到，数据接收线程是一直在进行数据接收的，每次接收的最大数据量是16个激光点，并不是等待一次扫描结束后批量接收）

因此 start_scan_time 并不指向开始扫描的时间，也可能是某次扫描的中间时刻；反而 end_scan_time 却是指向了当前扫描结束的时刻。

为了符合 sensor_msgs/LaserScan 的定义，必须重写 rplidar_ros 的代码，掌握以下原则。按照 ros 惯用的逆时针坐标系，将数据逆序排列，这时 end_scan_time 正好就是第一个扫描线的发生时间，然后将 angle_increment 和 time_increment 都赋为负数，anlge_min=360°，angle_max=0°。