From ff445d414d67ecd01eebada2b859b043a0f5a214 Mon Sep 17 00:00:00 2001 From: zhangwanjie Date: Fri, 11 Dec 2020 12:45:19 +0800 Subject: [PATCH] add pcl demoes --- wpb_home_tutorials/CMakeLists.txt | 16 ++ .../src/wpb_home_plane_detect.cpp | 109 ++++++++++++ .../src/wpb_home_pointcloud_cluster.cpp | 161 ++++++++++++++++++ 3 files changed, 286 insertions(+) create mode 100644 wpb_home_tutorials/src/wpb_home_plane_detect.cpp create mode 100644 wpb_home_tutorials/src/wpb_home_pointcloud_cluster.cpp diff --git a/wpb_home_tutorials/CMakeLists.txt b/wpb_home_tutorials/CMakeLists.txt index 2afc34e..e6d900e 100644 --- a/wpb_home_tutorials/CMakeLists.txt +++ b/wpb_home_tutorials/CMakeLists.txt @@ -358,6 +358,22 @@ target_link_libraries(wpb_home_pointcloud_node ${catkin_LIBRARIES} ) +add_executable(wpb_home_plane_detect + src/wpb_home_plane_detect.cpp +) +add_dependencies(wpb_home_plane_detect ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(wpb_home_plane_detect + ${catkin_LIBRARIES} +) + +add_executable(wpb_home_pointcloud_cluster + src/wpb_home_pointcloud_cluster.cpp +) +add_dependencies(wpb_home_pointcloud_cluster ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(wpb_home_pointcloud_cluster + ${catkin_LIBRARIES} +) + add_executable(wpb_home_behavior_node src/wpb_home_behavior_node.cpp ) diff --git a/wpb_home_tutorials/src/wpb_home_plane_detect.cpp b/wpb_home_tutorials/src/wpb_home_plane_detect.cpp new file mode 100644 index 0000000..158d9ee --- /dev/null +++ b/wpb_home_tutorials/src/wpb_home_plane_detect.cpp @@ -0,0 +1,109 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2017-2020, Waterplus http://www.6-robot.com +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the WaterPlus nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ +/*!****************************************************************** + @author ZhangWanjie + ********************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static tf::TransformListener *tf_listener; + +void PointcloudCB(const sensor_msgs::PointCloud2 &input) +{ + // 将点云数值从相机坐标系转换到机器人坐标系 + bool result = tf_listener->waitForTransform("/base_footprint", input.header.frame_id, input.header.stamp, ros::Duration(1.0)); + if(result == false) + { + return; + } + sensor_msgs::PointCloud2 pc_footprint; + pcl_ros::transformPointCloud("/base_footprint", input, pc_footprint, *tf_listener); + + //将点云数据从ROS格式转换到PCL格式 + pcl::PointCloud cloud_src; + pcl::fromROSMsg(pc_footprint , cloud_src); + + // 截取z轴方向，高度0.3米到1.5米内的点云 + pcl::PassThrough pass; + pass.setInputCloud (cloud_src.makeShared()); + pass.setFilterFieldName ("z"); + pass.setFilterLimits (0.3, 1.5); + pass.filter (cloud_src); + + // 定义模型分类器 + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::SACSegmentation 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;iindices[i]; + points_z_sum += cloud_src.points[point_index].z; + } + float plane_height = points_z_sum/point_num; + ROS_INFO("plane_height = %.2f" , plane_height); + } + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "wpb_home_plane_detect"); + tf_listener = new tf::TransformListener(); + ros::NodeHandle nh; + ros::Subscriber pc_sub = nh.subscribe("/kinect2/sd/points", 10 , PointcloudCB); + + ros::spin(); + + delete tf_listener; + + return 0; + +} \ No newline at end of file diff --git a/wpb_home_tutorials/src/wpb_home_pointcloud_cluster.cpp b/wpb_home_tutorials/src/wpb_home_pointcloud_cluster.cpp new file mode 100644 index 0000000..3fada99 --- /dev/null +++ b/wpb_home_tutorials/src/wpb_home_pointcloud_cluster.cpp @@ -0,0 +1,161 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2017-2020, Waterplus http://www.6-robot.com +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the WaterPlus nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ +/*!****************************************************************** + @author ZhangWanjie + ********************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static tf::TransformListener *tf_listener; + +void PointcloudCB(const sensor_msgs::PointCloud2 &input) +{ + // 将点云数值从相机坐标系转换到机器人坐标系 + bool result = tf_listener->waitForTransform("/base_footprint", input.header.frame_id, input.header.stamp, ros::Duration(1.0)); + if(result == false) + { + return; + } + sensor_msgs::PointCloud2 pc_footprint; + pcl_ros::transformPointCloud("/base_footprint", input, pc_footprint, *tf_listener); + + // 将点云数据从ROS格式转换到PCL格式 + pcl::PointCloud cloud_src; + pcl::fromROSMsg(pc_footprint , cloud_src); + + // 对设定范围内的点云进行截取 + pcl::PassThrough pass; + // 截取x轴方向，前方0.5米到1.5米内的点云 + pass.setInputCloud (cloud_src.makeShared()); + pass.setFilterFieldName ("x"); + pass.setFilterLimits (0.5, 1.5); + pass.filter (cloud_src); + // 截取y轴方向，左侧0.5米到右侧0.5米内的点云 + pass.setInputCloud (cloud_src.makeShared()); + pass.setFilterFieldName ("y"); + pass.setFilterLimits (-0.5, 0.5); + pass.filter (cloud_src); + // 截取z轴方向，高度0.5米到1.5米内的点云 + 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 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;iindices[i]; + points_z_sum += cloud_src.points[point_index].z; + } + float plane_height = points_z_sum/point_num; + + // 对点云再次进行截取，只保留平面以上的部分 + pass.setInputCloud (cloud_src.makeShared()); + pass.setFilterFieldName ("z"); + pass.setFilterLimits (plane_height + 0.2, 1.5); + pass.filter (cloud_src); + + // 对截取后的点云进行欧式距离分割 + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (cloud_src.makeShared()); // 输入截取后的点云 + std::vector cluster_indices; // 点云团索引 + pcl::EuclideanClusterExtraction ec; // 欧式聚类对象 + ec.setClusterTolerance (0.02); // 设置近邻搜索的搜索半径为2cm + ec.setMinClusterSize (100); // 设置一个聚类需要的最少的点数目为100 + ec.setMaxClusterSize (20000); // 设置一个聚类需要的最大点数目为20000 + ec.setSearchMethod (tree); // 设置点云的搜索机制 + ec.setInputCloud (cloud_src.makeShared()); // 输入截取后的点云 + ec.extract (cluster_indices); // 执行欧式聚类分割 + + // 计算每个分割出来的点云团的中心坐标 + int object_num = cluster_indices.size(); // 分割出的点云团个数 + ROS_INFO("object_num = %d",object_num); + for(int i = 0 ; i < object_num ; i ++) + { + int point_num = cluster_indices[i].indices.size(); // 点云团i中的点数 + 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; + ROS_INFO("object %d pos = ( %.2f , %.2f , %.2f)" ,i, object_x,object_y,object_z); + } + } + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "wpb_home_pointcloud_cluster"); + tf_listener = new tf::TransformListener(); + ros::NodeHandle nh; + ros::Subscriber pc_sub = nh.subscribe("/kinect2/sd/points", 10 , PointcloudCB); + + ros::spin(); + + delete tf_listener; + + return 0; + +} \ No newline at end of file