ros系统关于rplidarA1/A2的运行节点，程序标注如下，（看了几个小时）有些理解不是很到位，如有错误的地方欢迎指出。
该节点的主要功能就是通过/scan话题消息发布 关于激光雷达的数据 。。。
#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "std_srvs/Empty.h"
#include "rplidar.h"
#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif
#define DEG2RAD(x) ((x)*M_PI/180.)
using namespace rp::standalone::rplidar;
RPlidarDriver * drv = NULL;
void publish_scan(ros::Publisher *pub,
rplidar_response_measurement_node_hq_t *nodes,
size_t node_count, ros::Time start,
double scan_time, bool inverted,
float angle_min, float angle_max,
float max_distance,
std::string frame_id)
{
static int scan_count = 0;
sensor_msgs::LaserScan scan_msg;
scan_msg.header.stamp = start;
scan_msg.header.frame_id = frame_id;
scan_count++;
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;
}
scan_msg.angle_increment =
(scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count-1);
scan_msg.scan_time = scan_time;
scan_msg.time_increment = scan_time / (double)(node_count-1); //测量时间
scan_msg.range_min = 0.15;
scan_msg.range_max = max_distance;//8.0; //设置的最大量程
scan_msg.intensities.resize(node_count);
scan_msg.ranges.resize(node_count);
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);
}
}
pub->publish(scan_msg);
}
bool getRPLIDARDeviceInfo(RPlidarDriver * drv)
{
u_result op_result;
rplidar_response_device_info_t devinfo;
op_result = drv->getDeviceInfo(devinfo);
if (IS_FAIL(op_result)) {
if (op_result == RESULT_OPERATION_TIMEOUT) {
ROS_ERROR("Error, operation time out. RESULT_OPERATION_TIMEOUT! ");
} else {
ROS_ERROR("Error, unexpected error, code: %x",op_result);
}
return false;
}
// print out the device serial number, firmware and hardware version number..
printf("RPLIDAR S/N: ");
for (int pos = 0; pos < 16 ;++pos) {
printf("%02X", devinfo.serialnum[pos]);
}
printf("\n");
ROS_INFO("Firmware Ver: %d.%02d",devinfo.firmware_version>>8, devinfo.firmware_version & 0xFF);
ROS_INFO("Hardware Rev: %d",(int)devinfo.hardware_version);
return true;
}
bool checkRPLIDARHealth(RPlidarDriver * drv) //检查雷达健康状况
{
u_result op_result;
rplidar_response_device_health_t healthinfo;
op_result = drv->getHealth(healthinfo);
if (IS_OK(op_result)) {
ROS_INFO("RPLidar health status : %d", healthinfo.status);
if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
ROS_ERROR("Error, rplidar internal error detected. Please reboot the device to retry.");
return false;
} else {
return true;
}
} else {
ROS_ERROR("Error, cannot retrieve rplidar health code: %x", op_result);
return false;
}
}
bool stop_motor(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res)
{
if(!drv)
return false;
ROS_DEBUG("Stop motor");
drv->stop();
drv->stopMotor();
return true;
}
bool start_motor(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res)
{
if(!drv)
return false;
ROS_DEBUG("Start motor");
drv->startMotor();
drv->startScan(0,1);
return true;
}
static float getAngle(const rplidar_response_measurement_node_hq_t& node)
{
return node.angle_z_q14 * 90.f / 16384.f;
}
/**************************************************************
主函数分界线
*****************************************************************/
int main(int argc, char * argv[])
{
ros::init(argc, argv, "rplidar_node"); //初始化雷达节点
std::string serial_port;
int serial_baudrate = 115200; //设置接受雷达的串口波特率
std::string frame_id;
bool inverted = false; //不选择反转 反转情况就是把激光雷达倒着安装使用，或者特意把激光雷达电机的电源线反接（第一种情况较为合理）
bool angle_compensate = true; //角度补偿 标志位 不知道什么意思
float max_distance = 8.0; //最大量程 即测量半径8m
int angle_compensate_multiple = 1; //it stand of angle compensate at per 1 degree 角度补偿，因为激光雷达的角度信息可能是36.8 这时候要把它补为 37
std::string scan_mode;
ros::NodeHandle nh; //节点句柄
ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000); //消息发布 话题名称为scan 缓存区可存1000个消息
ros::NodeHandle nh_private("~");
nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0"); //这里直接设置串口为USB0 launch文件可以改动它
nh_private.param<int>("serial_baudrate", serial_baudrate, 115200/*256000*/);//ros run for A1 A2, change to 256000 if A3
nh_private.param<std::string>("frame_id", frame_id, "laser_frame"); //nh_private.param都是参数设置，可以从param文件中修改
nh_private.param<bool>("inverted", inverted, false); //默认false
nh_private.param<bool>("angle_compensate", angle_compensate, false);
nh_private.param<std::string>("scan_mode", scan_mode, std::string());
ROS_INFO( "RPLIDAR running on ROS package rplidar_ros. SDK Version:"RPLIDAR_SDK_VERSION""); //启动成功，终端显示雷达的版本信息
u_result op_result;
// create the driver instance
drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT); //这个函数？创建静态接口？不清楚
if (!drv) {
ROS_ERROR("Create Driver fail, exit");
return -2;
}
// make connection...
if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str());
RPlidarDriver::DisposeDriver(drv);
return -1;
}
// get rplidar device info
if (!getRPLIDARDeviceInfo(drv)) {
return -1;
}
// check health... 不健康是什么情况，不清楚
if (!checkRPLIDARHealth(drv)) {
RPlidarDriver::DisposeDriver(drv);
return -1;
}
ros::ServiceServer stop_motor_service = nh.advertiseService("stop_motor", stop_motor);
ros::ServiceServer start_motor_service = nh.advertiseService("start_motor", start_motor); //发布雷达 关于电机的指令 不得不说驱动的程序真的又长又臭
drv->startMotor();
RplidarScanMode current_scan_mode;
if (scan_mode.empty()) {
op_result = drv->startScan(false /* not force scan */, true /* use typical scan mode */, 0, ¤t_scan_mode); //这里好像是检查雷达的驱动方式
//1、强迫扫描 2、正常扫描
} else {
std::vector<RplidarScanMode> allSupportedScanModes; //创建一个未知长度的向量 装元素 元素具体是啥不清楚
op_result = drv->getAllSupportedScanModes(allSupportedScanModes);
if (IS_OK(op_result)) {
_u16 selectedScanMode = _u16(-1);
for (std::vector<RplidarScanMode>::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) {
if (iter->scan_mode == scan_mode) {
selectedScanMode = iter->id; //成功设置模式后 设置id
break;
}
}
if (selectedScanMode == _u16(-1)) { //不支持该模式 这错误没碰过
ROS_ERROR("scan mode `%s' is not supported by lidar, supported modes:", scan_mode.c_str());
for (std::vector<RplidarScanMode>::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) {
ROS_ERROR("\t%s: max_distance: %.1f m, Point number: %.1fK", iter->scan_mode,
iter->max_distance, (1000/iter->us_per_sample));
}
op_result = RESULT_OPERATION_FAIL;
} else {
op_result = drv->startScanExpress(false /* not force scan */, selectedScanMode, 0, ¤t_scan_mode); //这里的几个判断都是关于雷达启动模式
}
}
}
if(IS_OK(op_result))
{
//default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per us 正常USB接口供电，雷达频率只有6.7hz
angle_compensate_multiple = (int)(1000*1000/current_scan_mode.us_per_sample/10.0/360.0);
if(angle_compensate_multiple < 1)
angle_compensate_multiple = 1; //关于角度补偿
max_distance = current_scan_mode.max_distance; //设置最大量程 有个最小量程 0.15m 好像障碍物离雷达太近，检测不出来。
ROS_INFO("current scan mode: %s, max_distance: %.1f m, Point number: %.1fK , angle_compensate: %d", current_scan_mode.scan_mode,
current_scan_mode.max_distance, (1000/current_scan_mode.us_per_sample), angle_compensate_multiple); //输出雷达工作状态和工作参数
}
else
{
ROS_ERROR("Can not start scan: %08x!", op_result);
}
ros::Time start_scan_time; //记录时间 start end
ros::Time end_scan_time;
double scan_duration;
while (ros::ok()) {
rplidar_response_measurement_node_hq_t nodes[360*8]; //0~359度，每行代表一个扫描角度的数据 360*8? 每个信息点都由八个字节确定，具体看串口协议。
size_t count = _countof(nodes);
start_scan_time = ros::Time::now(); //starttime
op_result = drv->grabScanDataHq(nodes, count); //抓取一圈扫描数据序列 360*8个数据序列 。。。
end_scan_time = ros::Time::now(); //endtime
scan_duration = (end_scan_time - start_scan_time).toSec();
if (op_result == RESULT_OK) {
op_result = drv->ascendScanData(nodes, count); //将扫描到的数据按照角度递增排序0----360.....
float angle_min = DEG2RAD(0.0f); //扫描角度 可以在这里修该扫描角度
float angle_max = DEG2RAD(180.0f);
if (op_result == RESULT_OK) {
if (angle_compensate) //angel_compense默认设为false ，所以程序不走这，才会有底下一堆疑问，请直接看else
{
//const int angle_compensate_multiple = 1;
const int angle_compensate_nodes_count = 360*angle_compensate_multiple; //这里的放大倍数为1
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));//初始化，清0
int i = 0, j = 0;
for( ; i < count; i++ )
{
if (nodes[i].dist_mm_q2 != 0) //距离不是0
{
float angle = getAngle(nodes[i]); //获取距离不是零的角度 减少计算？
int angle_value = (int)(angle * angle_compensate_multiple); //这里强制整型 补偿值还是 1 有些废话了 乘法的运算量也蛮大的
if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value; //这里好像废话啊 offset while循环初始值一直为零，
//只修正一个 为了整个框架完善，显得有些冗余
for (j = 0; j < angle_compensate_multiple; j++) { //如果这次的
angle_compensate_nodes[angle_value-angle_compensate_offset+j] = 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);
}
else { //不用角度补偿 就直接走这
int start_node = 0, end_node = 0;
int i = 0;
// find the first valid node and last valid node
while (nodes[i++].dist_mm_q2 == 0);
start_node = i-1;
i = count -1;
while (nodes[i--].dist_mm_q2 == 0);
end_node = i+1;
angle_min = DEG2RAD(getAngle(nodes[start_node]));
angle_max = DEG2RAD(getAngle(nodes[end_node]));
publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +1,
start_scan_time, scan_duration, inverted,
angle_min, angle_max, max_distance,
frame_id);
}
} else if (op_result == RESULT_OPERATION_FAIL) {
// All the data is invalid, just publish them
float angle_min = DEG2RAD(0.0f);
float angle_max = DEG2RAD(359.0f);
publish_scan(&scan_pub, nodes, count,
start_scan_time, scan_duration, inverted,
angle_min, angle_max, max_distance,
frame_id); //话题发布消息 正常情况 程序就会一直走这 想让程序走补偿的情况，把变量给 true 就好了
}
}
ros::spinOnce(); //如果设备正常运行 程序就一直死循环 /scan 话题发布雷达消息
}
// done!
drv->stop();
drv->stopMotor(); //命令停止设备 总体来说，代码有些冗余了。当然这是我假定雷达完全ok的情况下
RPlidarDriver::DisposeDriver(drv); //就算雷达出问题了 ，我也完全不知道啊 。。。哈哈哈
return 0;
}
扫一扫
2524
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
