DORA中3D激光雷达数据可视化
- 1 总体思路
- 2 DORA数据接收节点
- 3 编写yml文件
- 4 启动节点
- 参考资料
截止目前 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” 话题进行可视化
DORA Show 3D 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）
