$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: project_inliers.cpp 35876 2011-02-09 01:04:36Z rusu $ 00035 * 00036 */ 00037 00038 #include <pluginlib/class_list_macros.h> 00039 #include "pcl_ros/filters/project_inliers.h" 00040 #include <pcl/io/io.h> 00041 00043 void 00044 pcl_ros::ProjectInliers::onInit () 00045 { 00046 // No need to call the super onInit as we are overwriting everything 00047 //Filter<sensor_msgs::PointCloud2>::onInit (); 00048 PCLNodelet::onInit (); 00049 00050 // ---[ Mandatory parameters 00051 // The type of model to use (user given parameter). 00052 int model_type; 00053 if (!pnh_->getParam ("model_type", model_type)) 00054 { 00055 NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ()); 00056 return; 00057 } 00058 // ---[ Optional parameters 00059 // True if all data will be returned, false if only the projected inliers. Default: false. 00060 bool copy_all_data = false; 00061 00062 // True if all fields will be returned, false if only XYZ. Default: true. 00063 bool copy_all_fields = true; 00064 00065 pnh_->getParam ("copy_all_data", copy_all_data); 00066 pnh_->getParam ("copy_all_fields", copy_all_fields); 00067 00068 pub_output_ = pnh_->advertise<PointCloud2> ("output", max_queue_size_); 00069 00070 // Subscribe to the input using a filter 00071 sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_); 00072 00073 /* 00074 TODO : implement use_indices_ 00075 if (use_indices_) 00076 {*/ 00077 00078 sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_); 00079 00080 sub_model_.subscribe (*pnh_, "model", max_queue_size_); 00081 00082 if (approximate_sync_) 00083 { 00084 sync_input_indices_model_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_); 00085 sync_input_indices_model_a_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); 00086 sync_input_indices_model_a_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); 00087 } 00088 else 00089 { 00090 sync_input_indices_model_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_); 00091 sync_input_indices_model_e_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_); 00092 sync_input_indices_model_e_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3)); 00093 } 00094 00095 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" 00096 " - model_type : %d\n" 00097 " - copy_all_data : %s\n" 00098 " - copy_all_fields : %s", 00099 getName ().c_str (), 00100 model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false"); 00101 00102 // Set given parameters here 00103 impl_.setModelType (model_type); 00104 impl_.setCopyAllFields (copy_all_fields); 00105 impl_.setCopyAllData (copy_all_data); 00106 } 00107 00109 void 00110 pcl_ros::ProjectInliers::input_indices_model_callback (const PointCloud2::ConstPtr &cloud, 00111 const PointIndicesConstPtr &indices, 00112 const ModelCoefficientsConstPtr &model) 00113 { 00114 if (pub_output_.getNumSubscribers () <= 0) 00115 return; 00116 00117 if (!isValid (model) || !isValid (indices) || !isValid (cloud)) 00118 { 00119 NODELET_ERROR ("[%s::input_indices_model_callback] Invalid input!", getName ().c_str ()); 00120 return; 00121 } 00122 00123 NODELET_DEBUG ("[%s::input_indices_model_callback]\n" 00124 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 00125 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n" 00126 " - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.", 00127 getName ().c_str (), 00128 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), 00129 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("inliers").c_str (), 00130 model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName ("model").c_str ()); 00131 00132 tf_input_orig_frame_ = cloud->header.frame_id; 00133 00134 IndicesPtr vindices; 00135 if (indices) 00136 vindices.reset (new std::vector<int> (indices->indices)); 00137 00138 model_ = model; 00139 computePublish (cloud, vindices); 00140 } 00141 00142 typedef pcl_ros::ProjectInliers ProjectInliers; 00143 PLUGINLIB_DECLARE_CLASS (pcl, ProjectInliers, ProjectInliers, nodelet::Nodelet); 00144