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


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Thu Jun 6 2019 21:01:44