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/io/io.h>
39 #include "pcl_ros/transforms.h"
40 #include "pcl_ros/filters/filter.h"
41 
42 /*//#include <pcl/filters/pixel_grid.h>
43 //#include <pcl/filters/filter_dimension.h>
44 */
45 
46 /*//typedef pcl::PixelGrid PixelGrid;
47 //typedef pcl::FilterDimension FilterDimension;
48 */
49 
50 // Include the implementations instead of compiling them separately to speed up compile time
51 //#include "extract_indices.cpp"
52 //#include "passthrough.cpp"
53 //#include "project_inliers.cpp"
54 //#include "radius_outlier_removal.cpp"
55 //#include "statistical_outlier_removal.cpp"
56 //#include "voxel_grid.cpp"
57 
58 /*//PLUGINLIB_EXPORT_CLASS(PixelGrid,nodelet::Nodelet);
59 //PLUGINLIB_EXPORT_CLASS(FilterDimension,nodelet::Nodelet);
60 */
61 
63 void
64 pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices)
65 {
66  PointCloud2 output;
67  // Call the virtual method in the child
68  filter (input, indices, output);
69 
70  PointCloud2::Ptr cloud_tf (new PointCloud2 (output)); // set the output by default
71  // Check whether the user has given a different output TF frame
72  if (!tf_output_frame_.empty () && output.header.frame_id != tf_output_frame_)
73  {
74  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 ());
75  // Convert the cloud into the different frame
76  PointCloud2 cloud_transformed;
77  if (!pcl_ros::transformPointCloud (tf_output_frame_, output, cloud_transformed, tf_listener_))
78  {
79  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 ());
80  return;
81  }
82  cloud_tf.reset (new PointCloud2 (cloud_transformed));
83  }
84  if (tf_output_frame_.empty () && output.header.frame_id != tf_input_orig_frame_)
85  // no tf_output_frame given, transform the dataset to its original frame
86  {
87  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 ());
88  // Convert the cloud into the different frame
89  PointCloud2 cloud_transformed;
90  if (!pcl_ros::transformPointCloud (tf_input_orig_frame_, output, cloud_transformed, tf_listener_))
91  {
92  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 ());
93  return;
94  }
95  cloud_tf.reset (new PointCloud2 (cloud_transformed));
96  }
97 
98  // Copy timestamp to keep it
99  cloud_tf->header.stamp = input->header.stamp;
100 
101  // Publish a boost shared ptr
102  pub_output_.publish (cloud_tf);
103 }
104 
106 void
108 {
109  // If we're supposed to look for PointIndices (indices)
110  if (use_indices_)
111  {
112  // Subscribe to the input using a filter
115 
116  if (approximate_sync_)
117  {
118  sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, pcl_msgs::PointIndices> > >(max_queue_size_);
120  sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
121  }
122  else
123  {
124  sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, pcl_msgs::PointIndices> > >(max_queue_size_);
126  sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
127  }
128  }
129  else
130  // Subscribe in an old fashion to input only (no filters)
131  sub_input_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ()));
132 }
133 
135 void
137 {
138  if (use_indices_)
139  {
142  }
143  else
145 }
146 
148 void
150 {
151  // Call the super onInit ()
153 
154  // Call the child's local init
155  bool has_service = false;
156  if (!child_init (*pnh_, has_service))
157  {
158  NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ());
159  return;
160  }
161 
162  pub_output_ = advertise<PointCloud2> (*pnh_, "output", max_queue_size_);
163 
164  // Enable the dynamic reconfigure service
165  if (!has_service)
166  {
167  srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (*pnh_);
168  dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2);
169  srv_->setCallback (f);
170  }
171 
172  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ());
173 }
174 
176 void
177 pcl_ros::Filter::config_callback (pcl_ros::FilterConfig &config, uint32_t level)
178 {
179  // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
180  if (tf_input_frame_ != config.input_frame)
181  {
182  tf_input_frame_ = config.input_frame;
183  NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ());
184  }
185  if (tf_output_frame_ != config.output_frame)
186  {
187  tf_output_frame_ = config.output_frame;
188  NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ());
189  }
190 }
191 
193 void
194 pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
195 {
196  // If cloud is given, check if it's valid
197  if (!isValid (cloud))
198  {
199  NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
200  return;
201  }
202  // If indices are given, check if they are valid
203  if (indices && !isValid (indices))
204  {
205  NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
206  return;
207  }
208 
210  if (indices)
211  NODELET_DEBUG ("[%s::input_indices_callback]\n"
212  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
213  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
214  getName ().c_str (),
215  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
216  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
217  else
218  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 ());
220 
221  // Check whether the user has given a different input TF frame
222  tf_input_orig_frame_ = cloud->header.frame_id;
223  PointCloud2::ConstPtr cloud_tf;
224  if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
225  {
226  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 ());
227  // Save the original frame ID
228  // Convert the cloud into the different frame
229  PointCloud2 cloud_transformed;
230  if (!pcl_ros::transformPointCloud (tf_input_frame_, *cloud, cloud_transformed, tf_listener_))
231  {
232  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 ());
233  return;
234  }
235  cloud_tf = boost::make_shared<PointCloud2> (cloud_transformed);
236  }
237  else
238  cloud_tf = cloud;
239 
240  // Need setInputCloud () here because we have to extract x/y/z
241  IndicesPtr vindices;
242  if (indices)
243  vindices.reset (new std::vector<int> (indices->indices));
244 
245  computePublish (cloud_tf, vindices);
246 }
247 
virtual void onInit()
Nodelet initialization routine.
Definition: filter.cpp:149
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
#define NODELET_ERROR(...)
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcl_nodelet.h:203
void publish(const boost::shared_ptr< M > &message) const
boost::shared_ptr< dynamic_reconfigure::Server< pcl_ros::FilterConfig > > srv_
Pointer to a dynamic reconfigure service.
Definition: filter.h:138
virtual void config_callback(pcl_ros::FilterConfig &config, uint32_t level)
Dynamic reconfigure service callback.
Definition: filter.cpp:177
f
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloud2, PointIndices > > > sync_input_indices_a_
Definition: filter.h:142
std::string tf_input_frame_
The input TF frame the data should be transformed into, if input.header.frame_id is different...
Definition: filter.h:86
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:92
const std::string & getName() const
message_filters::Subscriber< PointIndices > sub_indices_filter_
The message filter subscriber for PointIndices.
Definition: pcl_nodelet.h:121
std::string tf_input_orig_frame_
The original data input TF frame.
Definition: filter.h:89
virtual void unsubscribe()
Lazy transport unsubscribe routine.
Definition: filter.cpp:136
sensor_msgs::PointCloud2 PointCloud2
Definition: filter.h:60
bool isValid(const PointCloud2::ConstPtr &cloud, const std::string &topic_name="input")
Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-ze...
Definition: pcl_nodelet.h:140
boost::shared_ptr< ros::NodeHandle > pnh_
virtual void filter(const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output)=0
Virtual abstract filter method. To be implemented by every child.
virtual bool child_init(ros::NodeHandle &nh, bool &has_service)
Child initialization routine.
Definition: filter.h:102
tf::TransformListener tf_listener_
TF listener object.
Definition: pcl_nodelet.h:133
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
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:71
bool approximate_sync_
True if we use an approximate time synchronizer versus an exact one (false by default).
Definition: pcl_nodelet.h:130
void input_indices_callback(const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
PointCloud2 + Indices data callback.
Definition: filter.cpp:194
message_filters::Subscriber< PointCloud2 > sub_input_filter_
Definition: filter.h:71
int max_queue_size_
The maximum queue size (default: 3).
Definition: pcl_nodelet.h:127
ros::Publisher pub_output_
The output PointCloud publisher.
Definition: pcl_nodelet.h:124
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ExactTime< PointCloud2, PointIndices > > > sync_input_indices_e_
Synchronized input, and indices.
Definition: filter.h:141
#define NODELET_DEBUG(...)
void computePublish(const PointCloud2::ConstPtr &input, const IndicesPtr &indices)
Call the child filter () method, optionally transform the result, and publish it. ...
Definition: filter.cpp:64
bool use_indices_
Set to true if point indices are used.
Definition: pcl_nodelet.h:94
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:83
ros::Subscriber sub_input_
The input PointCloud subscriber.
Definition: filter.h:69
virtual void subscribe()
Lazy transport subscribe routine.
Definition: filter.cpp:107


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Mon Jun 10 2019 14:19:18