concatenate_data.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: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $
35  *
36  */
37 
39 #include <pcl/io/io.h>
40 #include "pcl_ros/transforms.h"
42 
44 
46 void
48 {
50 
51  // ---[ Mandatory parameters
52  pnh_->getParam ("output_frame", output_frame_);
53  pnh_->getParam ("approximate_sync", approximate_sync_);
54 
55  if (output_frame_.empty ())
56  {
57  NODELET_ERROR ("[onInit] Need an 'output_frame' parameter to be set before continuing!");
58  return;
59  }
60 
61  if (!pnh_->getParam ("input_topics", input_topics_))
62  {
63  NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!");
64  return;
65  }
67  {
68  NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
69  return;
70  }
71  if (input_topics_.size () == 1)
72  {
73  NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue.");
74  return;
75  }
76  if (input_topics_.size () > 8)
77  {
78  NODELET_ERROR ("[onInit] More than 8 topics passed!");
79  return;
80  }
81 
82  // ---[ Optional parameters
83  pnh_->getParam ("max_queue_size", maximum_queue_size_);
84 
85  // Output
86  pub_output_ = advertise<PointCloud2> (*pnh_, "output", maximum_queue_size_);
87 
89 }
90 
92 void
94 {
95  ROS_INFO_STREAM ("Subscribing to " << input_topics_.size () << " user given topics as inputs:");
96  for (int d = 0; d < input_topics_.size (); ++d)
97  ROS_INFO_STREAM (" - " << (std::string)(input_topics_[d]));
98 
99  // Subscribe to the filters
100  filters_.resize (input_topics_.size ());
101 
102  // 8 topics
103  if (approximate_sync_)
107  > (maximum_queue_size_));
108  else
112  > (maximum_queue_size_));
113 
114  // First input_topics_.size () filters are valid
115  for (int d = 0; d < input_topics_.size (); ++d)
116  {
118  filters_[d]->subscribe (*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_);
119  }
120 
121  // Bogus null filter
122  filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1));
123 
124  switch (input_topics_.size ())
125  {
126  case 2:
127  {
128  if (approximate_sync_)
129  ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
130  else
131  ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
132  break;
133  }
134  case 3:
135  {
136  if (approximate_sync_)
137  ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
138  else
139  ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
140  break;
141  }
142  case 4:
143  {
144  if (approximate_sync_)
145  ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
146  else
147  ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
148  break;
149  }
150  case 5:
151  {
152  if (approximate_sync_)
153  ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
154  else
155  ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
156  break;
157  }
158  case 6:
159  {
160  if (approximate_sync_)
161  ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
162  else
163  ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
164  break;
165  }
166  case 7:
167  {
168  if (approximate_sync_)
169  ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
170  else
171  ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
172  break;
173  }
174  case 8:
175  {
176  if (approximate_sync_)
177  ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
178  else
179  ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
180  break;
181  }
182  default:
183  {
184  NODELET_FATAL ("Invalid 'input_topics' parameter given!");
185  return;
186  }
187  }
188 
189  if (approximate_sync_)
190  ts_a_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
191  else
192  ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
193 }
194 
196 void
198 {
199  for (int d = 0; d < filters_.size (); ++d)
200  {
201  filters_[d]->unsubscribe ();
202  }
203 }
204 
206 void
208 {
209  //ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ());
210  PointCloud2::Ptr in1_t (new PointCloud2 ());
211  PointCloud2::Ptr in2_t (new PointCloud2 ());
212 
213  // Transform the point clouds into the specified output frame
214  if (output_frame_ != in1.header.frame_id)
216  else
217  in1_t = boost::make_shared<PointCloud2> (in1);
218 
219  if (output_frame_ != in2.header.frame_id)
221  else
222  in2_t = boost::make_shared<PointCloud2> (in2);
223 
224  // Concatenate the results
225  pcl::concatenatePointCloud (*in1_t, *in2_t, out);
226  // Copy header
227  out.header.stamp = in1.header.stamp;
228 }
229 
231 void
233  const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2,
234  const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4,
235  const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6,
236  const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8)
237 {
238  PointCloud2::Ptr out1 (new PointCloud2 ());
239  PointCloud2::Ptr out2 (new PointCloud2 ());
241  if (in3 && in3->width * in3->height > 0)
242  {
244  out1 = out2;
245  if (in4 && in4->width * in4->height > 0)
246  {
248  if (in5 && in5->width * in5->height > 0)
249  {
251  out1 = out2;
252  if (in6 && in6->width * in6->height > 0)
253  {
255  if (in7 && in7->width * in7->height > 0)
256  {
258  out1 = out2;
259  }
260  }
261  }
262  }
263  }
264  pub_output_.publish (boost::make_shared<PointCloud2> (*out1));
265 }
266 
269 
d
XmlRpc::XmlRpcValue input_topics_
Input point cloud topics.
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
int size() const
int maximum_queue_size_
The maximum number of messages that we can store in the queue.
bool approximate_sync_
True if we use an approximate time synchronizer versus an exact one (false by default).
Type const & getType() const
std::string output_frame_
Output TF frame the concatenated points should be transformed to.
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2 > > > ts_a_
Synchronizer.
message_filters::PassThrough< PointCloud2 > nf_
Null passthrough filter, used for pushing empty elements in the synchronizer.
boost::shared_ptr< ros::NodeHandle > pnh_
PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set ...
void input(const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2, const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4, const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6, const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8)
Input callback for 8 synchronized topics.
ros::Publisher pub_output_
The output PointCloud publisher.
bool concatenatePointCloud(const sensor_msgs::PointCloud2 &cloud1, const sensor_msgs::PointCloud2 &cloud2, sensor_msgs::PointCloud2 &cloud_out)
void combineClouds(const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out)
#define ROS_INFO_STREAM(args)
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
std::vector< boost::shared_ptr< message_filters::Subscriber< PointCloud2 > > > filters_
A vector of message filters.
#define NODELET_FATAL(...)
tf::TransformListener tf_
TF listener object.
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ExactTime< PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2 > > > ts_e_
void input_callback(const PointCloud2ConstPtr &input)
Input point cloud callback. Because we want to use the same synchronizer object, we push back empty e...
PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer, nodelet::Nodelet)


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Wed Jun 5 2019 19:52:52