segment_differences.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: segment_difference.cpp 30719 2010-07-09 20:28:41Z rusu $
00035  *
00036  */
00037 
00046 // ROS core
00047 #include <ros/ros.h>
00048 
00049 #include "pcl/io/pcd_io.h"
00050 #include "pcl/point_types.h"
00051 #include <pcl/segmentation/segment_differences.h>
00052 
00053 #include "pcl_ros/publisher.h"
00054 #include <pcl/filters/radius_outlier_removal.h>
00055 #include "pcl/segmentation/extract_clusters.h"
00056 #include <tf/transform_listener.h>
00057 #include "pcl/common/common.h"
00058 #include "pcl_ros/transforms.h"
00059 
00060 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00061 typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
00062 
00063 using namespace std;
00064 
00065 class SegmentDifferencesNode
00066 {
00067 protected:
00068   ros::NodeHandle nh_;
00069   
00070 public:
00071   string output_cloud_topic_, input_cloud_topic_;
00072   string output_filtered_cloud_topic_;
00073   
00074   pcl_ros::Publisher<pcl::PointXYZ> pub_diff_;
00075   pcl_ros::Publisher<pcl::PointXYZ> pub_filtered_;
00076   ros::Subscriber sub_;
00077   // Create the segmentation object
00078   pcl::SegmentDifferences <pcl::PointXYZ> seg_;
00079   pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem_;
00080   pcl::EuclideanClusterExtraction<pcl::PointXYZ> cluster_;
00081   KdTreePtr clusters_tree_;
00082   double rate_;
00083   int counter_;
00084   double distance_threshold_;
00085   bool segment_, take_first_cloud_, save_segmented_cloud_;
00086   double object_cluster_tolerance_;
00087   int object_cluster_min_size_, object_cluster_max_size_;
00088   tf::TransformListener listener_; 
00090   SegmentDifferencesNode  (ros::NodeHandle &n) : nh_(n)
00091   {
00092     // Maximum number of outgoing messages to be queued for delivery to subscribers = 1
00093     nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/cloud_pcd"));
00094     nh_.param("output_cloud_topic", output_cloud_topic_, std::string("difference"));
00095     nh_.param("output_filtered_cloud_topic", output_filtered_cloud_topic_, std::string("difference_filtered"));
00096     nh_.param("distance_threshold", distance_threshold_, 0.01);
00097     nh_.param("save_segmented_cloud_", save_segmented_cloud_, true);
00098     ROS_INFO ("Distance threshold set to %lf.", distance_threshold_);
00099     pub_diff_.advertise (nh_, output_cloud_topic_.c_str (), 1);
00100     ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_cloud_topic_).c_str ());
00101     pub_filtered_.advertise (nh_, output_filtered_cloud_topic_.c_str (), 1);
00102     ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_filtered_cloud_topic_).c_str ());
00103     sub_ = nh_.subscribe (input_cloud_topic_, 1,  &SegmentDifferencesNode::cloud_cb, this);
00104     ROS_INFO ("Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ());
00105     nh_.param("object_cluster_tolerance", object_cluster_tolerance_, 0.1);
00106     //min 100 points
00107     nh_.param("object_cluster_min_size", object_cluster_min_size_, 500);
00108     //set PCL classes
00109     seg_.setDistanceThreshold (distance_threshold_);
00110     clusters_tree_ = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
00111     clusters_tree_->setEpsilon (1);
00112     rate_ = 1;
00113     counter_ = 0;
00114     segment_ = take_first_cloud_ = false;
00115   }
00116 
00118   // cloud_cb (!)
00119   void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
00120   {
00121     pcl::PointCloud<pcl::PointXYZ> cloud_in;
00122     pcl::PointCloud<pcl::PointXYZ> output;
00123     pcl::PointCloud<pcl::PointXYZ> output_filtered;
00124     pcl::fromROSMsg(*pc, cloud_in);
00125     nh_.getParam("/segment_difference_interactive/segment", segment_);
00126     nh_.getParam("/segment_difference_interactive/take_first_cloud", take_first_cloud_);
00127     
00128     if (take_first_cloud_)
00129     {
00130       ROS_INFO("Setting target cloud with %ld points", cloud_in.points.size());
00131       //seg_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00132       seg_.setTargetCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00133       counter_++;
00134       nh_.setParam("/segment_difference_interactive/take_first_cloud", false);
00135       pub_diff_.publish (cloud_in);
00136     }
00137     else if (segment_)
00138     {
00139       ROS_INFO("Setting input cloud with %ld points", cloud_in.points.size());
00140       //seg_.setTargetCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00141       seg_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00142       seg_.segment (output);
00143       //counter_ = 0;
00144       outrem_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(output));
00145       outrem_.setRadiusSearch (0.02);
00146       outrem_.setMinNeighborsInRadius (10);
00147       outrem_.filter (output_filtered);
00148       
00149       //cluster
00150       std::vector<pcl::PointIndices> clusters;
00151       cluster_.setInputCloud (boost::make_shared<PointCloud>(output_filtered));
00152       cluster_.setClusterTolerance (object_cluster_tolerance_);
00153       cluster_.setMinClusterSize (object_cluster_min_size_);
00154       //    cluster_.setMaxClusterSize (object_cluster_max_size_);
00155       cluster_.setSearchMethod (clusters_tree_);
00156       cluster_.extract (clusters);
00157       ROS_INFO("[SegmentDifferencesNode:] Found %ld cluster", clusters.size());
00158 
00159       //get robot's pose
00160       tf::StampedTransform transform;
00161       listener_.waitForTransform("map", "base_laser_link", ros::Time(), ros::Duration(10.0));
00162       try
00163         {
00164           listener_.lookupTransform("map", "base_laser_link", ros::Time(), transform);
00165         }
00166       catch (tf::TransformException ex)
00167         {
00168           ROS_ERROR("getTransformIn %s",ex.what());
00169         }
00170       
00171       //get closest cluster
00172       pcl::PointCloud<pcl::PointXYZ> cloud_object_clustered;
00173       btVector3 robot_position = transform.getOrigin();
00174       btVector3 cluster_center;
00175       pcl::PointXYZ point_min;
00176       pcl::PointXYZ point_max;
00177       btScalar dist = 100.00;
00178       int closest_cluster = 0;
00179       if (clusters.size() > 0)
00180         {
00181           for (unsigned int i = 0; i < clusters.size(); i++)
00182             {
00183               pcl::copyPointCloud (output_filtered, clusters[i], cloud_object_clustered);
00184               getMinMax3D (cloud_object_clustered, point_min, point_max);
00185               //Calculate the centroid of the hull
00186               cluster_center.setX((point_max.x + point_min.x)/2);
00187               cluster_center.setY((point_max.y + point_min.y)/2);
00188               cluster_center.setZ((point_max.z + point_min.z)/2);
00189               if (dist > fabs(robot_position.distance(cluster_center)))
00190                 {
00191                   closest_cluster = i;
00192                   dist = fabs(robot_position.distance(cluster_center));
00193                 }
00194               ROS_INFO("Robot's distance to cluster %d is %fm", i, fabs(robot_position.distance(cluster_center)));
00195               cloud_object_clustered.points.clear();
00196             }
00197           ROS_INFO("Closest cluster: %d", closest_cluster);
00198           pcl::copyPointCloud (output_filtered, clusters[closest_cluster], cloud_object_clustered);
00199           ROS_INFO("Publishing difference cloud with %ld points to topic %s", cloud_object_clustered.points.size(), output_filtered_cloud_topic_.c_str());
00200           pub_filtered_.publish (cloud_object_clustered);
00201           if (save_segmented_cloud_)
00202             {
00203               pcl::PCDWriter writer;
00204               std::stringstream furniture_face;
00205               furniture_face << "furniture_face_" << ros::Time::now() << ".pcd";
00206               ROS_INFO("Writing to file: %s", furniture_face.str().c_str());
00207 
00208               bool found_transform = listener_.waitForTransform(cloud_object_clustered.header.frame_id, "map",
00209                                                           cloud_object_clustered.header.stamp, ros::Duration(10.0));
00210               if (found_transform)
00211                 {
00212                   pcl::PointCloud<pcl::PointXYZ> output_cloud;
00213                   //ROS_ASSERT_MSG(found_transform, "Could not transform to camera frame");
00214                   tf::StampedTransform transform;
00215                   listener_.lookupTransform("map", cloud_object_clustered.header.frame_id, cloud_object_clustered.header.stamp, transform);
00216                   Eigen::Matrix4f transform_matrix;                                                                                                      
00217                   pcl_ros::transformAsMatrix (transform, transform_matrix);    
00218                   pcl::transformPointCloud(cloud_object_clustered, output_cloud, transform_matrix);
00219                   writer.write (furniture_face.str(), output_cloud, false);
00220                 }
00221             }
00222         }
00223       //ROS_INFO("Publishing difference cloud with %ld points to topic %s", output.points.size(), output_filtered_cloud_topic_.c_str());
00224       //pub_filtered_.publish (output);
00225       segment_ = false;
00226       nh_.setParam("/segment_difference_interactive/segment", false);
00227     }
00228     else 
00229       return;
00230   }
00231 };
00232 
00233 int main (int argc, char** argv)
00234 {
00235   ros::init (argc, argv, "segment_difference_node");
00236   ros::NodeHandle n("~");
00237   SegmentDifferencesNode sd(n);
00238   ros::spin ();
00239   return (0);
00240 }
00241 
00242 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


autonomous_mapping
Author(s): Gheorghe Rus
autogenerated on Thu May 23 2013 08:58:16