$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: passthrough.cpp 36194 2011-02-23 07:49:21Z rusu $ 00035 * 00036 */ 00037 00038 #include <pluginlib/class_list_macros.h> 00039 #include "pcl_ros/filters/passthrough.h" 00040 00042 bool 00043 pcl_ros::PassThrough::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::FilterConfig> > (nh); 00048 dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&PassThrough::config_callback, this, _1, _2); 00049 srv_->setCallback (f); 00050 00051 return (true); 00052 } 00053 00055 void 00056 pcl_ros::PassThrough::config_callback (pcl_ros::FilterConfig &config, uint32_t level) 00057 { 00058 boost::mutex::scoped_lock lock (mutex_); 00059 00060 double filter_min, filter_max; 00061 impl_.getFilterLimits (filter_min, filter_max); 00062 00063 // Check the current values for filter min-max 00064 if (filter_min != config.filter_limit_min) 00065 { 00066 filter_min = config.filter_limit_min; 00067 NODELET_DEBUG ("[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_min); 00068 // Set the filter min-max if different 00069 impl_.setFilterLimits (filter_min, filter_max); 00070 } 00071 // Check the current values for filter min-max 00072 if (filter_max != config.filter_limit_max) 00073 { 00074 filter_max = config.filter_limit_max; 00075 NODELET_DEBUG ("[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.", getName ().c_str (), filter_max); 00076 // Set the filter min-max if different 00077 impl_.setFilterLimits (filter_min, filter_max); 00078 } 00079 00080 // Check the current value for the filter field 00081 //std::string filter_field = impl_.getFilterFieldName (); 00082 if (impl_.getFilterFieldName () != config.filter_field_name) 00083 { 00084 // Set the filter field if different 00085 impl_.setFilterFieldName (config.filter_field_name); 00086 NODELET_DEBUG ("[%s::config_callback] Setting the filter field name to: %s.", getName ().c_str (), config.filter_field_name.c_str ()); 00087 } 00088 00089 // Check the current value for keep_organized 00090 if (impl_.getKeepOrganized () != config.keep_organized) 00091 { 00092 NODELET_DEBUG ("[%s::config_callback] Setting the filter keep_organized value to: %s.", getName ().c_str (), config.keep_organized ? "true" : "false"); 00093 // Call the virtual method in the child 00094 impl_.setKeepOrganized (config.keep_organized); 00095 } 00096 00097 // Check the current value for the negative flag 00098 if (impl_.getFilterLimitsNegative () != config.filter_limit_negative) 00099 { 00100 NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false"); 00101 // Call the virtual method in the child 00102 impl_.setFilterLimitsNegative (config.filter_limit_negative); 00103 } 00104 00105 // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL 00106 if (tf_input_frame_ != config.input_frame) 00107 { 00108 tf_input_frame_ = config.input_frame; 00109 NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ()); 00110 } 00111 if (tf_output_frame_ != config.output_frame) 00112 { 00113 tf_output_frame_ = config.output_frame; 00114 NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ()); 00115 } 00116 } 00117 00118 typedef pcl_ros::PassThrough PassThrough; 00119 PLUGINLIB_DECLARE_CLASS (pcl, PassThrough, PassThrough, nodelet::Nodelet); 00120