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 
00062   impl_.setInputCloud (input);
00063   impl_.setIndices (indices);
00064   impl_.filter (output);
00065 }
00066 
00068 void
00069 pcl_ros::VoxelGrid::config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level)
00070 {
00071   boost::mutex::scoped_lock lock (mutex_);
00072 
00073   Eigen::Vector3f leaf_size = impl_.getLeafSize ();
00074 
00075   if (leaf_size[0] != config.leaf_size)
00076   {
00077     leaf_size.setConstant (config.leaf_size);
00078     NODELET_DEBUG ("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]);
00079     impl_.setLeafSize (leaf_size[0], leaf_size[1], leaf_size[2]);
00080   }
00081 
00082   double filter_min, filter_max;
00083   impl_.getFilterLimits (filter_min, filter_max);
00084   if (filter_min != config.filter_limit_min)
00085   {
00086     filter_min = config.filter_limit_min;
00087     NODELET_DEBUG ("[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", filter_min);
00088   }
00089   if (filter_max != config.filter_limit_max)
00090   {
00091     filter_max = config.filter_limit_max;
00092     NODELET_DEBUG ("[config_callback] Setting the maximum filtering value a point will be considered from to: %f.", filter_max);
00093   }
00094   impl_.setFilterLimits (filter_min, filter_max);
00095 
00096   if (impl_.getFilterLimitsNegative () != config.filter_limit_negative)
00097   {
00098     impl_.setFilterLimitsNegative (config.filter_limit_negative);
00099     NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false");
00100   }
00101 
00102   if (impl_.getFilterFieldName () != config.filter_field_name)
00103   {
00104     impl_.setFilterFieldName (config.filter_field_name);
00105     NODELET_DEBUG ("[config_callback] Setting the filter field name to: %s.", config.filter_field_name.c_str ());
00106   }
00107 
00108   // ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, we'll remove them and inherit from Filter
00109   if (tf_input_frame_ != config.input_frame)
00110   {
00111     tf_input_frame_ = config.input_frame;
00112     NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ());
00113   }
00114   if (tf_output_frame_ != config.output_frame)
00115   {
00116     tf_output_frame_ = config.output_frame;
00117     NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ());
00118   }
00119   // ]---
00120 }
00121 
00122 typedef pcl_ros::VoxelGrid VoxelGrid;
00123 PLUGINLIB_DECLARE_CLASS (pcl, VoxelGrid, VoxelGrid, nodelet::Nodelet);
00124 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


pcl_ros
Author(s): Maintained by Radu Bogdan Rusu
autogenerated on Tue Mar 5 2013 13:54:41