$search
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