ROS——RPLIDAR A1 SDK详解

  使用 RPLIDARD 的 SDK 其实重点在于看懂client.cpp和node.cpp两个sample代码，因此在这里我们讲从这里入手学习 RPLIDAR A1 的SDK。

在代码中比较重要的几个文件分别是：
1.rplidar_ros/sdk/include/rplidar_cmd.h该文件中主要定义了与 RPLIDAR 通讯时使用的各种数据的类型。
2.rplidar_ros/sdk/include/rplidar_driver.h该文件中主要声明SDK的各种函数的使用方法以及参数。
3.rplidar_ros/sdk/include/rptypes.h该文件中主要定义了函数返回参数的类型，也可以理解为异常参数的类型。

下面分析node.cpp文件
个人觉得先理解以下几个函数比较容易上手

bool getRPLIDARDeviceInfo(RPlidarDriver * drv)
{
    u_result     op_result;//函数返回值
    rplidar_response_device_info_t devinfo;
/*
在rplida_cmd.h文件中可以找到rplidar_response_device_info_t定义如下:
typedef struct _rplidar_response_device_info_t {
    _u8   model;
    _u16  firmware_version;
    _u8   hardware_version;
    _u8   serialnum[16];
} __attribute__((packed)) rplidar_response_device_info_t;
从上面的定义可以看出，在这里定义了设备信息，包括序列号，固件版本，设备型号等。
*/
    op_result = drv->getDeviceInfo(devinfo);
/*
在rplida_driver.h文件中可以找到getDeviceInfo函数的使用说明:
Get the device information of the RPLIDAR include the serial number, firmware version, device model etc.

\param info          The device information returned from the RPLIDAR

\param timeout       The operation timeout value (in millisecond) for the serial port communication  
virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT) = 0;
*/
    if (IS_FAIL(op_result)) {//通过函数返回值判断是否正常
        if (op_result == RESULT_OPERATION_TIMEOUT) {
            fprintf(stderr, "Error, operation time out.\n");
        } else {
            fprintf(stderr, "Error, unexpected error, code: %x\n", 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"
           "Firmware Ver: %d.%02d\n"
           "Hardware Rev: %d\n"
           , devinfo.firmware_version>>8
           , devinfo.firmware_version & 0xFF
           , (int)devinfo.hardware_version);
    return true;
}

下面分析checkRPLIDARHealth函数

bool checkRPLIDARHealth(RPlidarDriver * drv)
{
    u_result     op_result;//函数返回值
    rplidar_response_device_health_t healthinfo;
/*
在rplida_cmd.h文件中可以找到rplidar_response_device_health_t定义如下:
typedef struct _rplidar_response_device_health_t {
    _u8   status;//从RPLIDAR返回的运行状况信息
    _u16  error_code;//错误信息
} __attribute__((packed)) rplidar_response_device_health_t;
*/
    op_result = drv->getHealth(healthinfo);
/*
在rplida_driver.h文件中可以找到getHealth函数的使用说明:
Retrieve the health status of the RPLIDAR
The host system can use this operation to check whether RPLIDAR is in the self-protection mode.

\param health        The health status info returned from the RPLIDAR

\param timeout       The operation timeout value (in millisecond) for the serial port communication     
    virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT) = 0;
*/
    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;
    }
}

RPLIDAR 开启自传（电机启动通过皮带带动激光雷达转动）

bool start_motor(std_srvs::Empty::Request &req,
                               std_srvs::Empty::Response &res)
{
  if(!drv)
       return false;
  ROS_DEBUG("Start motor");
  drv->startMotor();
  /*
  在rplidar_driver.h中可以找到startMotor()函数的定义
  Start RPLIDAR's motor when using accessory board
    virtual u_result startMotor() = 0;
  */
  drv->startScan();;
  return true;
}

RPLIDAR 停止自传

bool stop_motor(std_srvs::Empty::Request &req,
                               std_srvs::Empty::Response &res)
{
  if(!drv)
       return false;

  ROS_DEBUG("Stop motor");
  drv->stop();
  /*
  在rplidar_driver.h中可以找到stop()函数的定义
  Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated

  \param timeout       The operation timeout value (in millisecond) for the serial port communication 
    virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT) = 0;
  */
  drv->stopMotor();
  /*
  在rplidar_driver.h中可以找到stopMotor()函数的定义
  Stop RPLIDAR's motor when using accessory board
    virtual u_result stopMotor() = 0;
  */
  return true;
}

下面分析主函数

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;

    ros::NodeHandle nh;
    ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);
    ros::NodeHandle nh_private("~");
    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);

    printf("RPLIDAR running on ROS package rplidar_ros\n"
           "SDK Version: "RPLIDAR_SDK_VERSION"\n");

    u_result     op_result;

    // create the driver instance
    drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);//通过 SDK 创建一个对应的 RPlidarDriver 实例，用来跟RPLIDAR通信

    if (!drv) {//连接失败
        fprintf(stderr, "Create Driver fail, exit\n");
        return -2;
    }

    // make connection...
    if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {//打开串口与APLIDAR连接
        fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n"
            , 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();//默认使用是能DTR启动电机旋转
    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];//0~359度，每行代表一个扫描角度的数据
        size_t   count = _countof(nodes);//一共有多少数据，应该是为了方便后期修改，所以定义了_countof()而不是直接给count赋值

        start_scan_time = ros::Time::now();//获取扫描开始时间
        op_result = drv->grabScanData(nodes, count);//grabScanDat（）抓取一圈扫描数据序列
        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);//ascendScanData()讲扫描到的数据按照角度递增排序

            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;
                    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) {
                            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);//从头开始寻找第一个不为0的数据
                    start_node = i-1;
                    i = count -1;
                    while (nodes[i--].distance_q2 == 0);//从尾开始寻找第一个不为0的数据
                    end_node = i+1;

                    angle_min = DEG2RAD((float)(nodes[start_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
                    /*通信协议中写到需要数据需要除以64，RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT=1，在这里右移一位是因为协议中数据是小端存储模式，在angle_q6_checkbit数据存储时最低位是C，即得到数据时需要将angle_q6_checkbit整体右移一位*/
                    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();
    }

    // done!
    drv->stop();
    drv->stopMotor();
    RPlidarDriver::DisposeDriver(drv);
    return 0;
}

  client.cpp文件订阅node.cpp中接收的数据。可能会有朋友问为何没有直接在node.cpp中直接将读到的数据输出，个人感觉应该是为了体现ROS发布订阅的特点，同时方便移植，如果想在其他节点使用数据，只需要订阅node中的数据即可。

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

#define RAD2DEG(x) ((x)*180./M_PI)  //弧度制数据转换为角度

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)//sensor_msgs命名空间下，LaserScan结构体里面的
{
    int count = scan->scan_time / scan->time_increment;
    ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count);
    ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));

    for(int i = 0; i < count; i++) {
        float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
        ROS_INFO(": [%f, %f]", degree, scan->ranges[i]);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "rplidar_node_client");
    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback);

    ros::spin();

    return 0;
}