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

2024-01-08 09:28
文章标签 rs 学习 教程 驱动 框架 机器人 激光雷达 dora 速腾

本文主要是介绍Dora-rs 机器人框架学习教程（4）——速腾激光雷达驱动，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

文章目录

  • 1、rslidar 雷达官方驱动
  • 2、rslidar dora 框架驱动
    • 2.1 驱动代码
    • 2.2 编译rslidar驱动
    • 2.3 编写rslidar_test.yml文件
    • 2.4 启动节点
  • 3、rslidar Debug节点
    • 3.1 新建调试程序
    • 3.2 启动节点
  • 参考资料

目标：编写一个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）——速腾激光雷达驱动的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/583006。

相关文章

电脑没有仿宋GB2312字体怎么办? 仿宋GB2312字体下载安装及调出来的教程

电脑没有仿宋GB2312字体怎么办? 仿宋GB2312字体下载安装及调出来的教程

《电脑没有仿宋GB2312字体怎么办?仿宋GB2312字体下载安装及调出来的教程》仿宋字体gb2312作为一种经典且常用的字体，广泛应用于各种场合，如何在计算机中调出仿宋字体gb2312？本文将为您... 仿宋_GB2312是公文标准字体之一，仿China编程宋是字体名称，GB2312是字php符编码标准名称（简
阅读更多...
VScode连接远程Linux服务器环境配置图文教程

VScode连接远程Linux服务器环境配置图文教程

《VScode连接远程Linux服务器环境配置图文教程》：本文主要介绍如何安装和配置VSCode,包括安装步骤、环境配置（如汉化包、远程SSH连接）、语言包安装（如C/C++插件）等,文中给出了详... 目录一、安装vscode二、环境配置1.中文汉化包2.安装remote-ssh，用于远程连接2.1安装2
阅读更多...
vscode保存代码时自动eslint格式化图文教程

vscode保存代码时自动eslint格式化图文教程

《vscode保存代码时自动eslint格式化图文教程》：本文主要介绍vscode保存代码时自动eslint格式化的相关资料,包括打开设置文件并复制特定内容,文中通过代码介绍的非常详细,需要的朋友... 目录1、点击设置2、选择远程--->点击右上角打开设置3、会弹出settings.json文件，将以下内
阅读更多...
Window Server创建2台服务器的故障转移群集的图文教程

Window Server创建2台服务器的故障转移群集的图文教程

《WindowServer创建2台服务器的故障转移群集的图文教程》本文主要介绍了在WindowsServer系统上创建一个包含两台成员服务器的故障转移群集,文中通过图文示例介绍的非常详细,对大家的... 目录一、 准备条件二、在ServerB安装故障转移群集三、在ServerC安装故障转移群集，操作与Ser
阅读更多...
windos server2022的配置故障转移服务的图文教程

windos server2022的配置故障转移服务的图文教程

《windosserver2022的配置故障转移服务的图文教程》本文主要介绍了windosserver2022的配置故障转移服务的图文教程,以确保服务和应用程序的连续性和可用性,文中通过图文介绍的非... 目录准备环境：步骤故障转移群集是 Windows Server 2022 中提供的一种功能，用于在多个
阅读更多...
利用Python编写一个简单的聊天机器人

利用Python编写一个简单的聊天机器人

《利用Python编写一个简单的聊天机器人》这篇文章主要为大家详细介绍了如何利用Python编写一个简单的聊天机器人,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 使用 python 编写一个简单的聊天机器人可以从最基础的逻辑开始，然后逐步加入更复杂的功能。这里我们将先实现一个简单的
阅读更多...
龙蜥操作系统Anolis OS-23.x安装配置图解教程(保姆级)

龙蜥操作系统Anolis OS-23.x安装配置图解教程(保姆级)

《龙蜥操作系统AnolisOS-23.x安装配置图解教程(保姆级)》：本文主要介绍了安装和配置AnolisOS23.2系统，包括分区、软件选择、设置root密码、网络配置、主机名设置和禁用SELinux的步骤，详细内容请阅读本文，希望能对你有所帮助... ‌AnolisOS‌是由阿里云推出的开源操作系统，旨
阅读更多...
PyTorch使用教程之Tensor包详解

PyTorch使用教程之Tensor包详解

《PyTorch使用教程之Tensor包详解》这篇文章介绍了PyTorch中的张量（Tensor）数据结构,包括张量的数据类型、初始化、常用操作、属性等,张量是PyTorch框架中的核心数据结构,支持... 目录1、张量Tensor2、数据类型3、初始化（构造张量）4、常用操作5、常用属性5.1 存储（st
阅读更多...
Java操作PDF文件实现签订电子合同详细教程

Java操作PDF文件实现签订电子合同详细教程

《Java操作PDF文件实现签订电子合同详细教程》：本文主要介绍如何在PDF中加入电子签章与电子签名的过程,包括编写Word文件、生成PDF、为PDF格式做表单、为表单赋值、生成文档以及上传到OB... 目录前言：先看效果：1.编写word文件1.2然后生成PDF格式进行保存1.3我这里是将文件保存到本地后
阅读更多...
windows系统下shutdown重启关机命令超详细教程

windows系统下shutdown重启关机命令超详细教程

《windows系统下shutdown重启关机命令超详细教程》shutdown命令是一个强大的工具,允许你通过命令行快速完成关机、重启或注销操作,本文将为你详细解析shutdown命令的使用方法,并提... 目录一、shutdown 命令简介二、shutdown 命令的基本用法三、远程关机与重启四、实际应用
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2