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_differences.cpp 35361 2011-01-20 04:34:49Z rusu $
00035  *
00036  */
00037 
00038 #include <pluginlib/class_list_macros.h>
00039 #include "pcl_ros/segmentation/segment_differences.h"
00040 #include <pcl/io/io.h>
00041 
00043 void
00044 pcl_ros::SegmentDifferences::onInit ()
00045 {
00046   // Call the super onInit ()
00047   PCLNodelet::onInit ();
00048 
00049   pub_output_ = pnh_->advertise<PointCloud> ("output", max_queue_size_);
00050 
00051   // Subscribe to the input using a filter
00052   sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00053   sub_target_filter_.subscribe (*pnh_, "target", max_queue_size_);
00054 
00055   // Enable the dynamic reconfigure service
00056   srv_ = boost::make_shared <dynamic_reconfigure::Server<SegmentDifferencesConfig> > (*pnh_);
00057   dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f =  boost::bind (&SegmentDifferences::config_callback, this, _1, _2);
00058   srv_->setCallback (f);
00059 
00060   if (approximate_sync_)
00061   {
00062     sync_input_target_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > (max_queue_size_);
00063     sync_input_target_a_->connectInput (sub_input_filter_, sub_target_filter_);
00064     sync_input_target_a_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
00065   }
00066   else
00067   {
00068     sync_input_target_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud> > > (max_queue_size_);
00069     sync_input_target_e_->connectInput (sub_input_filter_, sub_target_filter_);
00070     sync_input_target_e_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
00071   }
00072 
00073   NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00074                  " - max_queue_size    : %d",
00075                  getName ().c_str (),
00076                  max_queue_size_);
00077 }
00078 
00080 void
00081 pcl_ros::SegmentDifferences::config_callback (SegmentDifferencesConfig &config, uint32_t level)
00082 {
00083   if (impl_.getDistanceThreshold () != config.distance_threshold)
00084   {
00085     impl_.setDistanceThreshold (config.distance_threshold);
00086     NODELET_DEBUG ("[%s::config_callback] Setting new distance threshold to: %f.", getName ().c_str (), config.distance_threshold);
00087   }
00088 }
00089 
00090 
00092 void
00093 pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cloud, 
00094                                                     const PointCloudConstPtr &cloud_target)
00095 {
00096   if (pub_output_.getNumSubscribers () <= 0)
00097     return;
00098 
00099   if (!isValid (cloud) || !isValid (cloud_target, "target")) 
00100   {
00101     NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
00102     PointCloud output;
00103     output.header = cloud->header;
00104     pub_output_.publish (output.makeShared ());
00105     return;
00106   }
00107 
00108   NODELET_DEBUG ("[%s::input_indices_callback]\n"
00109                  "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00110                  "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00111                  getName ().c_str (),
00112                  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00113                  cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), cloud_target->header.stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ());
00114 
00115   impl_.setInputCloud (cloud);
00116   impl_.setTargetCloud (cloud_target);
00117 
00118   PointCloud output;
00119   impl_.segment (output);
00120 
00121   pub_output_.publish (output.makeShared ());
00122   NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (),
00123                      output.points.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
00124 }
00125 
00126 typedef pcl_ros::SegmentDifferences SegmentDifferences;
00127 PLUGINLIB_DECLARE_CLASS (pcl, SegmentDifferences, SegmentDifferences, nodelet::Nodelet);
00128 


pcl_ros
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:22:24