激光雷达标定及源码分享
本帖最后由 duanzhuohua 于 2024-5-23 12:17 编辑激光雷达安装在本体上时，其相对于本体的位姿有时并不完全与说明书相同。因此标定激光雷达相对本体的方向对于机器人准确导航非常关键。
本实验的目的是获取机器人正前方激光雷达的射线编号。（本实验在Spark-T机器人上进行了测试，您也可以用于其它本体）。
实验场景如附图（凳子垂直于本体的X轴，这样本体正前方的激光射线距离最短）
源码：
/**
* 节点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>=mind && scan_msg->ranges<=maxd)
{
count++;
ROS_INFO("index:%d angle:%f dist:%f x:%f y:%f",
i,bearingi,scan_msg->ranges,
scan_msg->ranges*cos(bearingi),scan_msg->ranges*sin(bearingi));
//该范围内的最小距离值及其编号（方向），场景：在机器人正前方放置一块木板，
//该木板与激光内的扫描面相交的部分距离范围为mind～maxd（环境中其它目标在此之外）
if (minmin>scan_msg->ranges){
minmin=scan_msg->ranges;
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,
scan_msg->ranges*cos(bearingi),
scan_msg->ranges*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);
return 1;
}
min_dist = atof(argv);//获取请求消息的第一个参数，atof函数把字符串转换为浮点数
max_dist = atof(argv);//获取请求消息的第二个参数
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;
}
沙发！:lol
页:
[1]