DORA 机器人中间件学习教程（5）——3D激光雷达数据可视化

截止目前 DORA还没有类似于ROS2中RVIZ这样的可视化工具，而在调试算法时我们又需要将数据进行可视化以验证代码的正确性。目前的解决方法是将DORA中的数据发送到ROS2中，利用RVIZ工具进行可视化。

1 总体思路

DORA提供了ROS2的样例 https://github.com/dora-rs/dora-ros2-bridge 。如下图所示，在这篇博客中我们基于该样例的思路，编写DORA节点，接收雷达驱动发送的激光点云，对数据进行反序列化并转发到ROS2中。

2 DORA数据接收节点

考虑到DORA对python环境下的ROS2接口支持的更好，在我们的系统中绝大部分可视化相关的脚本都基于python语言进行编写。
该代码可以从这个地址下载：https://github.com/dora-rs/autoware.universe/blob/feature/autoware_dora/dora-hardware/vendors/lidar/lidar_to_ros2.py

在上一篇博客中创建的激光雷达驱动节点目录下，创建名为 lidar_to_ros2.py 的文件，并填入以下内容：

from typing import Callable, Optional
import pyarrow as pa
from dora import DoraStatus
import dora
import numpy as np
import time
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":
            print("dora event value over, it len:",len(dora_event["value"]), '\n')
            pointdata_raw = np.array(dora_event["value"][16:]).reshape(-1, 16)
            pointdata_raw = pointdata_raw[:, 0:16]        
            points = pointdata_raw    
            print("==========================================================\n")
            current_time = time.time()
            sec = (current_time)
            nanosec = ((current_time - sec) * 1e9)
            lidar_data_dict = {
                "header": {
                    "stamp": {
                        "sec": np.int32(sec),
                        "nanosec": np.uint32(nanosec),
                    },
                    "frame_id": "rslidar",
                },
                "height": np.uint32(1),
                "width": np.uint32(len(points)),
                "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)},
                          {"name": "i", "offset": np.uint32(12), "datatype": np.uint8(7), "count": np.uint32(1)},],
                "is_bigendian": False,
                "point_step": np.uint32(16),
                "row_step": np.uint32(len(points)),
                "data": points.ravel().view(np.uint8),#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

注意运行该文件需要保证电脑上安装了ROS2的环境。上述代码中“self.ros2_node”函数表示创建一个ROS2节点，“ self.lidar_data_topic”则是对节点的话题和发布的数据类型进行描述，注意这里的 "sensor_msgs::PointCloud2"应该是ROS2中标注的数据类型。若使用了自定义的数据类型，则需要先对消息类型进行编译，并手动加载到ROS2环境中，保证系统能正确查找到自定义消息类型。

代码“pointdata_raw = np.array(dora_event[“value”][16:]).reshape(-1, 16)”则是将原来的序列化的数据（一个连续的uint8类型数组）转化为n行16列的2维数据，这里的16列是应为点云数据中一个反射点由 x,y,z,i 四个变量进行描述，每个变量都是一个float类型数据（占4个Byte）,因此一个激光反射点对应 16个Byte。

3 编写yml文件

创建dataflow.yml 文件，与上一篇激光雷达驱动的博客编写驱动文件位于同一级目录下，这里我们在一个yml文件中启动了lidr驱动节点和lidar2ROS2数据转发节点。

nodes:
  #rslidar driver   node
  - id: rslidar_driver
    custom:
      source: build/rslidar_driver
      inputs:
        tick: dora/timer/millis/100
      outputs:
        - pointcloud

  - id: lidar_to_ros2
    operator:
        python: lidar_to_ros2.py
        inputs:
          pointcloud: rslidar_driver/pointcloud

目录结构如下：

4 启动节点

新建一个终端启动dora节点

dora up 
dora start dataflow.yml --name test

再新建一个终端启动RVIZ2，选择 “/ros2_bridge/lidar_data” 话题进行可视化

参考资料

[1] https://dora-rs.ai/docs/guides/dora-ros2-bridges/
[2] https://github.com/dora-rs/autoware.universe/tree/feature/autoware_dora/dora-hardware/vendors/lidar

dora-rs目前资料较少 欢迎大家在评论区交流讨论（cenruping@vip.qq.com） O(∩_∩)O
或者加群交流（1149897304）