segment_object_in_hand.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2009, Willow Garage, Inc.
00004 *  All rights reserved.
00005 *
00006 *  Redistribution and use in source and binary forms, with or without
00007 *  modification, are permitted provided that the following conditions
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033   
00034 // Author(s): Kaijen Hsiao
00035 
00036 #include <string>
00037 #include <ros/ros.h>
00038 #include <tf/tf.h>
00039 #include <tf/transform_listener.h>
00040 #include <sensor_msgs/PointCloud2.h>
00041 #include <pcl/filters/passthrough.h>
00042 #include <pcl/filters/voxel_grid.h>
00043 #include <pcl/point_cloud.h>
00044 #include <pcl/point_types.h>
00045 #include <pcl/filters/passthrough.h>
00046 #include <pcl/io/io.h>
00047 #include "pcl/io/pcd_io.h"
00048 #include <pcl/filters/extract_indices.h>
00049 #include <pcl/segmentation/extract_clusters.h>
00050 #include <pcl/search/kdtree.h>
00051 #include <pcl_ros/transforms.h>
00052 
00053 #include "tabletop_object_detector/SegmentObjectInHand.h"
00054 
00055 namespace tabletop_object_detector {
00056 
00057 class ObjectInHandSegmenter
00058 {
00059   typedef pcl::PointXYZRGB    Point;
00060   typedef pcl::search::KdTree<Point>::Ptr KdTreePtr;
00061 
00062 public:
00063   ObjectInHandSegmenter(ros::NodeHandle nh) : nh_(nh), priv_nh_("~")
00064   {
00065     // Point cloud passthrough filter params (wrist frame)
00066     double box_w = 0.3;
00067     priv_nh_.param<double>("x_filter_min", x_filter_min_, -(box_w - 0.185));
00068     priv_nh_.param<double>("x_filter_max", x_filter_max_, box_w + 0.185);
00069     priv_nh_.param<double>("y_filter_min", y_filter_min_, -box_w);
00070     priv_nh_.param<double>("y_filter_max", y_filter_max_, box_w);
00071     priv_nh_.param<double>("z_filter_min", z_filter_min_, -box_w);
00072     priv_nh_.param<double>("z_filter_max", z_filter_max_, box_w);
00073 
00074     // Params for where we want the clusters from (containing points within dist from (x,y,z) in wrist_frame)
00075     priv_nh_.param<double>("object_center_x", object_center_x_, 0.185);
00076     priv_nh_.param<double>("object_center_y", object_center_y_, 0);
00077     priv_nh_.param<double>("object_center_z", object_center_z_, 0);
00078     priv_nh_.param<double>("max_dist_from_center", max_dist_from_center_, 0.10);
00079 
00080     // Euclidean clustering params
00081     priv_nh_.param<double>("cluster_distance", cluster_distance_, 0.025);
00082     priv_nh_.param<double>("min_cluster_size", min_cluster_size_, 0.025);
00083     priv_nh_.param<std::string>("self_filtered_cloud_name", self_filtered_cloud_name_, "/narrow_stereo_textured/object_modeling_points_filtered");
00084 
00085     // Publisher for resulting cluster
00086     priv_nh_.param<std::string>("visualization_topic", visualization_topic_, "segment_object_in_hand_output_cluster");
00087     pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>(visualization_topic_, 1);
00088 
00089     // Service for performing segmentation
00090     segmentation_srv_ = nh_.advertiseService(nh_.resolveName("segment_object_in_hand_srv"), 
00091                                              &ObjectInHandSegmenter::serviceCallback, this);
00092 
00093   }
00094 
00095 private:
00096 
00097   ros::NodeHandle nh_, priv_nh_;
00098   tf::TransformListener listener_;
00099 
00100   // For filtering of point cloud in the gripper frame
00101   double x_filter_min_, x_filter_max_;
00102   double y_filter_min_, y_filter_max_;
00103   double z_filter_min_, z_filter_max_;
00104 
00105   // Point cloud subscriber and publisher
00106   ros::Subscriber sub_cloud_;
00107   ros::Publisher pub_cloud_;
00108   std::string visualization_topic_;
00109 
00110   // Euclidean clustering params
00111   double cluster_distance_;
00112   double min_cluster_size_;
00113   std::string self_filtered_cloud_name_;
00114   
00115   // Cluster location params
00116   double object_center_x_, object_center_y_, object_center_z_;
00117   double max_dist_from_center_;
00118 
00119   // Service server for segmenting object in hand
00120   ros::ServiceServer segmentation_srv_;
00121 
00125   bool serviceCallback(SegmentObjectInHand::Request &request, 
00126                                            SegmentObjectInHand::Response &response)
00127   {
00128     ROS_DEBUG("Got service request for segment_object_in_hand");
00129     int result = segmentObject(request.wrist_frame, response.cluster);
00130     if(result == 0) response.result = response.SUCCESS;
00131     else if(result == 1) response.result = response.NO_CLOUD_RECEIVED;
00132     else if(result == 2) response.result = response.TF_ERROR;
00133     else response.result = response.OTHER_ERROR;
00134     return true;
00135   }
00136 
00137 
00141   int segmentObject(std::string wrist_frame, sensor_msgs::PointCloud2 &output_cluster)
00142   {
00143     ROS_DEBUG("waiting for self-filtered point cloud");
00144     pcl::PointCloud<Point>::Ptr input_cloud(new pcl::PointCloud<Point>());
00145         
00146     // wait for the next PointCloud2 message 
00147     sensor_msgs::PointCloud2ConstPtr cloud_msg = 
00148       ros::topic::waitForMessage<sensor_msgs::PointCloud2>(self_filtered_cloud_name_, 
00149                                                            nh_, ros::Duration(5.0));
00150     if(!cloud_msg)
00151     {
00152       ROS_ERROR("No point cloud received!");
00153       return 1;
00154     }
00155 
00156     ROS_DEBUG("Point cloud received; processing");
00157         pcl::PointCloud<Point> wrist_frame_cloud;
00158 
00159     if (!wrist_frame.empty())
00160     {
00161       // convert to a pcl::PointCloud
00162       pcl::fromROSMsg(*cloud_msg, *input_cloud);
00163 
00164       // downsample the cloud
00165       pcl::VoxelGrid<Point> vox;
00166       vox.setInputCloud(input_cloud);
00167       vox.setLeafSize(0.005, 0.005, 0.005);
00168       vox.filter(*input_cloud);
00169 
00170       // Convert cloud to wrist frame
00171       try
00172       {
00173         pcl_ros::transformPointCloud(wrist_frame, *input_cloud, wrist_frame_cloud, listener_);
00174       }
00175       catch (tf::TransformException ex)
00176       {
00177         ROS_ERROR("Failed to transform cloud from frame %s into frame %s",
00178                   input_cloud->header.frame_id.c_str(), wrist_frame.c_str());
00179         return 2;
00180       }
00181       ROS_DEBUG("Input cloud converted to %s frame", wrist_frame.c_str());
00182     }
00183     else
00184     {
00185       ROS_ERROR("no wrist_frame specified!");
00186       return 3;
00187     }
00188     
00189     // Passthrough to get rid of points not in range
00190     pcl::PassThrough<Point> pass_;
00191     pcl::PointCloud<Point> cloud_filtered;
00192     pcl::PointCloud<Point>::ConstPtr cloud_ptr = boost::make_shared<const pcl::PointCloud<Point> > (wrist_frame_cloud);
00193     pass_.setInputCloud (cloud_ptr);
00194     pass_.setFilterFieldName ("z");
00195     pass_.setFilterLimits (z_filter_min_, z_filter_max_);
00196     pass_.filter (wrist_frame_cloud);
00197     cloud_ptr = boost::make_shared<const pcl::PointCloud<Point> > (wrist_frame_cloud);
00198     pass_.setInputCloud (cloud_ptr);
00199     pass_.setFilterFieldName ("y");
00200     pass_.setFilterLimits (y_filter_min_, y_filter_max_);
00201     pass_.filter (wrist_frame_cloud);
00202     cloud_ptr = boost::make_shared<const pcl::PointCloud<Point> > (wrist_frame_cloud);
00203     pass_.setInputCloud (cloud_ptr);
00204     pass_.setFilterFieldName ("x");
00205     pass_.setFilterLimits (x_filter_min_, x_filter_max_);
00206     pass_.filter (wrist_frame_cloud);
00207     cloud_ptr = boost::make_shared<const pcl::PointCloud<Point> > (wrist_frame_cloud);
00208 
00209     if(wrist_frame_cloud.points.size() == 0){
00210       ROS_INFO("no points left after passthrough!");
00211       return 3;
00212     }
00213 
00214     //sensor_msgs::PointCloud2 debug_cloud;
00215     //pcl::toROSMsg(wrist_frame_cloud, debug_cloud);
00216     //pub_cloud_.publish(debug_cloud);
00217 
00218     // Find cluster indices
00219     pcl::EuclideanClusterExtraction<Point> pcl_cluster_;
00220     KdTreePtr clusters_tree_;
00221     std::vector<pcl::PointIndices> clusters;
00222     clusters_tree_ = boost::make_shared<pcl::search::KdTree<Point> > ();
00223     pcl_cluster_.setClusterTolerance (cluster_distance_);
00224     pcl_cluster_.setMinClusterSize (min_cluster_size_);
00225     pcl_cluster_.setSearchMethod (clusters_tree_); 
00226     pcl_cluster_.setInputCloud (cloud_ptr);
00227     pcl_cluster_.extract (clusters);
00228 
00229     // Combine all clusters that are close to the gripper and add points for the indices to the output_cluster_pcl
00230     pcl::PointCloud<Point> output_cluster_pcl;
00231     if(clusters.size() != 0)
00232     {
00233       combineClustersNearPoint(clusters, wrist_frame_cloud, wrist_frame, object_center_x_, object_center_y_, object_center_z_, max_dist_from_center_, output_cluster_pcl); 
00234       if(output_cluster_pcl.points.size() == 0)
00235       {
00236         ROS_WARN("No points in resulting object cloud!");
00237       }
00238 
00239       // convert back to PointCloud2
00240       pcl::toROSMsg(output_cluster_pcl, output_cluster);
00241       output_cluster.header.frame_id = wrist_frame;
00242       output_cluster.header.stamp = ros::Time::now();
00243       
00244       // Publish the resulting PointCloud2 for visualization
00245       pub_cloud_.publish(output_cluster);
00246       
00247     }
00248     else
00249     {
00250       ROS_WARN("No clusters detected!");
00251     }
00252     return 0;
00253   }
00254 
00258   void combineClustersNearPoint(std::vector<pcl::PointIndices> clusters, pcl::PointCloud<Point> input_cloud, std::string wrist_frame, double x, double y, double z, double dist, pcl::PointCloud<Point> &output_cluster)
00259   {
00260     output_cluster.header.stamp = ros::Time(0);
00261     for(size_t cluster_ind=0; cluster_ind < clusters.size(); cluster_ind++)
00262     {
00263       ROS_DEBUG("cluster_ind: %d, size: %d\n", (int)cluster_ind, (int)clusters[cluster_ind].indices.size());
00264 
00265       // Check to see if the cluster has points within range
00266       double point_dist;
00267       double xdiff, ydiff, zdiff;
00268       bool cluster_good = 0;
00269       for (size_t j = 0; j < clusters[cluster_ind].indices.size(); ++j)
00270       {
00271         xdiff = x - input_cloud.points[clusters[cluster_ind].indices[j]].x;
00272         ydiff = y - input_cloud.points[clusters[cluster_ind].indices[j]].y;
00273         zdiff = z - input_cloud.points[clusters[cluster_ind].indices[j]].z;
00274         point_dist = sqrt(xdiff*xdiff + ydiff*ydiff + zdiff*zdiff);
00275         if(point_dist < dist)
00276         {
00277           ROS_DEBUG("keeping cluster\n");
00278           cluster_good = 1;
00279           break;
00280         }
00281       }
00282 
00283       // If so, add this cluster to the resulting point cloud
00284       if(cluster_good)
00285       {
00286         double current_size = output_cluster.points.size();
00287         output_cluster.points.resize(current_size + clusters[cluster_ind].indices.size());
00288         for (size_t k= 0; k < clusters[cluster_ind].indices.size(); ++k)
00289         {
00290           output_cluster.points[current_size + k].x = input_cloud.points[clusters[cluster_ind].indices[k]].x;
00291           output_cluster.points[current_size + k].y = input_cloud.points[clusters[cluster_ind].indices[k]].y;
00292           output_cluster.points[current_size + k].z = input_cloud.points[clusters[cluster_ind].indices[k]].z;
00293           output_cluster.points[current_size + k].rgb = input_cloud.points[clusters[cluster_ind].indices[k]].rgb;
00294         }
00295       }
00296     }
00297     ROS_DEBUG("output_cluster size: %d\n", (int)output_cluster.points.size()); 
00298   }
00299 
00300 };
00301 
00302 } //namespace tabletop_object_detector
00303 
00304 int main(int argc, char **argv) 
00305 {
00306   ros::init(argc, argv, "segment_object_in_hand_node");
00307   ros::NodeHandle nh;
00308 
00309   tabletop_object_detector::ObjectInHandSegmenter node(nh);
00310   ROS_INFO("Segment object in hand ready for service requests");
00311   ros::spin();
00312   return 0;
00313 }


tabletop_object_detector
Author(s): Marius Muja and Matei Ciocarlie
autogenerated on Fri Jan 3 2014 11:48:47