extract_polygonal_prism_data.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: extract_polygonal_prism_data.hpp 32996 2010-09-30 23:42:11Z rusu $
00035  *
00036  */
00037 
00038 #include <pluginlib/class_list_macros.h>
00039 #include "pcl_ros/transforms.h"
00040 #include "pcl_ros/segmentation/extract_polygonal_prism_data.h"
00041 #include <pcl/io/io.h>
00042 
00044 void
00045 pcl_ros::ExtractPolygonalPrismData::onInit ()
00046 {
00047   // Call the super onInit ()
00048   PCLNodelet::onInit ();
00049 
00050   sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_);
00051   sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00052 
00053   // Enable the dynamic reconfigure service
00054   srv_ = boost::make_shared <dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > (*pnh_);
00055   dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2);
00056   srv_->setCallback (f);
00057 
00058   // Create the objects here
00059   if (approximate_sync_)
00060     sync_input_hull_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
00061   else
00062     sync_input_hull_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
00063 
00064   if (use_indices_)
00065   {
00066     sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00067     if (approximate_sync_)
00068       sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
00069     else
00070       sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
00071   }
00072   else
00073   {
00074     sub_input_filter_.registerCallback (bind (&ExtractPolygonalPrismData::input_callback, this, _1));
00075 
00076     if (approximate_sync_)
00077       sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, nf_);
00078     else
00079       sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, nf_);
00080   }
00081   // Register callbacks
00082   if (approximate_sync_)
00083     sync_input_hull_indices_a_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
00084   else
00085     sync_input_hull_indices_e_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
00086 
00087   // Advertise the output topics
00088   pub_output_ = pnh_->advertise<PointIndices> ("output", max_queue_size_);
00089 }
00090 
00092 void
00093 pcl_ros::ExtractPolygonalPrismData::config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level)
00094 {
00095   double height_min, height_max;
00096   impl_.getHeightLimits (height_min, height_max);
00097   if (height_min != config.height_min)
00098   {
00099     height_min = config.height_min;
00100     NODELET_DEBUG ("[%s::config_callback] Setting new minimum height to the planar model to: %f.", getName ().c_str (), height_min);
00101     impl_.setHeightLimits (height_min, height_max);
00102   }
00103   if (height_max != config.height_max)
00104   {
00105     height_max = config.height_max;
00106     NODELET_DEBUG ("[%s::config_callback] Setting new maximum height to the planar model to: %f.", getName ().c_str (), height_max);
00107     impl_.setHeightLimits (height_min, height_max);
00108   }
00109 }
00110 
00112 void
00113 pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
00114     const PointCloudConstPtr &cloud, 
00115     const PointCloudConstPtr &hull, 
00116     const PointIndicesConstPtr &indices)
00117 {
00118   // No subscribers, no work
00119   if (pub_output_.getNumSubscribers () <= 0)
00120     return;
00121 
00122   // Copy the header (stamp + frame_id)
00123   pcl::PointIndices inliers;
00124   inliers.header = cloud->header;
00125 
00126   // If cloud is given, check if it's valid
00127   if (!isValid (cloud) || !isValid (hull, "planar_hull"))
00128   {
00129     NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ());
00130     pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
00131     return;
00132   }
00133   // If indices are given, check if they are valid
00134   if (indices && !isValid (indices))
00135   {
00136     NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ());
00137     pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
00138     return;
00139   }
00140 
00142   if (indices)
00143     NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
00144                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00145                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00146                    "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00147                    getName ().c_str (), 
00148                    cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00149                    hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (),
00150                    indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00151   else
00152     NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
00153                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00154                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00155                    getName ().c_str (),
00156                    cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00157                    hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ());
00159 
00160   if (cloud->header.frame_id != hull->header.frame_id)
00161   {
00162     NODELET_DEBUG ("[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.", getName ().c_str (), hull->header.frame_id.c_str (), cloud->header.frame_id.c_str ());
00163     PointCloud planar_hull;
00164     if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_))
00165     {
00166       // Publish empty before return
00167       pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
00168       return;
00169     }
00170     impl_.setInputPlanarHull (planar_hull.makeShared ());
00171   }
00172   else
00173     impl_.setInputPlanarHull (hull);
00174 
00175   IndicesPtr indices_ptr;
00176   if (indices && !indices->header.frame_id.empty ())
00177     indices_ptr.reset (new std::vector<int> (indices->indices));
00178 
00179   impl_.setInputCloud (cloud);
00180   impl_.setIndices (indices_ptr);
00181 
00182   // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
00183   if (!cloud->points.empty ())
00184     impl_.segment (inliers);
00185   // Enforce that the TF frame and the timestamp are copied
00186   inliers.header = cloud->header;
00187   pub_output_.publish (boost::make_shared<const PointIndices> (inliers));
00188   NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ());
00189 }
00190 
00191 typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData;
00192 PLUGINLIB_DECLARE_CLASS (pcl, ExtractPolygonalPrismData, ExtractPolygonalPrismData, nodelet::Nodelet);
00193 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


pcl_ros
Author(s): Maintained by Radu Bogdan Rusu
autogenerated on Tue Mar 5 2013 13:54:41