思岚激光雷达A1/A2 rplidar节点 程序的理解

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, &current_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, &current_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;
}