Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
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
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