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

本文主要是介绍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函数是一个线程安全的队列popif (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 0for (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;}
#endiffree_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 1Vec_uint8_t result;std::shared_ptr<PointCloudMsg> msg = stuffed_cloud_queue.popWait();//这个popwait函数是一个线程安全的队列popif (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 0for (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 pointsu_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 itsize_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 timestampu_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;}#endifchar* 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 objectparam.input_type = InputType::ONLINE_LIDAR; //输入类型param.input_param.msop_port = 6699;   ///< Set the lidar msop port number, the default is 6699param.input_param.difop_port = 7788;  ///< Set the lidar difop port number, the default is 7788param.lidar_type = LidarType::RSHELIOS;   ///< Set the lidar type. Make sure this type is correct雷达类型param.print();//控制台输出参数信息LidarDriver<PointCloudMsg> driver;               ///< Declare the driver objectdriver.regPointCloudCallback(driverGetPointCloudFromCallerCallback, driverReturnPointCloudToCallerCallback); ///< Register the point cloud callback functionsdriver.regExceptionCallback(exceptionCallback);  ///< Register the exception callback functionif (!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 startRS_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_drivercustom:source: build/rslidar_driverinputs:tick: dora/timer/millis/100outputs:- 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 eventInput_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-1operators:- id: operator_rslidar_debugshared-library: build/operator_rslidar_debuginputs: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）

这篇关于Dora-rs 机器人框架学习教程（4）——速腾激光雷达驱动的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---