DORA 机器人中间件学习教程（6）——激光点云预处理

在DORA中通过驱动获取激光雷达数据后，激光点云预处理部分代码是参考了autoware官方代码并对其进行裁剪得到的，点云预处理主要包含三个节点：

• crop_box_point: 裁剪点云
• ring_outlier_fliter: 目的是去除昆虫、雨水等点云噪声。
• voxel_grid_downsample_filtre: 降采样

下图为 autoware官方的 ring_outlier_fliter 滤波器示意图

图片来源于 autoware官方

1 移植思路

Autoware 中基于ROS框架将多个节点组合成一个系统，因此移植算法只需要将找到ROS数据获取和发布函数部分的代码，将其替换为DORA中反序列化和序列化功能代码。
修改后的代码位于： https://github.com/dora-rs/autoware.universe/tree/feature/autoware_dora/localization/pointcloud_preprocessor

2 代码输入输出说明

DORA节点初始化函数：

extern "C"
{
#include "node_api.h"
#include "operator_api.h"
#include "operator_types.h"
}

#include <vector>
#include <iostream>
#include <Eigen/Dense>
#include <cstdint>
#include <memory>
#include <string.h>
#include <cassert>
#include <pcl/point_types.h>

 

int run(void *dora_context)
{

    while (true)
    {
        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)
        {
            //-------------------------------------------------------------------------------------------------------
            // 点云处理函数
			//---------------------------------------------------------------------------------------------------------
            char *output_data = (char *)point_data.ptr;
            size_t output_data_len = ((output_cloud.size() + 1) * 16);
            std::string out_id = "pointcloud";
            std::cout << "output_data_len: " << output_data_len << std::endl;
            int resultend = dora_send_output(dora_context, &out_id[0], out_id.length(), output_data, output_data_len);
            delete[] point_data.ptr;
            // int resultend = 0;

            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 << "crop_box_point node" << std::endl;
    auto dora_context = init_dora_context_from_env();
    auto ret = run(dora_context);
    free_dora_context(dora_context);

    return ret;
}

3 编写CmakeList.txt文件

DORA官方教程采用cargo编译器对代码进行编译，这里我们将其修改为cmake就行编译，在编译时需要指定DORA c/c++ API 接口目录，以及DORA源文件编译生成的 c/c++ 动态链接库。以下代码是CmakeList.txt文件中添加DORA目录和链接到DORA库的方法。
完整的文件位于：https://github.com/dora-rs/autoware.universe/blob/feature/autoware_dora/localization/pointcloud_preprocessor/CMakeLists.txt

include_directories(
  ${PCL_INCLUDE_DIRS}
  ${EIGEN3_INCLUDE_DIRS}
  ${YAMLCPP_INCLUDE_DIRS}
  $ENV{HOME}/dora/apis/c/node #dora的头文件路径 node_api.h
  $ENV{HOME}/dora/apis/c/operator
  ${CURRENT_DIR}/rs_driver/src #雷达的头文件路径
  $ENV{HOME}/dora/examples/c++-ros2-dataflow/build/ #C++ros的头文件路径
  
)

add_executable(ring_outlier_fliter ring_outlier_fliter.cc )
target_link_libraries(ring_outlier_fliter
  ${PCL_LIBRARIES}
  ${EIGEN_LIBRARIES}
  ${YAMLCPP_LIBRARIES}
  # glog::glog
  $ENV{HOME}/dora/target/release/libdora_node_api_c.a
  m
  rt
  dl 
  pthread
  pcap
)

4 编写yml文件

创建pointcloud_preprocessor.yml 文件，与上一篇激光雷达驱动的博客编写驱动文件位于同一级目录下，这里我们在一个yml文件中启动了lidr驱动节点和lidar2ROS2数据转发节点。
https://github.com/dora-rs/autoware.universe/blob/feature/autoware_dora/localization/pointcloud_preprocessor/pointcloud_preprocessor.yml

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

  # crop_box_point node
  - id: crop_box_point
    custom:
      source: build/crop_box_point
      inputs:
        pointcloud: rslidar_driver/pointcloud
      outputs: 
       - pointcloud

  # ring_outlier_fliter node
  - id: ring_outlier_fliter
    custom:
      source: build/ring_outlier_fliter
      inputs:
        pointcloud: crop_box_point/pointcloud
      outputs: 
       - pointcloud
  # voxel_grid_downsample_filtre node
  - id: voxel_grid_downsample_filtre
    custom:
      source: build/voxel_grid_downsample_filtre
      inputs:
       pointcloud: ring_outlier_fliter/pointcloud
      outputs: 
       - pointcloud

  - id: lidar_to_ros2
    operator:
        python: ../../dora-hardware/dora_to_ros2/lidar/lidar_to_ros2.py
        inputs:
          pointcloud: ring_outlier_fliter/pointcloud

上述yml文件描述的测试程序还包含有驱动和上一篇博客中提到的可视化节点。

此处目录结构发生了变化，这里我们将点云预处理相关的节点放到了另一个文件夹，pointcloud_preprocessor.yml 与预处理节点位于同一级目录，但是该yml文件中调用了激光雷达驱动节点（该节点位于其他目录下），因此需要保证该yml文件下第5行代码中 source描述的目录下能找到激光雷达驱动节点**“source: …/…/dora-hardware/dora_to_ros2/lidar/build/rslidar_driver_pcap”**

5 编译并启动节点

通过cmake对c代码进行编译：

cd pointcloud_preprocessor #这条指令可以不执行，但一定要在pointcloud_preprocessor路径下
mkdir build && cd build
cmake ..
cmake --build .

新建一个终端启动dora节点

dora up 
dora start pointcloud_preprocessor.yml --name test

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

参考资料

[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）