table_pose.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: table_pose.cpp 30899 2010-07-16 04:56:51Z rusu $
00035  *
00036  */
00037 
00042 #include <ros/ros.h>
00043 #include <sensor_msgs/PointCloud2.h>
00044 
00045 #include <pcl/point_types.h>
00046 #include <pcl/filters/voxel_grid.h>
00047 #include <pcl/filters/project_inliers.h>
00048 #include <pcl/features/normal_3d.h>
00049 #include <pcl/kdtree/kdtree_ann.h>
00050 #include <pcl/segmentation/sac_segmentation.h>
00051 
00052 #include <pcl/PointIndices.h>
00053 #include <pcl/ModelCoefficients.h>
00054 
00055 #include <table_pose/GetPose.h>
00056 
00057 using namespace pcl;
00058       
00059 ros::Publisher pub;
00060 
00061 bool
00062   getTablePose (table_pose::GetPose::Request &req, table_pose::GetPose::Response &res)
00063 {
00064   PointCloud<PointXYZ> cloud;
00065   pcl::fromROSMsg (req.data, cloud);
00066 
00067   // Use an ANN kd tree representation
00068   KdTreeANN<PointXYZ>::Ptr tree = boost::make_shared<KdTreeANN<PointXYZ> > ();
00069 
00070   // ---[ Create the voxel grid
00071   VoxelGrid<PointXYZ> grid;
00072   grid.setInputCloud (cloud.makeShared ());
00073   grid.setLeafSize (0.01, 0.01, 0.01);
00074   grid.setFilterFieldName ("z");
00075   grid.setFilterLimits (0.5, 1.2);
00076 
00077   PointCloud<PointXYZ> cloud_filtered;
00078   grid.filter (cloud_filtered);
00079 
00080   // ---[ Estimate the point normals
00081   NormalEstimation<PointXYZ, Normal> n3d;
00082   n3d.setInputCloud (cloud_filtered.makeShared ());
00083   n3d.setSearchMethod (tree);
00084   n3d.setKSearch (0.01);
00085   
00086   PointCloud<Normal> cloud_normals;
00087   n3d.compute (cloud_normals);
00088 
00089   // ---[ Perform segmentation
00090   SACSegmentationFromNormals<PointXYZ, Normal> seg;
00091   seg.setInputCloud (cloud_filtered.makeShared ());
00092   seg.setInputNormals (cloud_normals.makeShared ());
00093   seg.setNormalDistanceWeight (0.1);
00094   seg.setModelType (SACMODEL_NORMAL_PLANE);
00095   seg.setMethodType (SAC_RANSAC);
00096   seg.setDistanceThreshold (0.1);
00097   seg.setMaxIterations (10000);
00098 
00099   PointIndices table_inliers; 
00100   ModelCoefficients table_coefficients;
00101   seg.segment (table_inliers, table_coefficients);
00102 
00103   // ---[ Extract the table
00104   PointCloud<PointXYZ> table_projected;
00105   ProjectInliers<PointXYZ> proj;
00106   proj.setModelType (SACMODEL_NORMAL_PLANE);
00107   proj.setInputCloud (cloud_filtered.makeShared ());
00108   proj.setIndices (boost::make_shared<PointIndices> (table_inliers));
00109   proj.setModelCoefficients (boost::make_shared<ModelCoefficients> (table_coefficients));
00110   proj.filter (table_projected);
00111 
00112   // Publish (not needed) - to be removed
00113   PointCloud<PointXYZ> cloud_table;
00114   copyPointCloud (cloud_filtered, table_inliers, cloud_table);
00115   sensor_msgs::PointCloud2 cloud_out;
00116   pcl::toROSMsg (cloud_table, cloud_out);
00117   pub.publish (cloud_out);
00118 
00119   return (true);
00120 }
00121 
00122 /* ---[ */
00123 int
00124   main (int argc, char** argv)
00125 {
00126   ros::init (argc, argv, "table_pose");
00127 
00128   ros::NodeHandle nh;
00129   
00130   pub = nh.advertise<sensor_msgs::PointCloud2> ("table", 1);
00131 
00132   ros::ServiceServer service = nh.advertiseService ("get_table_pose", getTablePose);
00133   ros::spin ();
00134 
00135   return (0);
00136 }
00137 /* ]--- */
00138 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


table_pose
Author(s): Radu Bogdan Rusu
autogenerated on Thu May 23 2013 14:08:15