47 srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig> > (nh);
49 srv_->setCallback (f);
60 boost::mutex::scoped_lock lock (
mutex_);
61 pcl::PCLPointCloud2::Ptr pcl_input(
new pcl::PCLPointCloud2);
63 impl_.setInputCloud (pcl_input);
64 impl_.setIndices (indices);
65 pcl::PCLPointCloud2 pcl_output;
66 impl_.filter (pcl_output);
74 boost::mutex::scoped_lock lock (
mutex_);
76 Eigen::Vector3f leaf_size =
impl_.getLeafSize ();
78 if (leaf_size[0] != config.leaf_size)
80 leaf_size.setConstant (config.leaf_size);
81 NODELET_DEBUG (
"[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]);
82 impl_.setLeafSize (leaf_size[0], leaf_size[1], leaf_size[2]);
85 double filter_min, filter_max;
86 impl_.getFilterLimits (filter_min, filter_max);
87 if (filter_min != config.filter_limit_min)
89 filter_min = config.filter_limit_min;
90 NODELET_DEBUG (
"[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", filter_min);
92 if (filter_max != config.filter_limit_max)
94 filter_max = config.filter_limit_max;
95 NODELET_DEBUG (
"[config_callback] Setting the maximum filtering value a point will be considered from to: %f.", filter_max);
97 impl_.setFilterLimits (filter_min, filter_max);
99 if (
impl_.getFilterLimitsNegative () != config.filter_limit_negative)
101 impl_.setFilterLimitsNegative (config.filter_limit_negative);
102 NODELET_DEBUG (
"[%s::config_callback] Setting the filter negative flag to: %s.",
getName ().c_str (), config.filter_limit_negative ?
"true" :
"false");
105 if (
impl_.getFilterFieldName () != config.filter_field_name)
107 impl_.setFilterFieldName (config.filter_field_name);
108 NODELET_DEBUG (
"[config_callback] Setting the filter field name to: %s.", config.filter_field_name.c_str ());
virtual void filter(const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output)
Call the actual filter.
pcl_ros::VoxelGrid VoxelGrid
std::string tf_input_frame_
The input TF frame the data should be transformed into, if input.header.frame_id is different...
bool child_init(ros::NodeHandle &nh, bool &has_service)
Child initialization routine.
std::string tf_output_frame_
The output TF frame the data should be transformed into, if input.header.frame_id is different...
const std::string & getName() const
boost::shared_ptr< dynamic_reconfigure::Server< pcl_ros::VoxelGridConfig > > srv_
Pointer to a dynamic reconfigure service.
sensor_msgs::PointCloud2 PointCloud2
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
void config_callback(pcl_ros::VoxelGridConfig &config, uint32_t level)
Dynamic reconfigure callback.
boost::mutex mutex_
Internal mutex.
pcl::VoxelGrid< pcl::PCLPointCloud2 > impl_
The PCL filter implementation used.
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
PLUGINLIB_EXPORT_CLASS(VoxelGrid, nodelet::Nodelet)
#define NODELET_DEBUG(...)
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)