Go to the documentation of this file.00001 #include "ros/ros.h"
00002 #include "pcl_cloud_tools/GetClusters.h"
00003 #include <sensor_msgs/PointCloud2.h>
00004 #include "pcl/common/common.h"
00005 #include <pcl/ros/conversions.h>
00006 #include <pcl/point_cloud.h>
00007 #include "pcl/filters/extract_indices.h"
00008 #include <pcl/point_types.h>
00009 #include <pcl/io/pcd_io.h>
00010
00011 int main(int argc, char **argv)
00012 {
00013 ros::init(argc, argv, "extract_clusters_client");
00014 ros::NodeHandle n;
00015 ros::ServiceClient client = n.serviceClient < pcl_cloud_tools::GetClusters
00016 > ("/extract_clusters_server/cluster_tracking");
00017 pcl_cloud_tools::GetClusters srv;
00018 pcl::PointCloud < pcl::PointXYZ > cloud;
00019
00020
00021 cloud.width = 5;
00022 cloud.height = 1;
00023 cloud.is_dense = false;
00024 cloud.points.resize(cloud.width * cloud.height);
00025
00026 for (size_t i = 0; i < cloud.points.size(); ++i)
00027 {
00028 cloud.points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
00029 cloud.points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
00030 cloud.points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
00031 }
00032 sensor_msgs::PointCloud2 cloud_ros;
00033 pcl::toROSMsg(cloud, cloud_ros);
00034 cloud_ros.header.stamp = ros::Time::now();
00035 cloud_ros.header.frame_id = "camera_rgb_optical_frame";
00036 cloud_ros.header.seq = 0;
00037
00038
00039 srv.request.input_cloud = cloud_ros;
00040 std::vector<pcl::PointIndices> clusters_indices;
00041 if (client.call(srv))
00042 {
00043 ROS_INFO("Cluster(s) extraction suceeded");
00044 clusters_indices = srv.response.clusters_indices;
00045 }
00046 else
00047 {
00048 ROS_ERROR("Cluster extraction failed");
00049 return 1;
00050 }
00051
00052
00053 pcl::PointCloud < pcl::PointXYZ > cloud_object_clustered;
00054 if (int(clusters_indices.size()) >= 0)
00055 {
00056 for (int i = 0; i < clusters_indices.size(); i++)
00057 {
00058 pcl::copyPointCloud(cloud, clusters_indices[i], cloud_object_clustered);
00059 }
00060 }
00061 else
00062 {
00063 ROS_ERROR("Only %ld clusters found",
00064 clusters_indices.size());
00065 }
00066
00067 return 0;
00068 }