ROS2的三维视觉应用
实验介绍
在现代机器人系统中，三维视觉是一个越来越常见的组成部分。随着微软Kinect和英特尔Realsense 这类RGB-D相机的大量普及，基于三维视觉的相关算法也是百花齐放、空前繁荣，其中一部分技术方案已经开始应用到商用机器人上。在这一节实验中，将会了解三维视觉相机采集的数据是以什么形式存在于ROS2中，以及如何转换成三维点云库(PCL)的数据格式，为后续使用PCL对点云数据进行进一步处理奠定基础。
三维点云数据的获取是通过订阅三维相机驱动节点发布的话题，从话题中获取相机发出的消息包来实现的。三维点云的话题名称一般是“<相机名称>/<可选分辨率>/points”的格式。在这个实验中，使用的虚拟机器人配备的是 Kinect V2 相机，所以话题名称为“/kinect2/sd/points”，话题中的消息包格式为 sensor_msgs::PointCloud2。
这个实验将会实现一个订阅者节点，订阅相机发布的话题“/kinect2/sd/points”。从此话题中接收 sensor_msgs::PointCloud2类型的消息包，并将其中的点云数据转换成PCL格式然后把所有三维点的坐标值显示在终端。
实验过程
RGB-D相机的三维点云数据获取
cd ~/ros2_ws/srcbash
首先在工作空间中创建一个软件包，打开一个新的终端窗口，输入如下指令，进入工作空间。
cd ~/ros2_ws/src
然后用如下指令创建一个名为“pc_pkg”的软件包(“pc”是“Point Cloud”的简称)。
ros2 pkg create pc_pkg
创建好软件包后，接下来在这个软件包中创建一个节点，具体操作步骤如下。
1.编写节点代码
先创建这个节点的源码文件，在VSCode中找到[pc_pkg]软件包，用鼠标右键单击它的[src]子目录，在弹出的快捷菜单中选择[新建文件]，此时会提示输人文件名，输入“pc_data.cpp”，然后按[Enter]键创建文件。
下面编写这个源码文件，其内容如下。
#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>
std::shared_ptr<rclcpp::Node> node;
void PointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
pcl::PointCloud<pcl::PointXYZ> pointCloudIn;
pcl::fromROSMsg(*msg, pointCloudIn);
int cloudSize = pointCloudIn.points.size();
for (int i = 0; i < cloudSize; i++)
{
RCLCPP_INFO(node->get_logger(), "[i= %d] ( %.2f , %.2f , %.2f)",
i,
pointCloudIn.points[i].x,
pointCloudIn.points[i].y,
pointCloudIn.points[i].z);
}
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
node = std::make_shared<rclcpp::Node>("pointcloud_data_node");
auto pc_sub = node->create_subscription<sensor_msgs::msg::PointCloud2>(
"/kinect2/sd/points",
1,
PointcloudCallback
);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
上述代码可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与wpr_simulation2demo_cpp\10_pc_data.cpp 文件中的代码进行比对。
代码编写完成后，需要进行保存。保存成功后，编辑界面文件名后面的圆点符号会变成一个叉符号。
2.设置编译规则
节点源码的编译规则写在 pc_pkg 的 CMakeLists.txt 文件里。在 VSCode 中打开这个文件,在这个文件里添加节点的编译规则。首先使用如下代码寻找节点源码中用到的依赖项。
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"
)
最后使用如下代码添加节点编译完成后的安装规则。
install(TARGETS pc_data
DESTINATION lib/${PROJECT_NAME})
这些内容可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cmakelists\10_pc_data.txt 文件中的代码进行比对。
上述规则添加完毕后，一定要保存文件，否则规则无法生效。
3.修改软件包信息
在VSCode 中打开[pc_pkg]下的[package.xml]文件，使用如下代码添加依赖项信。
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>pcl_conversions</depend>
<depend>pcl_ros</depend>
这些内容可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与wpr_simulation2\demo_package\10_pc_data.xml 文件中的代码进行比对。
文件修改后，一定要保存文件，否则新的包信息无法生效。
4.编译软件包
修改完上述文件后，打开终端窗口，执行如下指令，进入工作空间。
cd ~/ros2_ws
然后执行如下指令，对工作空间中的所有软件包进行编译
colcon build
仿真运行点云数据获取程序
下面运行刚编写的节点。在运行前，确认已经下载了wpr_simulation2 仿真项目。然后，需要加载当前工作空间中的环境设置，这样ros2指令才能找到刚才编译后的软件包和节点文件。保持终端的当前路径依然在工作空间目录ros2_ws中然后执行如下指令。
source install/setup.bash
然后，执行如下指令，启动带有机器人的仿真环境
ros2 launch wpr_simulation2 wpb_table.launch.py
这时会启动仿真窗口，里面有一台虚拟机器人。机器人的面前摆放着一张桌子，桌子上放有两瓶饮料。
接下来运行刚才编写的节点 pc_data。在Terminator 终端中，按组合键[Ctrl+shift+O]，将终端分为上、下两个子窗口。在新的终端窗口中执行如下指令，加载工作空间的环境设置。
source install/setup.bash
然后执行如下指令，运行刚才编写的 pc_data 节点
ros2 run pc_pkg pc_data
节点运行起来之后，会不停地刷新显示点云所有点的三维坐标值，点云数据获取成功。
使用PCL进行物品检测
在前面的实验里，实现了从ROS2 机器人头部的RGB-D相机获取三维点云数据。这次将继续深入，使用PCL实现三维特征提取，并对桌面上的物体进行检测和定位。在编写例程代码前，先确定程序的实现思路:
1)对机器人头部相机采集到的三维点云消息包进行格式转换，从ROS2的点云格式转换为 PCL 点云格式，以方便后面调用PCL的函数对点云数据进行处理。
2)先使用 PCL 函数对点云数据进行平面提取，将桌面的高度确定下来。
3)将桌面高度以下的点集剔除掉，仅保留桌面之上的物体点云。
4)使用欧几里得分割法对保留下来的物体点云进行点云簇的提取，将桌面上的多个相隔较远的点云簇区分开。这时可以认为每个点云簇就表示一个物体的点云集合。计算每个物体点云集合的质心坐标，用来表示物体的空间位置。
编写物品检测程序
首先在工作空间中创建一个名为“pc_pkg”的软件包，如果前面已经创建过，这里可以直接跳过，打开一个新的终端窗口，输入如下指令，进入工作空间。
cd ~/ros2_ws/src
然后用如下指令创建软件包
ros2 pkg create pc_pkg
创建好软件包后，接下来在这个软件包中创建一个节点，具体操作步骤如下
1.编写节点代码
先创建这个节点的源码文件，在VSCode中找到[pc_pkg]软件包用鼠标右键单击它的[src]子目录，在弹出的快捷菜单中选择「新建文件]。此时会提示输入文件名，输入“pc_objects.cpp”，’，然后按[Enter]键创建文件。
下面编写这个源码文件，其内容如下
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_ros/transform_listener.h>
#include <pcl_ros/transforms.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
std::shared_ptr<rclcpp::Node> node;
tf2_ros::Buffer::SharedPtr tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
void PointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
bool result = tf_buffer_->canTransform(
"base_footprint",
msg->header.frame_id,
msg->header.stamp
);
if (!result)
{
return;
}
sensor_msgs::msg::PointCloud2 pc_footprint;
pcl_ros::transformPointCloud(
"base_footprint",
*msg,
pc_footprint,
*tf_buffer_
);
pcl::PointCloud<pcl::PointXYZ> cloud_src;
pcl::fromROSMsg(pc_footprint, cloud_src);
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud_src.makeShared());
pass.setFilterFieldName("x");
pass.setFilterLimits(0.5, 1.5);
pass.filter(cloud_src);
pass.setInputCloud(cloud_src.makeShared());
pass.setFilterFieldName("y");
pass.setFilterLimits(-0.5, 0.5);
pass.filter(cloud_src);
pass.setInputCloud(cloud_src.makeShared());
pass.setFilterFieldName("z");
pass.setFilterLimits(0.5, 1.5);
pass.filter(cloud_src);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::SACSegmentation<pcl::PointXYZ> segmentation;
segmentation.setInputCloud(cloud_src.makeShared());
segmentation.setModelType(pcl::SACMODEL_PLANE);
segmentation.setMethodType(pcl::SAC_RANSAC);
segmentation.setDistanceThreshold(0.05);
segmentation.setOptimizeCoefficients(true);
pcl::PointIndices::Ptr planeIndices(new pcl::PointIndices);
segmentation.segment(*planeIndices, *coefficients);
int point_num = planeIndices->indices.size();
float points_z_sum = 0;
for (int i = 0; i < point_num; i++)
{
int point_index = planeIndices->indices[i];
points_z_sum += cloud_src.points[point_index].z;
}
float plane_height = points_z_sum / point_num;
RCLCPP_INFO(node->get_logger(), "plane_height = %.2f", plane_height);
pass.setInputCloud(cloud_src.makeShared());
pass.setFilterFieldName("z");
pass.setFilterLimits(plane_height + 0.2, 1.5);
pass.filter(cloud_src);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud_src.makeShared());
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setInputCloud(cloud_src.makeShared());
ec.setMinClusterSize(100);
ec.setMaxClusterSize(25000);
ec.setClusterTolerance(0.1);
ec.setSearchMethod(tree);
std::vector<pcl::PointIndices> cluster_indices;
ec.extract(cluster_indices);
int object_num = cluster_indices.size();
RCLCPP_INFO(node->get_logger(), "object_num = %d",object_num);
for(int i = 0 ; i < object_num ; i ++)
{
int point_num = cluster_indices[i].indices.size();
float points_x_sum = 0;
float points_y_sum = 0;
float points_z_sum = 0;
for(int j = 0 ; j < point_num ; j ++)
{
int point_index = cluster_indices[i].indices[j];
points_x_sum += cloud_src.points[point_index].x;
points_y_sum += cloud_src.points[point_index].y;
points_z_sum += cloud_src.points[point_index].z;
}
float object_x = points_x_sum/point_num;
float object_y = points_y_sum/point_num;
float object_z = points_z_sum/point_num;
RCLCPP_INFO(
node->get_logger(),
"object %d pos=(%.2f , %.2f , %.2f)",
i,
object_x,
object_y,
object_z
);
}
RCLCPP_INFO(node->get_logger(), "---------------------" );
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
node = std::make_shared<rclcpp::Node>("pointcloud_objects_node");
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
auto pc_sub = node->create_subscription<sensor_msgs::msg::PointCloud2>(
"/kinect2/sd/points",
1,
PointcloudCallback
);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
上述代码可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cpp\10_pc_objects.cpp 文件中的代码进行比对。
代码编写完成后，需要进行保存。保存成功后，编辑界面文件名后面的圆点符号会变成一个叉符号。
2.设置编译规则
节点源码的编译规则写在 pc_pkg的CMakeLists.txt 文件里。在 VSCode 中打开这个文件在这个文件里添加节点的编译规则。首先使用如下代码寻找节点代码中用到的依赖项。
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(pcl_ros REQUIRED)
然后使用如下代码添加节点源码的编译规则。
add_executable(pc_objects src/pc_objects.cpp)
ament_target_dependencies(pc_objects
"rclcpp" "sensor_msgs" "pcl_conversions" "pcl_ros"
)
最后使用如下代码添加节点编译完成后的安装规则。
install(TARGETS pc_objects
DESTINATION lib/${PROJECT_NAME})
这些内容可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cmakelists\10_pc_objects.txt 文件中的代码进行比对。
上述规则添加完毕后，一定要保存文件，否则规则无法生效。
3.修改软件包信息
在 VSCode 中打开[pc_pkg]下的[package.xml]文件，使用如下代码添加依赖项信息
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>pcl_conversions</depend>
<depend>pcl_ros</depend>
这些内容可以从 wpr_simulation2的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_package\10_pc_objects.xml 文件中的代码进行比对。
文件修改后，一定要保存文件，否则新的包信息无法生效。
4.编译软件包
修改完上述文件后，打开终端窗口。执行如下指令，进入工作空间。
cd ~/ros2_ws
然后执行如下指令，对工作空间中的所有软件包进行编译
colcon build
仿真运行物品检测程序
下面运行刚编写的节点。在运行前，确认已经下载了wpr_simulation2 仿真项目。然后，需要加载当前工作空间中的环境设置，这样ros2指令才能找到刚才编译后的软件包和节点文件，保持终端的当前路径依然在工作空间目录ros2_ws中然后执行如下指令。
source install/setup.bash
然后，先启动带有机器人的仿真环境，执行如下指令
ros2 launch wpr_simulation2 wpb_table.launch.py
这时会启动仿真窗口，里面有一台虚拟机器人。机器人的面前摆放着一张桌子，桌子上放有两瓶饮料。
接下来运行刚才编写的节点 pc_objects。在 Terminator 终端中，按组合键[Ctrl+Shift+O]，将终端分为上、下两个子窗口。先在新的终端窗口中执行如下指令，加载工作空间的环境设置。
source install/setup.bash
然后执行如下指令，运行刚才编写的pc_objects 节点
ros2 run pc_pkg pc_objects
节点运行起来之后，会刷新显示物品检测的结果信息
其中，plane_height 表示程序检测到的桌面的高度，单位为米(m);object_num 表示在桌面上检测到的物品点云簇个数;objectnpos=(x,y,z)表示第n个物体质心的三维坐标值，单位为米(m);每一帧点云数据包处理结束会输出一条分割符号。
从输出信息可以看出，机器人检测到桌子的高度为0.78m;桌子上有两个物体;两个物体与机器人的距离一致，都是1.09m;横向位置不一致，0号物体在机器人右侧0.3m处1号物体在机器人左侧0.2m处;两个物体质心距离地面的高度都是0.99m。可以在仿真窗口里对比一下这些数值和两瓶饮料相对机器人的空间位置。至此，基于三维点云的物体检测功能就完成了。
实验总结
在本次实验中，我们成功实现了ROS2中的三维视觉应用，包括三维点云数据的获取和物品检测。我们创建了一个名为pointcloud_data_node的ROS2节点，订阅了/kinect2/sd/points话题，接收并转换点云数据，实时显示每个点的三维坐标值。在此基础上，我们实现了物品检测功能，通过平面提取、点云过滤和欧几里得聚类算法，准确检测并定位桌面上的物体，输出每个物体的质心坐标。实验验证了点云数据的正确性和实时性，以及物品检测算法的有效性和鲁棒性。通过这次实验，我们不仅掌握了ROS2和PCL的使用方法，还为未来的机器人项目开发积累了宝贵的经验和技术支持。
扫一扫
2721
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
