機(jī)器人操作系統(tǒng)（ROS2）入門與實(shí)踐機(jī)器人操作系統(tǒng)（ROS2）入門與實(shí)踐第1章LinuxUbuntu入門基礎(chǔ)第2章ROS2安裝與系統(tǒng)架構(gòu)第3章ROS2編程基礎(chǔ)第4章ROS2機(jī)器人運(yùn)動控制第5章激光雷達(dá)在ROS2中的使用第6章IMU在ROS2中的使用第7章ROS2中的SLAM環(huán)境建圖第8章ROS2中的NAV2自主導(dǎo)航第9章ROS2中的圖像視覺應(yīng)用第10章ROS2的三維視覺應(yīng)用第11章ROS2的機(jī)械臂應(yīng)用第12章基于ROS2的綜合應(yīng)用第10章10.2使用PCL進(jìn)行物品檢測

10.1RGB-D相機(jī)的三維點(diǎn)云數(shù)據(jù)獲取10.3本章小結(jié)第10章ROS2的三維視覺應(yīng)用

三維點(diǎn)云數(shù)據(jù)的獲取是通過訂閱三維相機(jī)驅(qū)動節(jié)點(diǎn)發(fā)布的話題，從話題中獲取相機(jī)發(fā)出的消息包來實(shí)現(xiàn)的。本實(shí)驗(yàn)中，使用的虛擬機(jī)器人配備的是KinectV2相機(jī)，話題名稱是"/kinect2/sd/points"。話題中的消息包格式為sensor_msgs::PointCloud2。本實(shí)驗(yàn)將會實(shí)現(xiàn)一個訂閱者節(jié)點(diǎn)，訂閱相機(jī)發(fā)布的話題"/kinect2/sd/points"。從此話題中接收sensor_msgs::PointCloud2類型的消息包，并將其中的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換成PCL的格式，然后把所有三維點(diǎn)的坐標(biāo)值顯示在終端程序里。10.1RGB-D相機(jī)的三維點(diǎn)云數(shù)據(jù)獲取10.1RGB-D相機(jī)的三維點(diǎn)云數(shù)據(jù)獲取

詳細(xì)操作步驟見教材P329-P338頁1、編寫節(jié)點(diǎn)代碼在VSCode中找到pc_pkg軟件包，在

“src”文件夾新建文件，命名為“pc_data.cpp”。10.1RGB-D相機(jī)的三維點(diǎn)云數(shù)據(jù)獲取10.1.1編寫點(diǎn)云數(shù)據(jù)獲取程序打開一個新的終端窗口，在工作空間中創(chuàng)建一個新的軟件包。cd~/ros2_ws/srcros2pkgcreatepc_pkg#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;

voidPointcloudCallback(constsensor_msgs::msg::PointCloud2::SharedPtrmsg){pcl::PointCloud<pcl::PointXYZ>pointCloudIn;pcl::fromROSMsg(*msg,pointCloudIn);

intcloudSize=pointCloudIn.points.size();for(inti=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);}}10.1RGB-D相機(jī)的三維點(diǎn)云數(shù)據(jù)獲取intmain(intargc,char**argv){rclcpp::init(argc,argv);

node=std::make_shared<rclcpp::Node>("pointcloud_data_node");autopc_sub=node->create_subscription<sensor_msgs::msg::PointCloud2>("/kinect2/sd/points",1,PointcloudCallback);

rclcpp::spin(node);

rclcpp::shutdown();

return0;}10.1RGB-D相機(jī)的三維點(diǎn)云數(shù)據(jù)獲取2、設(shè)置編譯規(guī)則find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)find_package(pcl_conversionsREQUIRED)find_package(pcl_rosREQUIRED)add_executable(pc_datasrc/pc_data.cpp)ament_target_dependencies(pc_data"rclcpp""sensor_msgs""pcl_conversions""pcl_ros")install(TARGETSpc_dataDESTINATIONlib/${PROJECT_NAME})3、修改軟件包信息<depend>rclcpp</depend><depend>sensor_msgs</depend><depend>pcl_conversions</depend><depend>pcl_ros</depend>10.1RGB-D相機(jī)的三維點(diǎn)云數(shù)據(jù)獲取sourceinstall/setup.bashros2launchwpr_simulation2wpb_table.launch.py10.1RGB-D相機(jī)的三維點(diǎn)云數(shù)據(jù)獲取4、編譯軟件包c(diǎn)d~/ros2_wscolconbuild10.1.2仿真運(yùn)行點(diǎn)云數(shù)據(jù)獲取程序sourceinstall/setup.bashros2runpc_pkgpc_data打開第2個子窗口。10.1RGB-D相機(jī)的三維點(diǎn)云數(shù)據(jù)獲取

在10.1節(jié)的實(shí)驗(yàn)里，實(shí)現(xiàn)了從ROS機(jī)器人頭部的RGB-D相機(jī)獲取三維點(diǎn)云數(shù)據(jù)。這一次將繼續(xù)深入，使用PCL實(shí)現(xiàn)三維特征提取，并對桌面上的物體進(jìn)行檢測和定位。

具體實(shí)現(xiàn)步驟如下：1）將機(jī)器人頭部相機(jī)采集到的三維點(diǎn)云消息包進(jìn)行格式轉(zhuǎn)換。從ROS2的點(diǎn)云格式轉(zhuǎn)換為PCL點(diǎn)云格式，方便后面調(diào)用PCL的函數(shù)對點(diǎn)云數(shù)據(jù)進(jìn)行處理。2）先使用PCL函數(shù)對點(diǎn)云數(shù)據(jù)進(jìn)行平面提取，將桌面的高度確定下來。3）將桌面高度以下的點(diǎn)集剔除掉，僅保留桌面之上的物體點(diǎn)云。4）使用歐幾里德分割法對保留下來的物體點(diǎn)云進(jìn)行點(diǎn)云簇的提取，將桌面上的多個相隔較遠(yuǎn)的點(diǎn)云簇區(qū)分開。這時可以認(rèn)為每個點(diǎn)云簇就表示一個物體的點(diǎn)云集合。計算每個物體點(diǎn)云集合的質(zhì)心坐標(biāo)，用來表示物體的空間位置。10.2使用PCL進(jìn)行物品檢測

詳細(xì)操作步驟見教材P339-P356頁1、編寫節(jié)點(diǎn)代碼在VSCode中找到pc_pkg軟件包，在

“src”文件夾中新建文件，命名為“pc_objects.cpp”。10.2使用PCL進(jìn)行物品檢測10.2.1編寫物品檢測程序下面編寫這個代碼文件內(nèi)容：#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<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::SharedPtrtf_buffer_;std::shared_ptr<tf2_ros::TransformListener>tf_listener_;10.2使用PCL進(jìn)行物品檢測voidPointcloudCallback(constsensor_msgs::msg::PointCloud2::SharedPtrmsg){boolresult=tf_buffer_->canTransform("base_footprint",msg->header.frame_id,msg->header.stamp);if(!result){return;}sensor_msgs::msg::PointCloud2pc_footprint;pcl_ros::transformPointCloud("base_footprint",*msg,pc_footprint,*tf_buffer_);10.2使用PCL進(jìn)行物品檢測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);10.2使用PCL進(jìn)行物品檢測pcl::ModelCoefficients::Ptrcoefficients(newpcl::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::PtrplaneIndices(newpcl::PointIndices);segmentation.segment(*planeIndices,*coefficients);intpoint_num=planeIndices->indices.size();floatpoints_z_sum=0;for(inti=0;i<point_num;i++){intpoint_index=planeIndices->indices[i];points_z_sum+=cloud_src.points[point_index].z;}10.2使用PCL進(jìn)行物品檢測floatplane_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>::Ptrtree(newpcl::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);intobject_num=cluster_indices.size();10.2使用PCL進(jìn)行物品檢測RCLCPP_INFO(node->get_logger(),"object_num=%d",object_num);for(inti=0;i<object_num;i++){intpoint_num=cluster_indices[i].indices.size();floatpoints_x_sum=0;floatpoints_y_sum=0;floatpoints_z_sum=0;for(intj=0;j<point_num;j++){intpoint_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;}floatobject_x=points_x_sum/point_num;floatobject_y=points_y_sum/point_num;floatobject_z=points_z_sum/point_num;RCLCPP_INFO(node->get_logger(),"object%dpos=(%.2f,%.2f,%.2f)",i,object_x,object_y,object_z);}RCLCPP_INFO(node->get_logger(),"---------------------");}10.2使用PCL進(jìn)行物品檢測intmain(intargc,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_);

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

rclcpp::spin(node);

rclcpp::shutdown();

return0;}10.2使用PCL進(jìn)行物品檢測2、設(shè)置編譯規(guī)則find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)find_package(pcl_conversionsREQUIRED)f