pointcloud_minmax_3d.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: transform_pointcloud.cpp 30719 2010-07-09 20:28:41Z rusu $
00035  *
00036  */
00037 
00047 // ROS core
00048 #include <ros/ros.h>
00049 
00050 #include "pcl/io/pcd_io.h"
00051 #include "pcl/point_types.h"
00052 #include "pcl/common/common.h"
00053 
00054 #include <tf/transform_listener.h>
00055 #include <visualization_msgs/Marker.h>
00056 #include <ias_table_msgs/TableCluster.h>
00057 
00058 #include "pcl/segmentation/extract_clusters.h"
00059 #include "pcl/kdtree/kdtree.h"
00060 #include "pcl/kdtree/kdtree_flann.h"
00061 
00062 using namespace std;
00063 
00064 class PointcloudMinMax3DNode
00065 {
00066 protected:
00067   ros::NodeHandle nh_;
00068   typedef pcl::search::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
00069 public:
00070   string output_cluster_topic_, input_cloud_topic_, output_cloud_cluster_topic_;
00071 
00072   ros::Subscriber sub_;
00073   ros::Publisher pub_, output_cloud_cluster_pub_;
00074   ros::Publisher vis_pub_, max_pub_, min_pub_;
00075 
00076   ias_table_msgs::TableCluster output_cluster_;
00077   sensor_msgs::PointCloud2 output_cloud_cluster_;
00078   pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_in_;
00079   pcl::PointXYZ point_min_;
00080   pcl::PointXYZ point_max_;
00081   //  Eigen::Vector4f point_min_;
00082   //Eigen::Vector4f point_max_;
00083   pcl::PointXYZ point_center_;
00084   visualization_msgs::Marker marker_center_;
00085   visualization_msgs::Marker marker_min_;
00086   visualization_msgs::Marker marker_max_;
00087 
00088   pcl::EuclideanClusterExtraction<pcl::PointXYZ> cluster_;
00089   KdTreePtr clusters_tree_;
00090   double object_cluster_tolerance_;
00091   int object_cluster_min_size_, object_cluster_max_size_;
00092 
00093   bool visualize_;
00094   int number_clusters_;
00096   PointcloudMinMax3DNode  (ros::NodeHandle &n) : nh_(n)
00097   {
00098     // Maximum number of outgoing messages to be queued for delivery to subscribers = 1
00099     nh_.param("input_cloud_topic", input_cloud_topic_, std::string("cloud_pcd"));
00100     nh_.param("output_cluster_topic", output_cluster_topic_, std::string("cluster"));
00101     nh_.param("visualize", visualize_, false);
00102     nh_.param("number_clusters", number_clusters_, 1);
00103     //5 cm between cluster
00104     nh_.param("object_cluster_tolerance", object_cluster_tolerance_, 0.03);
00105     //min 100 points
00106     nh_.param("object_cluster_min_size", object_cluster_min_size_, 50);
00107     //nh_.param("object_cluster_max_size", object_cluster_max_size_, 500);
00108     nh_.param("output_cluster_topic", output_cluster_topic_, std::string("cluster"));
00109     sub_ = nh_.subscribe (input_cloud_topic_, 1,  &PointcloudMinMax3DNode::cloud_cb, this);
00110     ROS_INFO ("[PointcloudMinMax3DNode:] Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ());
00111     pub_ = nh_.advertise<ias_table_msgs::TableCluster>(output_cluster_topic_, 1);
00112     ROS_INFO ("[PointcloudMinMax3DNode:] Will be publishing data on topic %s.", nh_.resolveName (output_cluster_topic_).c_str ());
00113     vis_pub_ = nh_.advertise<visualization_msgs::Marker>( "center_marker", 0 );
00114     min_pub_ = nh_.advertise<visualization_msgs::Marker>( "min_marker", 0 );
00115     max_pub_ = nh_.advertise<visualization_msgs::Marker>( "max_marker", 0 );
00116 
00117     nh_.param("output_cloud_cluster_topic", output_cloud_cluster_topic_, std::string("cloud_cluster"));
00118     //setup cluster object
00119     clusters_tree_ = boost::make_shared<pcl::search::KdTree<pcl::PointXYZ> > ();
00120     output_cloud_cluster_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(output_cloud_cluster_topic_, 5);
00121     clusters_tree_->setEpsilon (1);
00122   }
00123 
00125   // cloud_cb (!)
00126   void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
00127   {
00128     pcl::PointCloud<pcl::PointXYZ> cloud;
00129     pcl::fromROSMsg(*pc, cloud);
00130     cloud_in_ = boost::make_shared<const pcl::PointCloud<pcl::PointXYZ> > (cloud);
00131 
00132     std::vector<pcl::PointIndices> clusters;
00133     cluster_.setInputCloud (cloud_in_);
00134     cluster_.setClusterTolerance (object_cluster_tolerance_);
00135     cluster_.setMinClusterSize (object_cluster_min_size_);
00136     //    cluster_.setMaxClusterSize (object_cluster_max_size_);
00137     cluster_.setSearchMethod (clusters_tree_);
00138     cluster_.extract (clusters);
00139 
00140     for (unsigned long i = 0; i < clusters.size(); i++)
00141     {
00142       ROS_DEBUG("Cluster %ld sizes: %ld", i, clusters[i].indices.size());
00143     }
00144 
00145     ROS_INFO("[PointcloudMinMax3DNode] Number of clusters found matching the given constraints: %d.", (int)clusters.size ());
00146 
00147     int actual_number_clusters = (int)clusters.size();
00148     pcl::PointCloud<pcl::PointXYZ> cloud_object_cluster;
00149     ROS_INFO("actual number of clusters %ld", actual_number_clusters);
00150     if (actual_number_clusters == 0)
00151       {
00152         return;
00153       }
00154     for (int i = 0; i < number_clusters_; i++)
00155     {
00156       pcl::copyPointCloud (*cloud_in_, clusters[i], cloud_object_cluster);
00157         ROS_INFO ("[PointcloudMinMax3DNode] Got MinMax3D of biggest cluster with points %ld", cloud_object_cluster.points.size());
00158 
00159         pcl::getMinMax3D (cloud_object_cluster, point_min_, point_max_);
00160 
00161         //Calculate the centroid of the hull
00162         output_cluster_.header.stamp = ros::Time::now();
00163         output_cluster_.header.frame_id = pc->header.frame_id;
00164 
00165         output_cluster_.center.x = (point_max_.x + point_min_.x)/2;
00166         output_cluster_.center.y = (point_max_.y + point_min_.y)/2;
00167         output_cluster_.center.z = (point_max_.z + point_min_.z)/2;
00168 
00169         output_cluster_.min_bound.x = point_min_.x;
00170         output_cluster_.min_bound.y = point_min_.y;
00171         output_cluster_.min_bound.z = point_min_.z;
00172 
00173         output_cluster_.max_bound.x = point_max_.x;
00174         output_cluster_.max_bound.y = point_max_.y;
00175         output_cluster_.max_bound.z = point_max_.z;
00176 
00177 
00178         ROS_INFO("[PointcloudMinMax3DNode:] Published cluster to topic %s", output_cluster_topic_.c_str());
00179         pub_.publish (output_cluster_);
00180 
00181         if (visualize_)
00182         {
00183           compute_marker(marker_center_, output_cluster_.center);
00184           vis_pub_.publish( marker_center_ );
00185           compute_marker(marker_min_, output_cluster_.min_bound);
00186           min_pub_.publish( marker_min_ );
00187           compute_marker(marker_max_, output_cluster_.max_bound);
00188           max_pub_.publish( marker_max_ );
00189           toROSMsg(cloud_object_cluster, output_cloud_cluster_);
00190           output_cloud_cluster_pub_.publish(output_cloud_cluster_);
00191         }
00192     }
00193   }
00194 
00195   void compute_marker(visualization_msgs::Marker &marker,  geometry_msgs::Point32 &point)
00196   {
00197     marker.header.frame_id = output_cluster_.header.frame_id;
00198     marker.header.stamp =  output_cluster_.header.stamp;
00199     marker.ns = "object_cluster";
00200     marker.id = 0;
00201     marker.type = visualization_msgs::Marker::SPHERE;
00202     marker.action = visualization_msgs::Marker::ADD;
00203     marker.pose.position.x = point.x;
00204     marker.pose.position.y = point.y;
00205     marker.pose.position.z = point.z;
00206     marker.pose.orientation.x = 0.0;
00207     marker.pose.orientation.y = 0.0;
00208     marker.pose.orientation.z = 0.0;
00209     marker.pose.orientation.w = 1.0;
00210     marker.scale.x = 0.05;
00211     marker.scale.y = 0.05;
00212     marker.scale.z = 0.05;
00213     marker.color.a = 1.0;
00214     marker.color.r = 0.0;
00215     marker.color.g = 1.0;
00216     marker.color.b = 0.0;
00217     marker.lifetime = ros::Duration(1.0);
00218 
00219   }
00220 };
00221 
00222 int main (int argc, char** argv)
00223 {
00224   ros::init (argc, argv, "pointcloud_minmax_3d");
00225   ros::NodeHandle n("~");
00226   PointcloudMinMax3DNode mm(n);
00227   ros::spin ();
00228   return (0);
00229 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_cloud_tools
Author(s): Nico Blodow, Zoltan-Csaba Marton, Dejan Pangercic
autogenerated on Thu May 23 2013 17:11:36