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_ros/subscriber.h"
00055 
00056 
00057 #include <pcl/filters/radius_outlier_removal.h>
00058 using namespace std;
00059 
00060 class SegmentDifferencesNode
00061 {
00062 protected:
00063   ros::NodeHandle nh_;
00064   
00065 public:
00066   string output_cloud_topic_, input_cloud_topic_;
00067   string output_filtered_cloud_topic_;
00068   
00069   pcl_ros::Publisher<pcl::PointXYZ> pub_diff_;
00070   pcl_ros::Publisher<pcl::PointXYZ> pub_filtered_;
00071   pcl_ros::Subscriber<sensor_msgs::PointCloud2> sub_;
00072   // Create the segmentation object
00073   pcl::SegmentDifferences <pcl::PointXYZ> seg_;
00074   pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem_;
00075   double rate_;
00076   int counter_;
00077   double distance_threshold_;
00079   SegmentDifferencesNode  (ros::NodeHandle &n) : nh_(n)
00080   {
00081     // Maximum number of outgoing messages to be queued for delivery to subscribers = 1
00082     nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/cloud_pcd"));
00083     nh_.param("output_cloud_topic", output_cloud_topic_, std::string("difference"));
00084     nh_.param("output_filtered_cloud_topic", output_filtered_cloud_topic_, std::string("difference_filtered"));
00085     nh_.param("distance_threshold", distance_threshold_, 0.0005);
00086     ROS_INFO ("Distance threshold set to %lf.", distance_threshold_);
00087     pub_diff_.advertise (nh_, output_cloud_topic_.c_str (), 1);
00088     ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_cloud_topic_).c_str ());
00089     pub_filtered_.advertise (nh_, output_filtered_cloud_topic_.c_str (), 1);
00090     ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_filtered_cloud_topic_).c_str ());
00091 
00092     sub_.subscribe (nh_, input_cloud_topic_, 1,  boost::bind (&SegmentDifferencesNode::cloud_cb, this, _1));
00093     ROS_INFO ("Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ());
00094     //set PCL classes
00095     seg_.setDistanceThreshold (distance_threshold_);
00096     
00097     rate_ = 1;
00098     counter_ = 0;
00099   }
00100 
00102   // cloud_cb (!)
00103   void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
00104   {
00105     pcl::PointCloud<pcl::PointXYZ> cloud_in;
00106     pcl::PointCloud<pcl::PointXYZ> output;
00107     pcl::PointCloud<pcl::PointXYZ> output_filtered;
00108     pcl::fromROSMsg(*pc, cloud_in);
00109     
00110     if (counter_ == 0)
00111     {
00112       ROS_INFO("Setting input cloud with %ld points", cloud_in.points.size());
00113       seg_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00114       counter_++;
00115     }
00116     else
00117     {
00118       ROS_INFO("Setting target cloud with %ld points", cloud_in.points.size());
00119       seg_.setTargetCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00120       seg_.segment (output);
00121       counter_ = 0;
00122       ROS_INFO("Publishing difference cloud with %ld points", output.points.size());
00123       pub_diff_.publish (output);
00124       outrem_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(output));
00125       outrem_.setRadiusSearch (0.02);
00126       outrem_.setMinNeighborsInRadius (15);
00127       outrem_.filter (output_filtered);
00128       pub_filtered_.publish (output_filtered);
00129     }
00130   }
00131 };
00132 
00133 int main (int argc, char** argv)
00134 {
00135   ros::init (argc, argv, "segment_difference_node");
00136   ros::NodeHandle n("~");
00137   SegmentDifferencesNode sd(n);
00138   ros::spin ();
00139   return (0);
00140 }
00141 
00142 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_cloud_algos
Author(s): Nico Blodow, Dejan Pangercic, Zoltan-Csaba Marton
autogenerated on Thu May 23 2013 15:44:49