激光雷达标定及源码分享

duanzhuohua

激光雷达安装在本体上时，其相对于本体的位姿有时并不完全与说明书相同。因此标定激光雷达相对本体的方向对于机器人准确导航非常关键。

本实验的目的是获取机器人正前方激光雷达的射线编号。（本实验在Spark-T机器人上进行了测试，您也可以用于其它本体）。
实验场景如附图（凳子垂直于本体的X轴，这样本体正前方的激光射线距离最短）
源码：

1. /**
   
2. * 节点laserscan_calib01
   
3. * 使用LaserScan消息基础示例
   
4. * 从scan话题订阅sensor_msgs/LaserScan消息
   
5. * 显示第一次订阅的消息
   
6. * 显示一定距离范围内的消息（用于激光雷达相对与机器人本体的方位标定）
   
7. * 例如，让机器人在空扩的地方，正面向前方的柱子，距离1m~2m的范围的扫描点即为该柱子  
   
8. 
   
9. * 开发者：段琢华,duanzhuohua@163.com 2021-5-10
   
10. */
    
11. 
    
12. 
    
13. #include "ros/ros.h"
    
14. #include "sensor_msgs/LaserScan.h"
    
15. //申明sensor_msgs::LaserScan消息类型
    
16. #include <cmath>
    
17. #include <vector>
    
18. 
    
19. int first=1;//只对第一次获得的数据进行处理
    
20. double min_dist,max_dist;
    
21. 
    
22. void show(double mind,double maxd,const sensor_msgs::LaserScan::ConstPtr& scan_msg)
    
23. //显示scan_msg的信息，以及距离在mind-maxd之间的测量
    
24. {
    
25.   
    
26.   ROS_INFO("range_min %f", scan_msg->range_min);  
    
27.   ROS_INFO("range_max %f", scan_msg->range_max);
    
28.   ROS_INFO("angle_min %f", scan_msg->angle_min);  
    
29.   ROS_INFO("angle_max %f", scan_msg->angle_max);
    
30.   ROS_INFO("angle_increment %f", scan_msg->angle_increment);
    
31.   ROS_INFO("scan_msg->ranges.size()  %d", scan_msg->ranges.size() );
    
32.   const std::size_t num_measurements = std::ceil(
    
33.       (scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment) +1;
    
34.   ROS_INFO("ceil((scan_msg->angle_max - scan_msg->angle_min)/scan_msg->angle_increment)  %d", num_measurements );
    
35. 
    
36.   ROS_INFO("Ranges between %f to %f", mind, maxd);  
    
37.   double minmin=maxd;
    
38.   int mini;
    
39.   double bearingi;
    
40.   int count=0;
    
41. 
    
42.   for (unsigned int i = 0; i < scan_msg->ranges.size(); i++)
    
43.   {
    
44.    bearingi=scan_msg->angle_min+scan_msg->angle_increment*i;
    
45.    if (scan_msg->ranges[i]>=mind && scan_msg->ranges[i]<=maxd)
    
46.       {
    
47.         count++;
    
48.         ROS_INFO("index:%d angle:%f dist:%f x:%f y:%f",
    
49.              i,bearingi,scan_msg->ranges[i],
    
50.              scan_msg->ranges[i]*cos(bearingi),scan_msg->ranges[i]*sin(bearingi));
    
51.         //该范围内的最小距离值及其编号（方向），场景：在机器人正前方放置一块木板，
    
52.         //该木板与激光内的扫描面相交的部分距离范围为mind～maxd（环境中其它目标在此之外）
    
53.         if (minmin>scan_msg->ranges[i]){
    
54.            minmin=scan_msg->ranges[i];
    
55.            mini=i;
    
56.          }
    
57.         
    
58.       }   
    
59.   }
    
60.   if (count>0){
    
61.     bearingi=scan_msg->angle_min+scan_msg->angle_increment*mini;
    
62.     ROS_INFO("rightahead_index:%d angle:%f dist:%f x:%f y:%f",
    
63.              mini,bearingi,scan_msg->ranges[mini],
    
64.              scan_msg->ranges[mini]*cos(bearingi),
    
65.              scan_msg->ranges[mini]*sin(bearingi));
    
66.   } else ROS_INFO("nonething found!");
    
67.   
    
68. 
    
69. }
    
70. 
    
71. 
    
72. void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_msg)
    
73. {
    
74.   //回调函数，接收sensor_msgs::LaserScan消息
    
75.   std::vector<double> scan_ranges_doubles(scan_msg->ranges.begin(), scan_msg->ranges.end());
    
76. 
    
77.   if (!first) return;
    
78.   first=0;
    
79.   show(min_dist,max_dist,scan_msg);
    
80.   
    
81. }
    
82. 
    
83. int main(int argc, char **argv)
    
84. {
    
85.   ros::init(argc, argv, "laserscan_calib01");
    
86.   if (argc <= 2)
    
87.   {//命令行中处理命令本身，还要两个额外的参数min_dist,max_dist
    
88.    //如果参数不够，提示使用方法并退出
    
89.     ROS_INFO("usage: %s min_dist,max_dist",argv[0]);
    
90.     return 1;
    
91.   }
    
92.   
    
93.   min_dist = atof(argv[1]);//获取请求消息的第一个参数，atof函数把字符串转换为浮点数
    
94.   max_dist = atof(argv[2]);//获取请求消息的第二个参数
    
95.   ros::NodeHandle n;
    
96. 
    
97.   ros::Subscriber sub = n.subscribe("scan", 10, scanCallback);//初始化订阅者对象   
    
98.    
    
99.   ros::Rate loop_rate(10);
    
100.   //设置发布频率
     
101.   while (ros::ok())
     
102.   {   
     
103.    
     
104.     ros::spinOnce();
     
105.     loop_rate.sleep();
     
106.   }
     
107.   
     
108.   return 0;
     
109. }

复制代码

NXROS小助理

沙发！