下午拿到一个思岚科技的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°。