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）
原文地址：https://blog.csdn.net/crp997576280/article/details/135376558
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！