RPlidar学习（四）——源码分析

一丶数据类型分析

Sensor_msgs/LaserScan:

header(stamp[第一条射线的获得时间]+frame_id[围绕z轴测量的角度])

angle_min(起始角度)

angle_max(结束角度)

angle_increment(角距离)

time_increment(测量时间)

scan_time(扫描时间)

range_min(有效范围最小值)

range_max(有效范围最大值)

ranges(有效范围)

intensities(强度)

二丶rplidarNode源码分析

（1）头文件引用以及一些变量定义

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "std_srvs/Empty.h"

#include "rplidar.h" //RPLIDAR standard sdk, all-in-one header

#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif
//避免源库中没有_countof函数，定义此函数为返回数组元素个数
#define DEG2RAD(x) ((x)*M_PI/180.)

using namespace rp::standalone::rplidar;

RPlidarDriver * drv = NULL;//RPlidarDriver是在rplidar_driver.h头文件中定义好的一个类

（2）雷达发布消息子函数

void publish_scan(ros::Publisher *pub,//对象
                  rplidar_response_measurement_node_t *nodes,//雷达数据（该数据类型在rplidarcmd.h被定义，包含sync_quality，angle_q6_checkbit，distance_q2）
                  size_t node_count,ros::Time start,//次数(size_t是标准C库中定义的，应为unsigned int，在64位系统中为 long unsigned int)，起始时间(该数据类型在ros.h中被定义为获取时间）
                  double scan_time, bool inverted,//扫描时间，是否转换
                  float angle_min, float angle_max,////角度范围
                  std::string frame_id)//id
{
    static int scan_count = 0;（定义一个计数变量）
    sensor_msgs::LaserScan scan_msg;//用ros已有的雷达数据模型定义扫描所发出的消息

    scan_msg.header.stamp = start;//扫描起始时间
    scan_msg.header.frame_id = frame_id;//序列id
    scan_count++;//计数

    scan_msg.angle_min =  M_PI - angle_min;//M_PI为3.1415926，即180度

    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 = 6.;//最大的范围

    scan_msg.intensities.resize(node_count);//信号质量
    scan_msg.ranges.resize(node_count);//范围
    if (!inverted) { // assumes scan window at the top
        for (size_t i = 0; i < node_count; i++) {
            float read_value = (float) nodes[i].distance_q2/4.0f/1000;
            if (read_value == 0.0)
                scan_msg.ranges[i] = std::numeric_limits<float>::infinity();//返回编译器允许的float类型无穷大
            else
                scan_msg.ranges[i] = read_value;
            scan_msg.intensities[i] = (float) (nodes[i].sync_quality >> 2);
        }
    } else {
        for (size_t i = 0; i < node_count; i++) {
            float read_value = (float)nodes[i].distance_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].sync_quality >> 2);//信号质量信息
        }
    }

    pub->publish(scan_msg);

}

（2）检查rplidar设备健康信息

bool checkRPLIDARHealth(RPlidarDriver * drv)
{
    u_result     op_result;
    rplidar_response_device_health_t healthinfo;

    op_result = drv->getHealth(healthinfo);
    if (IS_OK(op_result)) {
        printf("RPLidar health status : %d\n", healthinfo.status);
        
        if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
            fprintf(stderr, "Error, rplidar internal error detected."
                            "Please reboot the device to retry.\n");
            return false;
        } else {
            return true;
        }

    } else {
        fprintf(stderr, "Error, cannot retrieve rplidar health code: %x\n",
                        op_result);
        return false;
    }

}

（3）雷达开始/结束函数

bool stop_motor(std_srvs::Empty::Request &req,
                std_srvs::Empty::Response &res)//创建一个服务
{
  if(!drv)
    return false;

  ROS_DEBUG("Stop motor");//ROS_DEBUG的信息在程序运行时不直接显示的Terminal,须利用rxconsole查看，一些调试中需要观察的信息用ROS_DEBUG显示
  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();;
  return true;

}

（4）main函数分析

int main(int argc, char * argv[]) {
    ros::init(argc, argv, "rplidar_node");//启动该节点并设置其名称

    std::string serial_port; //串口号
    int serial_baudrate = 115200;//串口波特率
    std::string frame_id;//id
    bool inverted = false;//是否转换标志位
    bool angle_compensate = true;//角度补偿标志位

    ros::NodeHandle nh;//下面是设置节点进程的句柄
    ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);//将节点设置成发布者，并将所发布的主题和类型的名称告知节点管理器。第一个参数是缓冲区大小，名为scan，如果主题发布速度较快，则将缓冲区设置成1000个消息）

    ros::NodeHandle nh_private("~");

//launch可以进行一些初始化

    nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0");
    nh_private.param<int>("serial_baudrate", serial_baudrate, 115200);
    nh_private.param<std::string>("frame_id", frame_id, "laser_frame");
    nh_private.param<bool>("inverted", inverted, "false");
    nh_private.param<bool>("angle_compensate", angle_compensate, "true");
    
    u_result     op_result;
   
    // create the driver instance创建静态接口
    drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
    
    if (!drv)//创建失败退出-2 {
        fprintf(stderr, "Create Driver fail, exit\n");
        return -2;
    }

    // make connection...建立链接，失败返回-1
    if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
        fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n"
            , serial_port.c_str());
        RPlidarDriver::DisposeDriver(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);
    
    // start scan...开电机进行扫描

    drv->startScan();

//记录扫描起始时间，持续时间

    ros::Time start_scan_time;
    ros::Time end_scan_time;
    double scan_duration;

    while (ros::ok()) {

//储存信号质量，角度信息，距离信息的变量

        rplidar_response_measurement_node_t nodes[360*2];

        size_t   count = _countof(nodes);//得到数组长度

//得到扫描时间，雷达数据

        start_scan_time = ros::Time::now();
        op_result = drv->grabScanData(nodes, count);
        end_scan_time = ros::Time::now();

        scan_duration = (end_scan_time - start_scan_time).toSec() * 1e-3;

//成功扫描

        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) {

//进行角度补偿

                if (angle_compensate) {
                    const int angle_compensate_nodes_count = 360;
                    const int angle_compensate_multiple = 1;

                    int angle_compensate_offset = 0;

//初始化，清0

                    rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count];
                    memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t));
                    int i = 0, j = 0;
                    for( ; i < count; i++ ) {

                        if (nodes[i].distance_q2 != 0)//距离不是0 {

//计算当前角度，如果角度比上次小则，记录

                            float angle = (float)((nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
                            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++)//只修正一个{
                                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,
                             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++].distance_q2 == 0);
                    start_node = i-1;
                    i = count -1;
                    while (nodes[i--].distance_q2 == 0);
                    end_node = i+1;

                    angle_min = DEG2RAD((float)(nodes[start_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
                    angle_max = DEG2RAD((float)(nodes[end_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);

                    publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +1,
                             start_scan_time, scan_duration, inverted,  
                             angle_min, angle_max,
                             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,
                             frame_id);
            }
        }

        ros::spinOnce();//如果有一个订阅者出现，ROS就会更新并读取所有主题
    }
    
    // done!
    RPlidarDriver::DisposeDriver(drv);
    return 0;

}

以上是rplidarNode的代码解释，基本上每个雷达程序都需要以此为基础代码，必须引用。。。