📚 课程目录第2章点云预处理与地面滤波

2. 点云预处理与地面滤波

任务目标

本案例基于Autoware Universe的pointcloud_preprocessor与ground_segmentation节点，对Velodyne VLP-16发布的原始点云依次执行：①去除距离过近/过远的点；②移除NaN与离群点；③地面分割，最终输出仅含障碍物（非地面）的点云话题，供后续聚类与感知模块使用。

环境与依赖

Autoware Universe（>= 2024.03）已按官方文档完成编译安装
已安装Velodyne驱动（velodyne_driver），VLP-16话题输出为 /points_raw（sensor_msgs/PointCloud2）
已配置车辆/传感器TF树，base_link与velodyne坐标变换可用

实现方案

采用Autoware官方推荐的节点组合方式，通过launch文件串联三个处理节点。核心处理逻辑由Autoware内置节点完成，本案例提供可直接运行的launch配置与参数调优指引。

关键完整代码

# ground_filter.launch.py
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 输入点云话题（VLP-16原始输出）
    input_topic = LaunchConfiguration('input_topic', default='/points_raw')
    # 输出障碍物点云话题
    output_topic = LaunchConfiguration('output_topic', default='/points_no_ground')

    # 参数文件路径（使用Autoware预置的VLP-16示例参数）
    cropbox_param = os.path.join(
        get_package_share_directory('pointcloud_preprocessor'),
        'param/crop_box_filter_param.yaml')
    outlier_param = os.path.join(
        get_package_share_directory('pointcloud_preprocessor'),
        'param/outlier_filter_param.yaml')
    ground_param = os.path.join(
        get_package_share_directory('ground_segmentation'),
        'param/ground_segmentation_param.yaml')

    return LaunchDescription([
        DeclareLaunchArgument('input_topic', default_value='/points_raw'),
        DeclareLaunchArgument('output_topic', default_value='/points_no_ground'),

        # 1. 距离滤波：去除3m以内和100m以外的点
        Node(
            package='pointcloud_preprocessor',
            executable='crop_box_filter_node',
            name='crop_box_filter',
            parameters=[cropbox_param],
            remappings=[('input', input_topic),
                        ('output', '/points_cropped')]
        ),

        # 2. 离群点与NaN移除
        Node(
            package='pointcloud_preprocessor',
            executable='outlier_filter_node',
            name='outlier_filter',
            parameters=[outlier_param],
            remappings=[('input', '/points_cropped'),
                        ('output', '/points_filtered')]
        ),

        # 3. 地面分割（ray_ground_filter算法）
        Node(
            package='ground_segmentation',
            executable='ray_ground_filter_node',
            name='ray_ground_filter',
            parameters=[ground_param],
            remappings=[('input', '/points_filtered'),
                        ('output', output_topic)]
        ),
    ])

参数配置说明

上述launch引用了Autoware预置参数文件，VLP-16场景下需确认以下关键参数与传感器匹配：

crop_box_filter：设置min_x/min_y/min_z与max_x/max_y/max_z，建议min_z设为-1.5（去除车底噪声），max_z设为3.0（去除高架桥等空中点）
outlier_filter：VLP-16建议voxel_size_x/y/z为0.1，min_points为2
ray_ground_filter：sensor_height设为VLP-16实际安装高度（如1.8m），min_angle/max_angle对应雷达扫描范围（VLP-16为-15°~+15°）

运行方式

# 终端1：启动VLP-16驱动（假设已配置好IP）
ros2 launch velodyne_driver velodyne_driver_node.launch.py

# 终端2：启动地面滤波launch
ros2 launch ground_filter.launch.py

# 终端3：可视化验证
ros2 run rviz2 rviz2

在RViz2中添加PointCloud2显示，话题分别选择/points_raw（原始点云）与/points_no_ground（滤波后），固定坐标系设为velodyne或base_link。

验证方法与通过标准

话题连通性：执行 ros2 topic list，确认存在 /points_no_ground；执行 ros2 topic hz /points_no_ground，输出频率应接近10Hz（VLP-16的发布频率）
点云数量对比：在RViz中对比原始话题与输出话题，输出点云的密度明显降低，地面点（通常呈水平连续分布）被有效移除
障碍物保留：在雷达视野内放置行人或锥桶，确认这些物体在输出话题中完整保留，未被误删
无空话题：ros2 topic echo /points_no_ground --once 能返回非空的PointCloud2消息

常见问题

输出为空：检查crop_box_filter的z轴范围是否过窄，或ray_ground_filter的sensor_height与实际安装高度偏差过大
地面分割不彻底：适当增大ray_ground_filter的sensor_height误差容忍度，或调整angle_threshold参数（默认0.1弧度）
节点启动失败：确认Autoware环境已source（source install/setup.bash），且参数文件路径存在