Dora读取激光雷达数据并发布到ROS2

最新推荐文章于 2024-06-10 13:20:52 发布

微微辣就好

最新推荐文章于 2024-06-10 13:20:52 发布

阅读量1.2k

点赞数 11

文章标签： python

本文链接：https://blog.csdn.net/MRFantastic/article/details/135956340

版权

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^7$ ,剩下的23位位尾数位，从高到低代表 $2^{-1}$ 到 $2^{-23}$ 。存储的flaot32数据的值为三部分的乘积。将32位中的第i位上面的数记为 $b_i$ （如0位上面的数是0， $b_0=0$ ）:

符号位： $sign=(-1)^{b_{31}}$

指数位： $2^{\sum\limits_{i=23}^{30}(b_{i}*2^{i-23})-127}$
尾数位： $fraction=1+\sum\limits_{i=0}^{22}b_{i}*2^{i-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$
比如对于一个点的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消息，因为测试代码里就三个点，为了方便观察，每个点设置的大了些。
在这里插入图片描述

微微辣就好

关注

11
点赞
踩
12

收藏

觉得还不错? 一键收藏
0
评论
Dora读取激光雷达数据并发布到ROS2

读取激光雷达数据的Dora节点是一个c++节点，将激光雷达数据发布到ROS2中的Dora节点是一个Python节点，二者之间可以通过数据流文件(dataflow.yml)传递数据。
复制链接

扫一扫