filter.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id: filter.cpp 35876 2011-02-09 01:04:36Z rusu $
35  *
36  */
37 
38 #include "pcl_ros/transforms.h"
39 #include "pcl_ros/filters/filter.h"
40 
41 /*//#include <pcl/filters/pixel_grid.h>
42 //#include <pcl/filters/filter_dimension.h>
43 */
44 
45 /*//typedef pcl::PixelGrid PixelGrid;
46 //typedef pcl::FilterDimension FilterDimension;
47 */
48 
49 // Include the implementations instead of compiling them separately to speed up compile time
50 //#include "extract_indices.cpp"
51 //#include "passthrough.cpp"
52 //#include "project_inliers.cpp"
53 //#include "radius_outlier_removal.cpp"
54 //#include "statistical_outlier_removal.cpp"
55 //#include "voxel_grid.cpp"
56 
57 /*//PLUGINLIB_EXPORT_CLASS(PixelGrid,nodelet::Nodelet);
58 //PLUGINLIB_EXPORT_CLASS(FilterDimension,nodelet::Nodelet);
59 */
60 
62 void
63 pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices)
64 {
65  PointCloud2 output;
66  // Call the virtual method in the child
67  filter (input, indices, output);
68 
69  PointCloud2::Ptr cloud_tf (new PointCloud2 (output)); // set the output by default
70  // Check whether the user has given a different output TF frame
71  if (!tf_output_frame_.empty () && output.header.frame_id != tf_output_frame_)
72  {
73  NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ());
74  // Convert the cloud into the different frame
75  PointCloud2 cloud_transformed;
76  if (!pcl_ros::transformPointCloud (tf_output_frame_, output, cloud_transformed, tf_listener_))
77  {
78  NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ());
79  return;
80  }
81  cloud_tf.reset (new PointCloud2 (cloud_transformed));
82  }
83  if (tf_output_frame_.empty () && output.header.frame_id != tf_input_orig_frame_)
84  // no tf_output_frame given, transform the dataset to its original frame
85  {
86  NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ());
87  // Convert the cloud into the different frame
88  PointCloud2 cloud_transformed;
89  if (!pcl_ros::transformPointCloud (tf_input_orig_frame_, output, cloud_transformed, tf_listener_))
90  {
91  NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ());
92  return;
93  }
94  cloud_tf.reset (new PointCloud2 (cloud_transformed));
95  }
96 
97  // Copy timestamp to keep it
98  cloud_tf->header.stamp = input->header.stamp;
99 
100  // Publish a boost shared ptr
101  pub_output_.publish (cloud_tf);
102 }
103 
105 void
107 {
108  // If we're supposed to look for PointIndices (indices)
109  if (use_indices_)
110  {
111  // Subscribe to the input using a filter
112  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
113  sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
114 
115  if (approximate_sync_)
116  {
117  sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, pcl_msgs::PointIndices> > >(max_queue_size_);
118  sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
119  sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
120  }
121  else
122  {
123  sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, pcl_msgs::PointIndices> > >(max_queue_size_);
124  sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
125  sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
126  }
127  }
128  else
129  // Subscribe in an old fashion to input only (no filters)
130  sub_input_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ()));
131 }
132 
134 void
136 {
137  if (use_indices_)
138  {
139  sub_input_filter_.unsubscribe();
140  sub_indices_filter_.unsubscribe();
141  }
142  else
143  sub_input_.shutdown();
144 }
145 
147 void
149 {
150  // Call the super onInit ()
152 
153  // Call the child's local init
154  bool has_service = false;
155  if (!child_init (*pnh_, has_service))
156  {
157  NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ());
158  return;
159  }
160 
161  pub_output_ = advertise<PointCloud2> (*pnh_, "output", max_queue_size_);
162 
163  // Enable the dynamic reconfigure service
164  if (!has_service)
165  {
166  srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (*pnh_);
167  dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2);
168  srv_->setCallback (f);
169  }
170 
171  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ());
172 }
173 
175 void
176 pcl_ros::Filter::config_callback (pcl_ros::FilterConfig &config, uint32_t /*level*/)
177 {
178  // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
179  if (tf_input_frame_ != config.input_frame)
180  {
181  tf_input_frame_ = config.input_frame;
182  NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ());
183  }
184  if (tf_output_frame_ != config.output_frame)
185  {
186  tf_output_frame_ = config.output_frame;
187  NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ());
188  }
189 }
190 
192 void
193 pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
194 {
195  // If cloud is given, check if it's valid
196  if (!isValid (cloud))
197  {
198  NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
199  return;
200  }
201  // If indices are given, check if they are valid
202  if (indices && !isValid (indices))
203  {
204  NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
205  return;
206  }
207 
209  if (indices)
210  NODELET_DEBUG ("[%s::input_indices_callback]\n"
211  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
212  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
213  getName ().c_str (),
214  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
215  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
216  else
217  NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
219 
220  // Check whether the user has given a different input TF frame
221  tf_input_orig_frame_ = cloud->header.frame_id;
222  PointCloud2::ConstPtr cloud_tf;
223  if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
224  {
225  NODELET_DEBUG ("[%s::input_indices_callback] Transforming input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
226  // Save the original frame ID
227  // Convert the cloud into the different frame
228  PointCloud2 cloud_transformed;
229  if (!pcl_ros::transformPointCloud (tf_input_frame_, *cloud, cloud_transformed, tf_listener_))
230  {
231  NODELET_ERROR ("[%s::input_indices_callback] Error converting input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
232  return;
233  }
234  cloud_tf = boost::make_shared<PointCloud2> (cloud_transformed);
235  }
236  else
237  cloud_tf = cloud;
238 
239  // Need setInputCloud () here because we have to extract x/y/z
240  IndicesPtr vindices;
241  if (indices)
242  vindices.reset (new std::vector<int> (indices->indices));
243 
244  computePublish (cloud_tf, vindices);
245 }
246 
pcl_ros::PCLNodelet::tf_listener_
tf::TransformListener tf_listener_
TF listener object.
Definition: pcl_nodelet.h:134
filter.h
NODELET_ERROR
#define NODELET_ERROR(...)
pcl_ros::Filter::onInit
virtual void onInit()
Nodelet initialization routine.
Definition: filter.cpp:148
pcl::getFieldsList
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
pcl_ros::Filter::config_callback
virtual void config_callback(pcl_ros::FilterConfig &config, uint32_t level)
Dynamic reconfigure service callback.
Definition: filter.cpp:176
pcl_ros::Filter::IndicesPtr
pcl::IndicesPtr IndicesPtr
Definition: filter.h:61
pcl_ros::PCLNodelet::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:84
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
pcl_ros::Filter::input_indices_callback
void input_indices_callback(const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
PointCloud2 + Indices data callback.
Definition: filter.cpp:193
f
f
transforms.h
pcl_ros::PCLNodelet::pub_output_
ros::Publisher pub_output_
The output PointCloud publisher.
Definition: pcl_nodelet.h:125
pcl_ros::PCLNodelet::onInit
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcl_nodelet.h:204
pcl_ros::Filter::subscribe
virtual void subscribe()
Lazy transport subscribe routine.
Definition: filter.cpp:106
pcl_ros::Filter::computePublish
void computePublish(const PointCloud2::ConstPtr &input, const IndicesPtr &indices)
Call the child filter () method, optionally transform the result, and publish it.
Definition: filter.cpp:63
pcl_ros::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
Apply a rigid transform defined by a 3D offset and a quaternion.
Definition: transforms.hpp:85
pcl_ros::Filter::tf_output_frame_
std::string tf_output_frame_
The output TF frame the data should be transformed into, if input.header.frame_id is different.
Definition: filter.h:91
pcl_ros::Filter::tf_input_orig_frame_
std::string tf_input_orig_frame_
The original data input TF frame.
Definition: filter.h:88
pcl_ros::Filter::unsubscribe
virtual void unsubscribe()
Lazy transport unsubscribe routine.
Definition: filter.cpp:135
pcl_ros::Filter::PointCloud2
sensor_msgs::PointCloud2 PointCloud2
Definition: filter.h:59
nodelet::Nodelet::getName
const std::string & getName() const
pcl_ros::Filter::filter
virtual void filter(const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output)=0
Virtual abstract filter method. To be implemented by every child.
NODELET_DEBUG
#define NODELET_DEBUG(...)


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Fri Jul 12 2024 02:54:40