Dora-rs 机器人框架学习教程（4）——速腾激光雷达驱动

目标：编写一个C++节点读取速腾雷达数据。
程序的基本思路是新建一个C++ dora节点，在该节点中调用rslidar类成员函数读取雷达数据，并将数据序列化发布到dora中去。
本文使用的激光雷达是速腾 RSHELIOS 32线激光雷达

这里我们也参考了之前有大佬写的驱动程序的思路 github

1、rslidar 雷达官方驱动

这里需要用到速腾官方的驱动库rs_driver，因此对使用的程序简单介绍。
在 rs_driver 库中demo文件夹下提供了一个雷达数据读取范例程序 demo_online.cpp ，可在demo文件夹下创建build目录，对 demo_online.cpp 进行编译，生成可执行文件，以测试激光雷达的连线是否正确。

mkdir build
cd build
cmake ..
make
./demo_online

若出现如下信息则表示激光雷达与电脑数据链路正确。

程序上主要涉及到使用rs_driver库中的 **RSDriverParam ** 参数类 和 数据读取函数 processCloud（）

1、RSDriverParam 类涉及激光雷达类型和端口

2、函数 void processCloud（void）主要是获取激光雷达数据

2、rslidar dora 框架驱动

2.1 驱动代码

新建 rslidar_driver.cc文件，将以下内容复制到文件中
（注意将include 路径 /home/dora/dora_project/dora/apis/c/node/node_api.h"替换为你电脑上的路径）

extern "C"
{
#include "/home/dora/dora_project/dora/apis/c/node/node_api.h"   
#include "/home/dora/dora_project/dora/apis/c/operator/operator_api.h"
#include "/home/dora/dora_project/dora/apis/c/operator/operator_types.h"
}

#include <iostream>
#include <vector>


// rs lidar driver
#include <rs_driver/api/lidar_driver.hpp>

#ifdef ENABLE_PCL_POINTCLOUD
#include <rs_driver/msg/pcl_point_cloud_msg.hpp>
#else
#include <rs_driver/msg/point_cloud_msg.hpp>
#endif
typedef PointXYZI PointT;
typedef PointCloudT<PointT> PointCloudMsg;

using namespace robosense::lidar;

SyncQueue<std::shared_ptr<PointCloudMsg>> free_cloud_queue;
SyncQueue<std::shared_ptr<PointCloudMsg>> stuffed_cloud_queue;
//
// @brief point cloud callback function. The caller should register it to the lidar driver.
//        Via this fucntion, the driver gets an free/unused point cloud message from the caller.
// @param msg  The free/unused point cloud message.
//
std::shared_ptr<PointCloudMsg> driverGetPointCloudFromCallerCallback(void)//从free队列里面取
{
  // Note: This callback function runs in the packet-parsing/point-cloud-constructing thread of the driver, 
  //       so please DO NOT do time-consuming task here.
  std::shared_ptr<PointCloudMsg> msg = free_cloud_queue.pop();
  if (msg.get() != NULL)
  {
    return msg;
  }

  return std::make_shared<PointCloudMsg>();
}

//
// @brief point cloud callback function. The caller should register it to the lidar driver.
//        Via this function, the driver gets/returns a stuffed point cloud message to the caller. 
// @param msg  The stuffed point cloud message.
//
void driverReturnPointCloudToCallerCallback(std::shared_ptr<PointCloudMsg> msg)//填到一个新的队列里
{
  // Note: This callback function runs in the packet-parsing/point-cloud-constructing thread of the driver, 
  //       so please DO NOT do time-consuming task here. Instead, process it in caller's own thread. (see processCloud() below)
  stuffed_cloud_queue.push(msg);
}

//
// @brief exception callback function. The caller should register it to the lidar driver.
//        Via this function, the driver inform the caller that something happens.
// @param code The error code to represent the error/warning/information
//
std::string exceptionCallback(const Error& code)//错误报告
{
  // Note: This callback function runs in the packet-receving and packet-parsing/point-cloud_constructing thread of the driver, 
  //       so please DO NOT do time-consuming task here.
  RS_WARNING << code.toString() << RS_REND;
  return "";
}

bool to_exit_process = false;
void processCloud(void)
{
  while (!to_exit_process)
  {
    std::shared_ptr<PointCloudMsg> msg = stuffed_cloud_queue.popWait();//这个popwait函数是一个线程安全的队列pop
    if (msg.get() == NULL)
    {
      continue;
    }

    // Well, it is time to process the point cloud msg, even it is time-consuming.
    RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND;

#if 0
    for (auto it = msg->points.begin(); it != msg->points.end(); it++)
    {
      std::cout << std::fixed << std::setprecision(3) 
                << "(" << it->x << ", " << it->y << ", " << it->z << ", " << (int)it->intensity << ")" 
                << std::endl;
    }
#endif

    free_cloud_queue.push(msg);//这里是说，上面那个#if里面的东西已经把这个点云处理完了，东西都取出来了，那这个点云实例（占内存的）我们就可以重复利用了，就空闲了，把它放入待使用区（free区）
  }
}
int run(void *dora_context)
{
    unsigned char counter = 0;

    //for (int i = 0; i < 20; i++)
    to_exit_process = false;
    while(!to_exit_process)
    {
        void *event = dora_next_event(dora_context);
        if (event == NULL)
        {
            printf("[c node] ERROR: unexpected end of event\n");
            return -1;
        }

        enum DoraEventType ty = read_dora_event_type(event);

        if (ty == DoraEventType_Input)
        {
            // copy from rslidar driver
            #if 1
            Vec_uint8_t result;
            
                std::shared_ptr<PointCloudMsg> msg = stuffed_cloud_queue.popWait();//这个popwait函数是一个线程安全的队列pop
                if (msg.get() == NULL)
                {
                    continue;
                }

                // Well, it is time to process the point cloud msg, even it is time-consuming.
                RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND;

                #if 0
                    for (auto it = msg->points.begin(); it != msg->points.end(); it++)
                    {
                    std::cout << std::fixed << std::setprecision(3) 
                                << "(" << it->x << ", " << it->y << ", " << it->z << ", " << (int)it->intensity << ")" 
                                << std::endl;
                    }
                #endif

                //free_cloud_queue.push(msg);//这里是说，上面那个#if里面的东西已经把这个点云处理完了，东西都取出来了，那这个点云实例（占内存的）我们就可以重复利用了，就空闲了，把它放入待使用区（free区）
                
                if (sizeof(PointT) <= 16)
                {
                    RS_MSG << "sizeof(PointT) <= 16 " << RS_REND;
                    size_t cloudSize = (((msg->points.size()) + 1) * 16);  // 4byte for message seq, 4bytes empty, 8byte for timestamp,
                                                                        // others for points
                    u_int8_t* bytePointCloud = (u_int8_t*)(new PointT[cloudSize / sizeof(PointT)]);
                    
                    u_int32_t* seq = (u_int32_t*)bytePointCloud;
                    *seq = msg->seq;
                    double* timestamp = (double*)(bytePointCloud + 8);
                    *timestamp = msg->timestamp;
                    // PointT* point = (PointT*)(bytePointCloud + 16);
                    // std::vector<PointT>::iterator pointPtr = msg->points.begin();
                    // for (int i = 0; i < msg->points.size(); ++i){
                    //   *point++ = pointPtr[i];
                    // }
                    memcpy(bytePointCloud+16,&(msg->points[0]),cloudSize-16);

                    free_cloud_queue.push(msg);
                    
                    result.ptr = bytePointCloud;
                    result.len = cloudSize;
                    result.cap = cloudSize;
                    //return result;
                }
                else if (sizeof(PointT) == 24)
                {                                   // just write them here, I didn't test it
                    size_t cloudSize =
                        ((msg->points.size()) * 24);  // 24 bytes for each point, 4*3 bytes for coordinates, 1 byte for intensity, 1
                                                    // byte because of byte aligned 2 bytes for rings, 8 bytes for timestamp

                    u_int8_t* bytePointCloud = (u_int8_t*)new PointT[cloudSize / sizeof(PointT)];
                    memcpy(bytePointCloud,&(msg->points[0]),cloudSize);
                    // PointT* point = (PointT*)(bytePointCloud);
                    // std::vector<PointT>::iterator pointPtr = msg->points.begin();
                    // for (int i = 0; i < msg->points.size(); ++i)
                    // {
                    //   *(point++) = pointPtr[i];
                    // }

                    free_cloud_queue.push(msg);
                    //Vec_uint8_t result;
                    result.ptr = bytePointCloud;
                    result.len = cloudSize;
                    result.cap = cloudSize;
                    //return result;
                }
                else
                {
                    std::cerr << "point size error! This may happen when your system is not byte aligned!";
                    result = { .ptr = NULL };
                    result.len = 0;
                    result.cap = 0;
                    //return result;
                }
            
            #endif
 
            char* output_data = (char *)result.ptr;
            size_t output_data_len = result.len;
            counter += 1;

            std::string out_id = "pointcloud";
            size_t data_len = 1 ;
            int resultend=dora_send_output(dora_context, &out_id[0], out_id.length(), output_data, output_data_len);
           
           std::cout
                << "dora_send_output: out_id "<<out_id<< "  out_data_len: "<<output_data_len<<std::endl;
                
            if (resultend != 0)
            {
                std::cerr << "failed to send output" << std::endl;
                return 1;
            }
        }
        else if (ty == DoraEventType_Stop)
        {
            printf("[c node] received stop event\n");
        }
        else
        {
            printf("[c node] received unexpected event: %d\n", ty);
        }

        free_dora_event(event);
    }
    return 0;
}

int main()
{
    std::cout << "rslidar driver for dora " << std::endl;

    RSDriverParam param;                  ///< Create a parameter object
    param.input_type = InputType::ONLINE_LIDAR; //输入类型
    param.input_param.msop_port = 6699;   ///< Set the lidar msop port number, the default is 6699
    param.input_param.difop_port = 7788;  ///< Set the lidar difop port number, the default is 7788
    param.lidar_type = LidarType::RSHELIOS;   ///< Set the lidar type. Make sure this type is correct雷达类型
    param.print();//控制台输出参数信息

    LidarDriver<PointCloudMsg> driver;               ///< Declare the driver object
    driver.regPointCloudCallback(driverGetPointCloudFromCallerCallback, driverReturnPointCloudToCallerCallback); ///< Register the point cloud callback functions
    driver.regExceptionCallback(exceptionCallback);  ///< Register the exception callback function
    if (!driver.init(param))                         ///< Call the init function
    {
        RS_ERROR << "Driver Initialize Error..." << RS_REND;
        return -1;
    }
    //std::thread cloud_handle_thread = std::thread(processCloud);

    driver.start();  ///< The driver thread will start
    RS_DEBUG << "RoboSense Lidar-Driver Linux online demo start......" << RS_REND;
 
    auto dora_context = init_dora_context_from_env();
    auto ret = run(dora_context);
    free_dora_context(dora_context);
 
    to_exit_process = true;
    driver.stop();
    std::cout << "exit rslidar driver ..." << std::endl;
    return ret;
}

上述代码中，可以通过修改变量 RSDriverParam param 的成员变量设置激光雷达的类型、端口号等参数信息。

2.2 编译rslidar驱动

clang++  rslidar_driver.cc -lm -lrt -ldl -pthread -std=c++14 -lpcap -ldora_node_api_c -L /home/dora/dora_project/dora/target/release --output build/rslidar_driver -I rs_driver/src

上述命令中 路径 “/home/dora/dora_project/dora/target/release” 需要修改为你电脑上dora源码的路径

2.3 编写rslidar_test.yml文件

nodes:
  #rslidar driver   node
  - id: rslidar_driver
    custom:
      source: build/rslidar_driver
      inputs:
        tick: dora/timer/millis/100
      outputs:
        - pointcloud

2.4 启动节点

在终端中输入以下命令，启动代码节点

dora start rslidar_test.yml --name test

查看输出日志

dora logs test rslidar_driver

3、rslidar Debug节点

该节点的功能是 接收dora框架中雷达节点发布的数据，并将数据相关信息显示在log文件中，以检验驱动节点是否正确。

节点与operator的区别：节点会从main函数中进入，operator有点类似于函数调用的形式，给定一个信号量，则调用一次函数

3.1 新建调试程序

新建文件 operator_rslidar_debug.cc

注意将路径 /home/dora/dora_project/dora/apis/c/node/node_api.h"替换为你电脑上的路径

extern "C"
{
#include "/home/dora/dora_project/dora/apis/c/node/node_api.h"
#include "/home/dora/dora_project/dora/apis/c/operator/operator_api.h"
#include "/home/dora/dora_project/dora/apis/c/operator/operator_types.h"
}

#include <memory>
#include <iostream>
#include <vector>
#include <string.h>
#include <cassert>

class Operator
{
public:
    Operator();
};

Operator::Operator() {}

extern "C" DoraInitResult_t dora_init_operator()
{
    Operator *op = std::make_unique<Operator>().release();

    DoraInitResult_t result = {.operator_context = (void *)op};
    return result;
}

extern "C" DoraResult_t dora_drop_operator(void *operator_context)
{
    delete (Operator *)operator_context;
    return {};
}

extern "C" OnEventResult_t dora_on_event(
    RawEvent_t *event,
    const SendOutput_t *send_output,
    void *operator_context)
{
    if (event->input != NULL)
    {
        // input event
        Input_t *input = event->input;
        char *id = dora_read_input_id(input);

        Vec_uint8_t data = dora_read_data(input);
        assert(data.ptr != NULL);

        std::cout
            << "C++ Operator (C-API) received input `" << id << "` with data: [";
        for (int i = 0; i < data.len; i++)
        {
            std::cout << (unsigned int)data.ptr[i] << ", ";
        }
        std::cout << "]" << "input_data_len:"<< data.len <<std::endl;

        const char *out_id = "half-status";
        char *out_id_heap = strdup(out_id);

        size_t out_data_len = 1;
        uint8_t *out_data_heap = (uint8_t *)malloc(out_data_len);
        *out_data_heap = *data.ptr / 2;

        // DoraResult_t send_result = dora_send_operator_output(send_output, out_id_heap, out_data_heap, out_data_len);
        DoraResult_t  send_result= {};
        OnEventResult_t result = {.result = send_result, .status = DORA_STATUS_CONTINUE};

        dora_free_data(data);
        dora_free_input_id(id);

        return result;
    }
    if (event->stop)
    {
        printf("C operator received stop event\n");
    }

    OnEventResult_t result = {.status = DORA_STATUS_CONTINUE};
    return result;
}

编译文件 注意这里链接的时候要链接到debug文件下的 dora_operator_api_c 库（完全是试出来的）

clang++ -c operator_rslidar_debug.cc -std=c++14 -o build/operator_rslidar_debug.o -fPIC
clang++ -shared build/operator_rslidar_debug.o -o build/liboperator_rslidar_debug.so -l dora_operator_api_c -L  /home/dora/dora_project/dora/target/debug 

在 rslidar_test.yml文件中添加以下内容

  - id: runtime-node-1
    operators:
      - id: operator_rslidar_debug
        shared-library: build/operator_rslidar_debug
        inputs:
          pointcloud: rslidar_driver/pointcloud

3.2 启动节点

dora start rslidar_test.yml --name test

查看输出日志

dora logs test runtime-node-1

通过日志文件可以看到，调试节点接收到了雷达节点发布的消息，并且识别到了ID、打印了点云数据长度

参考资料

[1] https://github.com/RoboSense-LiDAR/rs_driver/tree/main
[2] https://github.com/dora-rs/dora-hardware/tree/main/vendors/lidar/Robosense

dora-rs目前资料较少 欢迎大家点赞在评论区交流讨论（cenruping@vip.qq.com） O(∩_∩)O
或者加群水一波（1149897304）