voxel_grid.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $
00035  *
00036  */
00037 
00038 #include <pluginlib/class_list_macros.h>
00039 #include "pcl_ros/filters/voxel_grid.h"
00040 
00042 bool
00043 pcl_ros::VoxelGrid::child_init (ros::NodeHandle &nh, bool &has_service)
00044 {
00045   // Enable the dynamic reconfigure service
00046   has_service = true;
00047   srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig> > (nh);
00048   dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>::CallbackType f = boost::bind (&VoxelGrid::config_callback, this, _1, _2);
00049   srv_->setCallback (f);
00050 
00051   return (true);
00052 }
00053 
00055 void
00056 pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input, 
00057                             const IndicesPtr &indices, 
00058                             PointCloud2 &output)
00059 {
00060   boost::mutex::scoped_lock lock (mutex_);
00061   pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
00062   pcl_conversions::toPCL (*(input), *(pcl_input));
00063   impl_.setInputCloud (pcl_input);
00064   impl_.setIndices (indices);
00065   pcl::PCLPointCloud2 pcl_output;
00066   impl_.filter (pcl_output);
00067   pcl_conversions::moveFromPCL(pcl_output, output);
00068 }
00069 
00071 void
00072 pcl_ros::VoxelGrid::config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level)
00073 {
00074   boost::mutex::scoped_lock lock (mutex_);
00075 
00076   Eigen::Vector3f leaf_size = impl_.getLeafSize ();
00077 
00078   if (leaf_size[0] != config.leaf_size)
00079   {
00080     leaf_size.setConstant (config.leaf_size);
00081     NODELET_DEBUG ("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]);
00082     impl_.setLeafSize (leaf_size[0], leaf_size[1], leaf_size[2]);
00083   }
00084 
00085   double filter_min, filter_max;
00086   impl_.getFilterLimits (filter_min, filter_max);
00087   if (filter_min != config.filter_limit_min)
00088   {
00089     filter_min = config.filter_limit_min;
00090     NODELET_DEBUG ("[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", filter_min);
00091   }
00092   if (filter_max != config.filter_limit_max)
00093   {
00094     filter_max = config.filter_limit_max;
00095     NODELET_DEBUG ("[config_callback] Setting the maximum filtering value a point will be considered from to: %f.", filter_max);
00096   }
00097   impl_.setFilterLimits (filter_min, filter_max);
00098 
00099   if (impl_.getFilterLimitsNegative () != config.filter_limit_negative)
00100   {
00101     impl_.setFilterLimitsNegative (config.filter_limit_negative);
00102     NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false");
00103   }
00104 
00105   if (impl_.getFilterFieldName () != config.filter_field_name)
00106   {
00107     impl_.setFilterFieldName (config.filter_field_name);
00108     NODELET_DEBUG ("[config_callback] Setting the filter field name to: %s.", config.filter_field_name.c_str ());
00109   }
00110 
00111   // ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, we'll remove them and inherit from Filter
00112   if (tf_input_frame_ != config.input_frame)
00113   {
00114     tf_input_frame_ = config.input_frame;
00115     NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ());
00116   }
00117   if (tf_output_frame_ != config.output_frame)
00118   {
00119     tf_output_frame_ = config.output_frame;
00120     NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ());
00121   }
00122   // ]---
00123 }
00124 
00125 typedef pcl_ros::VoxelGrid VoxelGrid;
00126 PLUGINLIB_EXPORT_CLASS(VoxelGrid,nodelet::Nodelet);
00127 


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