ROS开发专栏---ROS2 三维视觉应用（2）---使用 PCL 进行桌面物品检测实验---适配Ubuntu 22.04

🎬 渡水无言 ：个人主页渡水无言

❄专栏传送门 ：《linux专栏》《嵌入式linux驱动开发》《linux系统移植专栏》

❄专栏传送门 ：《freertos专栏》《STM32 HAL库专栏》《linux裸机开发专栏》

❄专栏传送门 ：《产品测评专栏》《Ai智能体专栏）《ROS开发专栏》

⭐️流水不争先，争的是滔滔不绝

📚博主简介：第二十届中国研究生电子设计竞赛全国二等奖 |国家奖学金 | 省级三好学生

| 省级优秀毕业生获得者 | csdn新星杯TOP1 | 半导纵横专栏博主 | 211在读研究生

在这里主要分享自己学习的linux嵌入式领域知识；有分享错误或者不足的地方欢迎大佬指导，也欢迎各位大佬互相三连

前言

一、实验原理与通信架构

[二、PCL 核心算法解析](#二、PCL 核心算法解析)

三、编写桌面物品检测节点源码

[四、配置 CMakeLists.txt](#四、配置 CMakeLists.txt)

[五、配置 package.xml](#五、配置 package.xml)

前言

在上一期的博客实验里，实现了从ROS机器人头部的RGB-D相机获取三维点云数据。这一次将继续深入，使用PCL实现三维特征提取，并对桌面上的物体进行检测和定位。

一、实验原理与通信架构

仿真环境中，Kinect V2 RGB-D 相机会持续发布 /kinect2/sd/points 三维点云话题，包含桌面、物体、背景的所有点云数据。本实验通过以下步骤实现物品检测：

二、PCL 核心算法解析

RANSAC 平面分割：随机采样一致性算法，从点云中拟合出最大的平面模型，对应仿真环境中的桌面，快速分离桌面与物体点云。

体素降采样（VoxelGrid）：将点云按三维网格划分，每个网格仅保留一个点，大幅减少点云数量，提升后续处理速度。

直通滤波（PassThrough）：根据空间坐标范围过滤点云，移除相机后方、地面下方等无关背景点云。

欧氏聚类（EuclideanCluster）：根据点与点之间的空间距离，将不同物体的点云分割为独立聚类，实现多物体分离。

三、编写桌面物品检测节点源码

在 pc_pkg/src 目录下新建 pc_object_detect.cpp，完整代码如下：

cpp 复制代码

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/common.h>   // 必须加这个头文件！

std::shared_ptr<rclcpp::Node> node;

// 点云话题回调函数
void PointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
    // 1. ROS点云格式转为PCL点云格式
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*msg, *cloud);

    RCLCPP_INFO(node->get_logger(), "原始点云点数: %ld", (long int)cloud->points.size());

    // 2. 直通滤波：过滤后方和下方点云，只保留桌面区域
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.1, 1.5);
    pass.filter(*cloud_filtered);

    pass.setInputCloud(cloud_filtered);
    pass.setFilterFieldName("x");
    pass.setFilterLimits(-1.0, 1.0);
    pass.filter(*cloud_filtered);

    // 3. 体素降采样：减少点云数量，提升处理速度
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setInputCloud(cloud_filtered);
    vg.setLeafSize(0.01f, 0.01f, 0.01f);
    vg.filter(*cloud_filtered);

    RCLCPP_INFO(node->get_logger(), "降采样后点云点数: %ld", (long int)cloud_filtered->points.size());

    // 4. RANSAC平面分割：提取桌面平面
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(1000);
    seg.setDistanceThreshold(0.02);
    seg.setInputCloud(cloud_filtered);
    seg.segment(*inliers, *coefficients);

    if (inliers->indices.empty())
    {
        RCLCPP_ERROR(node->get_logger(), "未检测到桌面平面");
        return;
    }

    // 5. 移除桌面平面点云，仅保留物体点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_objects(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(cloud_filtered);
    extract.setIndices(inliers);
    extract.setNegative(true);
    extract.filter(*cloud_objects);

    if (cloud_objects->empty())
    {
        RCLCPP_INFO(node->get_logger(), "桌面上未检测到物体");
        return;
    }

    // 6. 欧氏聚类：分离不同物体点云
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(cloud_objects);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.05);
    ec.setMinClusterSize(50);
    ec.setMaxClusterSize(10000);
    ec.setSearchMethod(tree);
    ec.setInputCloud(cloud_objects);
    ec.extract(cluster_indices);

    RCLCPP_INFO(node->get_logger(), "检测到物体数量: %ld", (long int)cluster_indices.size());

    // 7. 遍历每个物体聚类，计算包围盒与三维信息
    int object_id = 0;
    for (const auto& indices : cluster_indices)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
        for (const auto& idx : indices.indices)
            cloud_cluster->points.push_back((*cloud_objects)[idx]);
        cloud_cluster->width = cloud_cluster->points.size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

        // ====================== 修复这里！======================
        pcl::PointXYZ min_pt, max_pt;
        pcl::getMinMax3D<pcl::PointXYZ>(*cloud_cluster, min_pt, max_pt);
        // ======================================================

        float center_x = (min_pt.x + max_pt.x) / 2.0;
        float center_y = (min_pt.y + max_pt.y) / 2.0;
        float center_z = (min_pt.z + max_pt.z) / 2.0;
        float width = max_pt.x - min_pt.x;
        float height = max_pt.y - min_pt.y;
        float depth = max_pt.z - min_pt.z;

        RCLCPP_INFO(node->get_logger(), 
            "物体 %d 信息: 中心(%.2f, %.2f, %.2f) 尺寸(%.2f, %.2f, %.2f)",
            object_id++, center_x, center_y, center_z, width, height, depth);
    }
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    node = std::make_shared<rclcpp::Node>("pc_object_detect_node");

    auto pc_sub = node->create_subscription<sensor_msgs::msg::PointCloud2>(
        "/kinect2/sd/points", 1, PointcloudCallback);

    RCLCPP_INFO(node->get_logger(), "桌面物品检测节点已启动");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

四、配置 CMakeLists.txt

打开 pc_pkg/CMakeLists.txt，追加物品检测节点编译配置，全部代码如下：

cpp 复制代码

cmake_minimum_required(VERSION 3.8)
project(pc_pkg)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(pcl_ros REQUIRED)

# 编译点云数据节点
add_executable(pc_data src/pc_data.cpp)
ament_target_dependencies(pc_data
  rclcpp
  sensor_msgs
  pcl_conversions
  pcl_ros
)

# 编译物品检测节点
add_executable(pc_object_detect src/pc_object_detect.cpp)
ament_target_dependencies(pc_object_detect
  rclcpp
  sensor_msgs
  pcl_conversions
  pcl_ros
)

# 安装两个节点
install(TARGETS
  pc_data
  pc_object_detect
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

五、配置 package.xml

无需新增依赖，原有 PCL 相关依赖已满足，保留即可，全部代码如下：

cpp 复制代码

<?xml version="1.0"?>
<package format="3">
  <name>pc_pkg</name>
  <version>0.0.0</version>
  <description>ROS2三维视觉PCL点云处理功能包</description>
  <maintainer email="todo@todo.todo">todo</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
  <depend>pcl_conversions</depend>
  <depend>pcl_ros</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

六、编译与仿真完整运行

6.1、编译功能包

打开终端，输入以下指令：

cpp 复制代码

cd ~/ros2_zice
colcon build
source install/setup.bash

6.2、运行过程

启动Gazebo仿真场景

启动 Gazebo 仿真环境，加载机器人、桌面与物品模型，自动发布三维点云话题。

cpp 复制代码

source install/setup.bash
ros2 launch wpr_simulation2 wpb_table.launch.py

运行桌面物品检测节点

新建终端，输入以下指令：

cpp 复制代码

source install/setup.bash
ros2 run pc_pkg pc_object_detect

如下图所示：

节点运行后，成功检测到桌面平面高度为 1.23 米，并识别出桌面上共有 3 个物体。其中，0 号物体的中心坐标为 (-0.00, 0.47, 1.25)，与桌面平面距离较近；1 号物体中心坐标为 (-0.40, -0.04, 1.06)，位于机器人左前方；2 号物体中心坐标为 (0.18, -0.04, 1.05)，位于机器人右前方，两者与机器人的距离相近，横向分布在桌面两侧。

总结

本期博客我们使用PCL实现三维特征提取，实现了对桌面上的物体进行检测和定位。