project_inliers.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: 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_EXPORT_CLASS(ProjectInliers,nodelet::Nodelet);
00144 


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Thu May 5 2016 04:16:43