spark实验5-激光雷达信息处理-标定
上一节
下一节
1、二维激光雷达传感器
测距原理：三角测距(rplidar A1)、TOF（飞行时间）测距（rplidar C1）
https://bucket-download.slamtec.com/9049dfabcba70679557103820a8b6e1dfe938f27/SLAMTEC_rplidar_datasheet_C1_v1.1_cn.pdf
1、二维（单线）激光雷达数据格式
在ROS中，二维激光雷达消息格式为sensor_msgs/LaserScan，可以通过指令rosmsg show sensor_msgs/LaserScan 来显示，如下
[sensor_msgs/LaserScan]:
std_msgs/Header header
uint32 seq
time stamp
string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities
其中，angle_min为最小角度，
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities
一次典型的二维激光扫描：
其中红色点为激光雷达感知到的环境数据，坐标系为激光雷达坐标系。
对应的数据通过指令rostopic echo -n 1 /scan显示如下
header:
seq: 169388
stamp:
secs: 1710639412
nsecs: 218304993
frame_id: "lidar_link"
angle_min: 0.0
angle_max: 6.26573181152
angle_increment: 0.0174532923847
time_increment: 2.3761715795e-07
scan_time: 0.000186054225196
range_min: 0.15000000596
range_max: 5.0
ranges: [4.007999897003174, 3.990000009536743, 4.070000171661377, 4.214000225067139, 4.363999843597412, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 2.0139999389648438, 2.0409998893737793, 2.0179998874664307, 1.1430000066757202, 1.149999976158142, 0.9399999976158142, 0.843999981880188, 0.7760000228881836, inf, inf, inf, 0.6399999856948853, 0.6430000066757202, 0.6549999713897705, 0.6549999713897705, inf, inf, inf, inf, inf, 0.6890000104904175, 0.6919999718666077, 0.6959999799728394, 0.699999988079071, 0.7049999833106995, 0.7110000252723694, 0.7160000205039978, 0.722000002861023, 0.7329999804496765, 0.7450000047683716, 0.7599999904632568, 0.7730000019073486, 0.7730000019073486, 0.7450000047683716, 0.7609999775886536, 0.7749999761581421, 0.8040000200271606, 0.8040000200271606, 0.8149999976158142, 0.8309999704360962, 0.843999981880188, 0.8560000061988831, 0.8669999837875366, 0.8820000290870667, 0.8939999938011169, 0.9160000085830688, 0.9330000281333923, 0.9490000009536743, 0.9660000205039978, 0.9850000143051147, 1.0130000114440918, 1.034999966621399, 1.059000015258789, 1.1059999465942383, inf, inf, 1.1649999618530273, 1.2029999494552612, 1.2350000143051147, 1.2690000534057617, 1.3049999475479126, 1.3569999933242798, 1.4559999704360962, 1.468999981880188, 1.559000015258789, 1.6160000562667847, 1.6640000343322754, inf, inf, inf, inf, 2.177999973297119, 2.2799999713897705, 2.5199999809265137, 2.194000005722046, 2.8519999980926514, 3.947000026702881, 3.938999891281128, 3.930999994277954, 3.9260001182556152, 3.921999931335449, 3.884999990463257, 3.88100004196167, 4.01200008392334, 3.871999979019165, 3.868000030517578, 3.868000030517578, 3.871999979019165, 3.868000030517578, 3.871999979019165, 3.868000030517578, 3.871999979019165, 3.875999927520752, 3.875999927520752, 3.88100004196167, 3.884999990463257, 3.8889999389648438, 3.8929998874664307, 3.930999994277954, 3.9690001010894775, 3.9820001125335693, 3.999000072479248, 3.999000072479248, 4.007999897003174, 4.015999794006348, 4.019999980926514, 4.107999801635742, 4.125999927520752, 4.078999996185303, 4.146999835968018, 4.146999835968018, 4.173999786376953, inf, 3.812000036239624, 3.753000020980835, 3.756999969482422, 3.756999969482422, 3.753000020980835, 3.753000020980835, 3.75, 3.7809998989105225, 4.070000171661377, 3.9600000381469727, 3.828000068664551, 3.7839999198913574, 3.686000108718872, 3.624000072479248, 2.9739999771118164, inf, inf, 2.765000104904175, 2.677999973297119, 2.627000093460083, 2.5950000286102295, 2.565999984741211, 2.509999990463257, 2.499000072479248, 2.4539999961853027, 2.427999973297119, 2.4030001163482666, 2.378999948501587, 2.430000066757202, 2.4839999675750732, 2.5429999828338623, 2.7119998931884766, 2.677999973297119, 2.628000020980835, 2.5999999046325684, 2.575000047683716, 2.563999891281128, 2.5390000343322754, inf, 1.6139999628067017, 1.6030000448226929, 1.590999960899353, 1.5809999704360962, 1.5709999799728394, 1.5579999685287476, 1.5440000295639038, 1.5390000343322754, 1.5299999713897705, 1.5219999551773071, 1.5169999599456787, 1.5089999437332153, 1.5010000467300415, 1.49399995803833, 1.4900000095367432, 1.4889999628067017, 1.4819999933242798, 1.4759999513626099, 1.475000023841858, 1.4769999980926514, 1.503999948501587, inf, 1.4329999685287476, 1.4579999446868896, 1.4630000591278076, 1.4630000591278076, 1.4630000591278076, 1.465000033378601, 1.4630000591278076, 1.4639999866485596, 1.4700000286102295, 1.4709999561309814, 1.4730000495910645, 2.2639999389648438, 2.2660000324249268, 2.2690000534057617, inf, inf, inf, 2.309000015258789, 2.313999891281128, 2.319999933242798, 2.3489999771118164, 2.3559999465942383, 2.374000072479248, 2.384000062942505, 2.390000104904175, 2.421999931335449, 2.430000066757202, 2.438999891281128, 2.4769999980926514, 2.486999988555908, 2.510999917984009, 2.5339999198913574, 2.559999942779541, 2.571000099182129, 2.617000102996826, 2.630000114440918, 2.674999952316284, 2.687999963760376, 2.7179999351501465, 2.755000114440918, 2.7909998893737793, 2.821000099182129, 2.8580000400543213, 2.9130001068115234, 2.936000108718872, 3.010999917984009, 3.0739998817443848, 3.0950000286102295, 3.1640000343322754, 3.2109999656677246, 3.259999990463257, 3.325000047683716, 3.4049999713897705, 3.4860000610351562, 3.5190000534057617, 3.6059999465942383, 3.6530001163482666, 3.808000087738037, inf, 4.007999897003174, 4.01200008392334, 4.133999824523926, 4.2829999923706055, 4.36899995803833, 4.515999794006348, 4.624000072479248, 3.4140000343322754, inf, inf, inf, inf, 2.694000005722046, inf, inf, inf, inf, inf, 2.5409998893737793, 2.5409998893737793, 2.5390000343322754, 2.5390000343322754, 2.5220000743865967, 2.5360000133514404, 2.5220000743865967, 2.5220000743865967, inf, inf, 2.5160000324249268, inf, 4.01200008392334, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 3.88100004196167, 3.571000099182129, 3.2799999713897705, 3.0789999961853027, 2.756999969482422, 2.507999897003174, 2.371000051498413, 2.236999988555908, 2.11899995803833, 2.0179998874664307, 1.8600000143051147, 1.7640000581741333, 1.7680000066757202, 1.7999999523162842, 1.812999963760376, 1.8350000381469727, inf, inf, inf, 1.847000002861023, 1.8890000581741333, 1.9110000133514404, 1.9390000104904175, 1.972000002861023, 2.0220000743865967, 1.9570000171661377, 2.046999931335449, 2.118000030517578, 3.563999891281128, 3.4560000896453857, 1.86899995803833, 3.325000047683716, inf, inf, inf, inf, 3.010999917984009, 2.9719998836517334, 2.8949999809265137, inf, 2.821000099182129, 2.7839999198913574, 2.749000072479248, 2.7019999027252197, 2.7119998931884766, 2.7780001163482666, 2.8429999351501465, 2.9130001068115234, 2.9670000076293945, 3.0409998893737793, 4.204999923706055, 4.146999835968018, 4.083000183105469, 4.083000183105469, 4.065999984741211]
intensities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
激光雷达数据读取方法，核心代码：
void show(double mind,double maxd,const sensor_msgs::LaserScan::ConstPtr& scan_msg)
//显示scan_msg的信息，以及距离在mind-maxd之间的测量
{
ROS_INFO("range_min %f", scan_msg->range_min);
ROS_INFO("range_max %f", scan_msg->range_max);
ROS_INFO("angle_min %f", scan_msg->angle_min);
ROS_INFO("angle_max %f", scan_msg->angle_max);
ROS_INFO("angle_increment %f", scan_msg->angle_increment);
ROS_INFO("scan_msg->ranges.size() %d", scan_msg->ranges.size() );
const std::size_t num_measurements = std::ceil(
(scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment) +1;
ROS_INFO("ceil((scan_msg->angle_max - scan_msg->angle_min)/scan_msg->angle_increment) %d", num_measurements );
ROS_INFO("Ranges between %f to %f", mind, maxd);
double minmin=maxd;
int mini;
double bearingi;
int count=0;
for (unsigned int i = 0; i < scan_msg->ranges.size(); i++)
{
bearingi=scan_msg->angle_min+scan_msg->angle_increment*i;
if (scan_msg->ranges[i]>=mind && scan_msg->ranges[i]<=maxd)
{
count++;
ROS_INFO("index:%d angle:%f dist:%f x:%f y:%f",
i,bearingi,scan_msg->ranges[i],
scan_msg->ranges[i]*cos(bearingi),scan_msg->ranges[i]*sin(bearingi));
//该范围内的最小距离值及其编号（方向），场景：在机器人正前方放置一块木板，
//该木板与激光内的扫描面相交的部分距离范围为mind～maxd（环境中其它目标在此之外）
if (minmin>scan_msg->ranges[i]){
minmin=scan_msg->ranges[i];
mini=i;
}
}
}
if (count>0){
bearingi=scan_msg->angle_min+scan_msg->angle_increment*mini;
ROS_INFO("rightahead_index:%d angle:%f dist:%f x:%f y:%f",
mini,bearingi,scan_msg->ranges[mini],
scan_msg->ranges[mini]*cos(bearingi),
scan_msg->ranges[mini]*sin(bearingi));
} else ROS_INFO("nonething found!");
}
2、激光雷达标定：编写节点，对激光雷达信息进行处理，本实验主要对激光雷达进行标定。就是获得激光雷达相对于机器人的位姿。具体获得机器人正前方的激光雷达射线编号。
前提：要求已经完成了实验2（spark机器人控制（2）--编写节点完成对机器人的控制）。
步骤1：
新建一个终端,进入simple_nodes包（注，此处改为自己所建立的包）
$roscd simple_nodes
注：执行上述指令后，当前目录应该为~/ros_tut/src/simple_nodes$，在此基础上继续执行以下指令
$cd src
步骤2：
将laserscan_calib01.cpp下载（拷贝至该当前目录，拷贝的方法，通过scp命令：台式机与spark联通后，cmd启动台式机终端，转到laserscan_calib01.cpp文件所在的目录，通过下面的指令进行复制：
scp laserscan_calib01.cpp spark@10.42.0.1:~/ros_tut/src/simple_nodes/src
按提示输入yes,密码输入spark ）
拷贝成功后,在spark机器人端通过ls命令可以看到laserscan_calib01.cpp(源代码如下，也可以直接拷贝）
/**
* 节点laserscan_calib01
* 使用LaserScan消息基础示例
* 从scan话题订阅sensor_msgs/LaserScan消息
* 显示第一次订阅的消息
* 显示一定距离范围内的消息（用于激光雷达相对与机器人本体的方位标定）
* 例如，让机器人在空扩的地方，正面向前方的柱子，距离1m~2m的范围的扫描点即为该柱子
* 开发者：段琢华,duanzhuohua@163.com 2021-5-10
*/
#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
//申明sensor_msgs::LaserScan消息类型
#include <cmath>
#include <vector>
int first=1;//只对第一次获得的数据进行处理
double min_dist,max_dist;
void show(double mind,double maxd,const sensor_msgs::LaserScan::ConstPtr& scan_msg)
//显示scan_msg的信息，以及距离在mind-maxd之间的测量
{
ROS_INFO("range_min %f", scan_msg->range_min);
ROS_INFO("range_max %f", scan_msg->range_max);
ROS_INFO("angle_min %f", scan_msg->angle_min);
ROS_INFO("angle_max %f", scan_msg->angle_max);
ROS_INFO("angle_increment %f", scan_msg->angle_increment);
ROS_INFO("scan_msg->ranges.size() %d", scan_msg->ranges.size() );
const std::size_t num_measurements = std::ceil(
(scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment) +1;
ROS_INFO("ceil((scan_msg->angle_max - scan_msg->angle_min)/scan_msg->angle_increment) %d", num_measurements );
ROS_INFO("Ranges between %f to %f", mind, maxd);
double minmin=maxd;
int mini;
double bearingi;
int count=0;
for (unsigned int i = 0; i < scan_msg->ranges.size(); i++)
{
bearingi=scan_msg->angle_min+scan_msg->angle_increment*i;
if (scan_msg->ranges[i]>=mind && scan_msg->ranges[i]<=maxd)
{
count++;
ROS_INFO("index:%d angle:%f dist:%f x:%f y:%f",
i,bearingi,scan_msg->ranges[i],
scan_msg->ranges[i]*cos(bearingi),scan_msg->ranges[i]*sin(bearingi));
//该范围内的最小距离值及其编号（方向），场景：在机器人正前方放置一块木板，
//该木板与激光内的扫描面相交的部分距离范围为mind～maxd（环境中其它目标在此之外）
if (minmin>scan_msg->ranges[i]){
minmin=scan_msg->ranges[i];
mini=i;
}
}
}
if (count>0){
bearingi=scan_msg->angle_min+scan_msg->angle_increment*mini;
ROS_INFO("rightahead_index:%d angle:%f dist:%f x:%f y:%f",
mini,bearingi,scan_msg->ranges[mini],
scan_msg->ranges[mini]*cos(bearingi),
scan_msg->ranges[mini]*sin(bearingi));
} else ROS_INFO("nonething found!");
}
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_msg)
{
//回调函数，接收sensor_msgs::LaserScan消息
std::vector<double> scan_ranges_doubles(scan_msg->ranges.begin(), scan_msg->ranges.end());
if (!first) return;
first=0;
show(min_dist,max_dist,scan_msg);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserscan_calib01");
if (argc <= 2)
{//命令行中处理命令本身，还要两个额外的参数min_dist,max_dist
//如果参数不够，提示使用方法并退出
ROS_INFO("usage: %s min_dist,max_dist",argv[0]);
return 1;
}
min_dist = atof(argv[1]);//获取请求消息的第一个参数，atof函数把字符串转换为浮点数
max_dist = atof(argv[2]);//获取请求消息的第二个参数
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("scan", 10, scanCallback);//初始化订阅者对象
ros::Rate loop_rate(10);
//设置发布频率
while (ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
$cd ..
$gedit CMakeLists.txt
将下面的两行加入到文件末尾，然后保存文件并退出编辑
add_executable(laserscan_calib01 src/laserscan_calib01.cpp)
target_link_libraries(laserscan_calib01 ${catkin_LIBRARIES})
$cd ~/ros_tut
$catkin_make
构建成功后继续后面的步骤，否则根据提示信息修改源程序，再通过步骤1来构建。
步骤2：测试节点
新开一个终端，启动机器人建图节点
$roslaunch spark_slam 2d_slam.launch
确保机器人在一个比较空阔的地方，周围1米内没有障碍。
然后将一个长方形障碍垂直于机器人X轴，放置于机器人前方（约0.5m处），另开一个终端执行下面指令
$rosrun simple_nodes laserscan_calib01 0.2 1
记录获得的结果。据此分析机器人正前方激光雷达射线的编号。
实验场景：
在spark-9ca7设备上实验结果如下：
[ INFO] [1712624673.114900911]: rightahead_index:156 angle:-0.411295 dist:0.393000 x:0.360225 y:-0.157120
即机器人正前方的射线编号为156号（在ranges数组中的下标）
3、根据标定结果设置机器人模型参数
激光雷达数据如下：
$ rostopic echo /scan -n 1
header:
seq: 58090
stamp:
secs: 1712626839
nsecs: 77204753
frame_id: "lidar_link"
angle_min: -3.14159274101
angle_max: 3.14159274101
angle_increment: 0.0175019092858
time_increment: 1.39922036624e-07
scan_time: 0.000184417251148
range_min: 0.15000000596
range_max: 5.0
根据标定结果以及上述信息，计算获得激光雷达相对于本体的旋转角度（绕z轴）：
156*0.017502= 2.73
3.14-2.73=0.41
在模型文件
~/spark/src/spark/spark_description/urdf/sensors/spark_lidar.urdf
中设置该值如下：
<!-- lidar -->
<robot name="lidar_frame">
<link name="lidar_link"/>
<joint name="lidar_joint" type="fixed">
<origin xyz="0 0 0.08" rpy="0 0 0.41" />
<parent link="base_link"/>
<child link="lidar_link"/>
</joint>
</robot>
修改了模型参数后，重新启动SLAM（roslaunch spark_slam 2d_slam.launch）,结果如下
其中，坐标系为本体坐标系，可以看到，前方的凳子于本体的X轴垂直，如实际的场景一致。
