Dora读取激光雷达数据并发布到ROS2
Dora C++节点与Python节点通信
读取激光雷达数据的Dora节点是一个c++节点,将激光雷达数据发布到ROS2中的Dora节点是一个Python节点,二者之间可以通过数据流文件(dataflow.yml)传递数据。
# dataflow.yml
nodes:
#rslidar driver node
- id: rslidar_driver
custom:
source: build/rslidar_driver
inputs:
tick: dora/timer/millis/100
outputs:
- pointcloud
- id: ros2test
operator:
python: ros2test.py
inputs:
pointcloud: rslidar_driver/pointcloud
Dora c++节点读取到的雷达数据
编译下载的雷达驱动rs_driver后可以找到雷达数据的消息类型文件 point_cloud_msg.hpp
// point_cloud_msg.hpp
#pragma once
#include <vector>
#include <string>
struct PointXYZI
{
float x;
float y;
float z;
uint8_t intensity;
};
struct PointXYZIRT
{
float x;
float y;
float z;
uint8_t intensity;
uint16_t ring;
double timestamp;
};
template <typename T_Point>
class PointCloudT
{
public:
typedef T_Point PointT;
typedef std::vector<PointT> VectorT;
uint32_t height = 0; ///< Height of point cloud
uint32_t width = 0; ///< Width of point cloud
bool is_dense = false; ///< If is_dense is true, the point cloud does not contain NAN points,
double timestamp = 0.0;
uint32_t seq = 0; ///< Sequence number of message
std::string frame_id = ""; ///< Point cloud frame id
VectorT points;
};
PointCloudT是dora c++雷达驱动节点接收到的雷达数据类型,包括点云的宽、高等,以及存储点云的Vector。Dora c++雷达驱动节点读取到的点云中的每个点用结构体PointXYZI表示,即点的坐标x,y,z以及反射强度i,每个点的大小为13个字节。
Dora Python节点接收到的雷达数据
参考Dora C++雷达驱动:https://rupingcen.blog.csdn.net/article/details/135376558 可以了解到Python节点接收到的雷达数据是什么样的。主要是关注 int run(void *dora_context) 这个函数。
// 部分int run(void *dora_context)
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;
}
// Dora c++雷达驱动节点发送数据
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);
接收到的点云数据被存储在 bytePointCloud 这个uint8数组中,而且是直接将点云数据坐标(XYZI)地址上的内容拷贝到bytePointCloud中,最后转换为char数组发送给Dora Python节点。所以Dora Python节点接收到的是uint8数组,大小为16 + 点云中点的数目*16 字节,最开始的16个字节:4个字节为seq,4个空白字节(无意义),8个字节为时间戳,后面的每16个字节代表点云中的一个点的数据。Dora Python节点打印收到的消息是这样的
# Dora Python节点接收到的部分数据
[0 0 0 0 0 0 0 0 28 191 8 248 134 109 217 65 147 214 67 190 99 101 226 62 244 49 244 61 32 0 0 93 ... ]
# 根据每个数的意义可以改写一下
lidar_data={
"seq":[0 0 0 0], # uint32
"empty":[0 0 0 0],
"timestamp":[28 191 8 248 134 109 217 65], # double
"Point_1":{
"x":[147 214 67 190], # float32
"y":[99 101 226 62], # float32
"z":[244 49 244 61], # float32
"i":[32 0 0 93], # uint8
},
...
}
Dora C++雷达驱动节点读取到到的点云数据是PointCloudT,每个点的坐标x,y,z是float32类型,Dora Python节点接收到的是一个uint8数组,需要转换一下。之前提到Dora c++雷达驱动节点中,点云坐标数据(XYZI)地址上的内容直接拷贝给了bytePointCloud,最后Dora Python节点接收到的数据也是bytePointCloud,那么将Dora C++节点读取到的点云坐标数据(XYZI)与Dora Python节点接收到的数据的对应部分都转换为二进制,它们应该是一样的,然后再将二进制数据按照c++存储flaot32的规则进行转换,就可以将uint8数组还原回点云坐标。
flaot32类型在c++中的存储格式参考:https://zhuanlan.zhihu.com/p/632347955
下图也来源于这个知乎网址。
32位中最高位为符号位:0代表正数,1代表负数。符号位后面八位是指数位,从低到高代表
2
0
2^0
20到
2
7
2^7
27,剩下的23位位尾数位,从高到低代表
2
−
1
2^{-1}
2−1到
2
−
23
2^{-23}
2−23。存储的flaot32数据的值为三部分的乘积。将32位中的第i位上面的数记为
b
i
b_i
bi(如0位上面的数是0,
b
0
=
0
b_0=0
b0=0):
符号位:
s
i
g
n
=
(
−
1
)
b
31
sign=(-1)^{b_{31}}
sign=(−1)b31
指数位:
e
x
p
o
n
e
n
t
=
2
∑
i
=
23
30
(
b
i
∗
2
i
−
23
)
−
127
exponent = 2^{\sum\limits_{i=23}^{30}(b_{i}*2^{i-23})-127}
exponent=2i=23∑30(bi∗2i−23)−127
尾数位:
f
r
a
c
t
i
o
n
=
1
+
∑
i
=
0
22
b
i
∗
2
i
−
23
fraction=1+\sum\limits_{i=0}^{22}b_{i}*2^{i-23}
fraction=1+i=0∑22bi∗2i−23
float32的值:
F
l
o
a
t
32
V
a
l
u
e
=
s
i
g
n
∗
e
x
p
o
n
e
n
t
∗
f
r
a
c
t
i
o
m
Float32Value = sign*exponent*fractiom
Float32Value=sign∗exponent∗fractiom
比如对于一个点的x坐标: “x”:[147 214 67 190],可以把数组中的元素通过 bin() 函数转换为二进制(得到的是字符串,前两个字符是’0b’,通过切片去掉),把四个字符串放在一个列表里。对于比较小的数,转换成二进制不够8位,需要在前面补0(如60,转为二进制位为 111100 )
#uint8_array是输入的uint8数组,是np_array
# transfrom the element in uint8 array to binary
bin_element_array = [bin(uint8_array[i])[2:] for i in range(0, 4)]
for i in range(0, 4):
if len(bin_element_array[i]) < 8:
bin_element_array[i] = "0" * (8-len(bin_element_array[i])) + bin_element_array[i]
把得到的四个字符串划分为符号(sign),指数(exp),尾数(fraction)三个部分,按照规则进行计算,得到float32的值。
需要说明一下的是,uint8数组中四个数转换成二进制后需要倒着排,比如 “x”:[147 214 67 190],190转换成的二进制数’10111110’对应最高的8位,剩下的三个数对应的二进制67:‘01000011’,214:‘11010110’, 147:‘10010011’,这四个数转换为32位二进制是(从左往右位数从高到低):
‘10111110 01000011 11010110 10010011’
def uint8_2_float(uint8_array):
# transfrom the datatype from uint8 array to float32
# input: a uint8 array(np array)
# output: float32 data
# transfrom the element in uint8 array to binary ,then transform to string
bin_element_array = [bin(uint8_array[i])[2:] for i in range(0, 4)]
for i in range(0, 4):
if len(bin_element_array[i]) < 8:
bin_element_array[i] = "0" * (8-len(bin_element_array[i])) + bin_element_array[i] # the binary_element shoud have 8 bit
sign = int(bin_element_array[3][0]) # if 0, the number is nonnegtive ,else the number is negtive
exp_bit = bin_element_array[3][1:] + bin_element_array[2][0] # 8 bit for exponent
fraction_bit = bin_element_array[2][1:] + bin_element_array[1] + bin_element_array[0] # 23 bit for fraction
# compute the exponent
exp_num = 0
for i in range(0,8):
exp_num = exp_num + int(exp_bit[i]) * 2**(7-i)
exp_result = 2**(exp_num - 127)
# compute the fraction
fraction_num = 0
for i in range(0, 23):
fraction_num += int(fraction_bit[i]) * 2**(-1 * (i+1))
fraction_result = fraction_num + 1
return (-1)**(sign) * exp_result * fraction_result
Dora Python节点向ROS2发送激光雷达数据
向ROS2发布消息的Dora节点是一个Operator。使用Dora向ROS2发布消息,需要先在用 *dora.experimental.ros2_bridge.Ros2Context()*创建一个ros2_context,再用这个ros2_context创建ros2节点(ros2_node),再使用ros2_node创建话题与发布者(与ROS2的的写法不大相同,Dora里面的ros2话题要单独创建)
from typing import Callable, Optional
import pyarrow as pa
from dora import DoraStatus
import dora
import numpy as np
class Operator:
def __init__(self) -> None:
self.ros2_context = dora.experimental.ros2_bridge.Ros2Context()
# create ros2 node
self.ros2_node = self.ros2_context.new_node(
"lidar2ros", # 节点名称
"/ros2_bridge", # namespace
dora.experimental.ros2_bridge.Ros2NodeOptions(rosout=True)
)
# create ros2 qos
self.topic_qos = dora.experimental.ros2_bridge.Ros2QosPolicies(
reliable=True, max_blocking_time=0.1
)
# create ros2 topic
self.lidar_data_topic = self.ros2_node.create_topic(
"/ros2_bridge/lidar_data", # 话题名称
"sensor_msgs::PointCloud2", # 话题的接口类型
self.topic_qos
)
# create ros2 publisher
self.lidar_data_publisher = self.ros2_node.create_publisher(self.lidar_data_topic)
激光雷达数据格式PointCloud2
执行 ros2 interface show sensor_msgs/msg/PointCloud2可以查看PointCloud2的详细信息:
# This message holds a collection of N-dimensional points, which may
# contain additional information such as normals, intensity, etc. The
# point data is stored as a binary blob, its layout described by the
# contents of the "fields" array.
#
# The point cloud data may be organized 2d (image-like) or 1d (unordered).
# Point clouds organized as 2d images may be produced by camera depth sensors
# such as stereo or time-of-flight.
# Time of sensor data acquisition, and the coordinate frame ID (for 3d points).
std_msgs/Header header
builtin_interfaces/Time stamp
int32 sec
uint32 nanosec
string frame_id
# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
uint32 height
uint32 width
# Describes the channels and their layout in the binary data blob.
PointField[] fields
uint8 INT8 = 1
uint8 UINT8 = 2
uint8 INT16 = 3
uint8 UINT16 = 4
uint8 INT32 = 5
uint8 UINT32 = 6
uint8 FLOAT32 = 7
uint8 FLOAT64 = 8
string name #
uint32 offset #
uint8 datatype #
uint32 count #
bool is_bigendian # Is this data bigendian?
uint32 point_step # Length of a point in bytes
uint32 row_step # Length of a row in bytes
uint8[] data # Actual point data, size is (row_step*height)
bool is_dense # True if there are no invalid points
理解PointCloud2可以参考:
https://www.guyuehome.com/45026
https://zhuanlan.zhihu.com/p/421691350
https://answers.ros.org/question/273182/trying-to-understand-pointcloud2-msg/
头信息header中的fram_id是点云坐标相对于哪个坐标系得到的,为了方便再rviz2中进行测试,先设置为了"map"
激光雷达产生的点云是无序点云,无序点云的高位1,宽为点云中点的数目。(RGBD相机生成的点云为有序点云,高不再是1)
参考上面第三个网址,PointCloud2在ROS2中是以byte stream的形式进行传递的。fields相当于一种编码规则,声明了点云中每个点包含的数据格式。比如一个点只包含xyz坐标,每个坐标都是float32型的数据。如果把x,y,z坐标称为x,y,z三个channel(不清楚是不是这么叫,只是感觉和图像中的RGB channel有些类似),fields就是一个PointField类型的数组,其中包含了对每个channel的描述,发布者根据这种描述对数据编码,接收数据的节点进行解码,完成数据的传递。一个点云数据有多少个channel,filelds的长度就是多少,比如点云数据有坐标x,y,z与反射强度i四个channel,fields的长度就为4,只有xyz坐标,fields的长度就是3。每个filelds的元素包含四部分内:
name: string类型,channel的名称,如“x”, “y”, ”z”
offset: 偏移量,单位是字节(byte),也是此channel的起始位置,比如channel”x”与“y”的数据都是float32,也就是4个字节,那么在对点云信息的byte stream进行解码时,获得channel “z”的数据就要偏移8个字节。
datatype: 数据类型,channel数据的数据类型,PointCloud2给了一个数据类型的对照列表,比如数据类型时float32,datatype就为 7
uint8 INT8 = 1
uint8 UINT8 = 2
uint8 INT16 = 3
uint8 UINT16 = 4
uint8 INT32 = 5
uint8 UINT32 = 6
uint8 FLOAT32 = 7
uint8 FLOAT64 = 8
count:此channel有几个数据
Dora Python节点数据发布
数据发布在Operator类中的 dora_event中实现,雷达数据要写成一个字典的格式,并且 *“data”*要设置为float32,否则不能再RVIZ中显示:https://answers.ros.org/question/197309/rviz-does-not-display-pointcloud2-if-encoding-not-float32/
Dora Python节点完整代码如下:
# demo_pub_lidar_data.py
from typing import Callable, Optional
import pyarrow as pa
from dora import DoraStatus
import dora
import numpy as np
class Operator:
def __init__(self) -> None:
self.ros2_context = dora.experimental.ros2_bridge.Ros2Context()
# create ros2 node
self.ros2_node = self.ros2_context.new_node(
"lidar2ros",
"/ros2_bridge",
dora.experimental.ros2_bridge.Ros2NodeOptions(rosout=True)
)
# create ros2 qos
self.topic_qos = dora.experimental.ros2_bridge.Ros2QosPolicies(
reliable=True, max_blocking_time=0.1
)
# create ros2 topic
self.lidar_data_topic = self.ros2_node.create_topic(
"/ros2_bridge/lidar_data",
"sensor_msgs::PointCloud2",
self.topic_qos
)
# create ros2 publisher
self.lidar_data_publisher = self.ros2_node.create_publisher(self.lidar_data_topic)
def on_event(
self,
dora_event,
send_output,
) -> DoraStatus:
if dora_event["type"] == "INPUT":
# create a virtual PointCloud2 message for testing
points=np.array([[10.0, 10.0, 10.0],[5.0, 5.0, 5.0],[3.0, 3.0, 3.0]])
lidar_data_dict = {
"header": {
"stamp": {
"sec": np.int32(112233),
"nanosec": np.uint32(332211),
},
"frame_id": "map",
},
"height": np.uint32(1),
"width": np.uint32(3),
"fields":[{"name": "x", "offset": np.uint32(0), "datatype": np.uint8(7), "count": np.uint32(1)},
{"name": "y", "offset": np.uint32(4), "datatype": np.uint8(7), "count": np.uint32(1)},
{"name": "z", "offset": np.uint32(8), "datatype": np.uint8(7), "count": np.uint32(1)},],
"is_bigendian": False,
"point_step": np.uint32(12),
"row_step": np.uint32(36),
"data": np.asarray(points, np.float32).ravel().view(np.uint8),
"is_dense": False,
}
# print(pa.array([lidar_data_dict]))
self.lidar_data_publisher.publish(pa.array([lidar_data_dict]))
return DoraStatus.CONTINUE
测试用的数据流文件
# demo.yml
nodes:
- id: lidar_data_pub
operator:
python: demo_pub_lidar_data.py
inputs:
tick: dora/timer/millis/100
启动Dora Python节点后,可以查看ros2的topic,可以发现ros2话题创建成功
使用 ros2 topic echo /ros2_bridge/lidar_data监听话题,可以看到部分消息
启动RVIZ2,显示PointCloud2消息,因为测试代码里就三个点,为了方便观察,每个点设置的大了些。