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_ros/transforms.h"
41 
43 
45 void
47 {
49 
50  // ---[ Mandatory parameters
51  pnh_->getParam ("output_frame", output_frame_);
52  pnh_->getParam ("approximate_sync", approximate_sync_);
53 
54  if (output_frame_.empty ())
55  {
56  NODELET_ERROR ("[onInit] Need an 'output_frame' parameter to be set before continuing!");
57  return;
58  }
59 
60  if (!pnh_->getParam ("input_topics", input_topics_))
61  {
62  NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!");
63  return;
64  }
66  {
67  NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
68  return;
69  }
70  if (input_topics_.size () == 1)
71  {
72  NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue.");
73  return;
74  }
75  if (input_topics_.size () > 8)
76  {
77  NODELET_ERROR ("[onInit] More than 8 topics passed!");
78  return;
79  }
80 
81  // ---[ Optional parameters
82  pnh_->getParam ("max_queue_size", maximum_queue_size_);
83 
84  // Output
85  pub_output_ = advertise<PointCloud2> (*pnh_, "output", maximum_queue_size_);
86 
88 }
89 
91 void
93 {
94  ROS_INFO_STREAM ("Subscribing to " << input_topics_.size () << " user given topics as inputs:");
95  for (int d = 0; d < input_topics_.size (); ++d)
96  ROS_INFO_STREAM (" - " << (std::string)(input_topics_[d]));
97 
98  // Subscribe to the filters
99  filters_.resize (input_topics_.size ());
100 
101  // 8 topics
102  if (approximate_sync_)
106  > (maximum_queue_size_));
107  else
111  > (maximum_queue_size_));
112 
113  // First input_topics_.size () filters are valid
114  for (int d = 0; d < input_topics_.size (); ++d)
115  {
116  filters_[d].reset (new message_filters::Subscriber<PointCloud2> ());
117  filters_[d]->subscribe (*pnh_, (std::string)(input_topics_[d]), maximum_queue_size_);
118  }
119 
120  // Bogus null filter
121  filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1));
122 
123  switch (input_topics_.size ())
124  {
125  case 2:
126  {
127  if (approximate_sync_)
128  ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
129  else
130  ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
131  break;
132  }
133  case 3:
134  {
135  if (approximate_sync_)
136  ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
137  else
138  ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
139  break;
140  }
141  case 4:
142  {
143  if (approximate_sync_)
144  ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
145  else
146  ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
147  break;
148  }
149  case 5:
150  {
151  if (approximate_sync_)
152  ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
153  else
154  ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
155  break;
156  }
157  case 6:
158  {
159  if (approximate_sync_)
160  ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
161  else
162  ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
163  break;
164  }
165  case 7:
166  {
167  if (approximate_sync_)
168  ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
169  else
170  ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
171  break;
172  }
173  case 8:
174  {
175  if (approximate_sync_)
176  ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
177  else
178  ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
179  break;
180  }
181  default:
182  {
183  NODELET_FATAL ("Invalid 'input_topics' parameter given!");
184  return;
185  }
186  }
187 
188  if (approximate_sync_)
189  ts_a_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
190  else
191  ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
192 }
193 
195 void
197 {
198  for (size_t d = 0; d < filters_.size (); ++d)
199  {
200  filters_[d]->unsubscribe ();
201  }
202 }
203 
205 void
207 {
208  //ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ());
209  PointCloud2::Ptr in1_t (new PointCloud2 ());
210  PointCloud2::Ptr in2_t (new PointCloud2 ());
211 
212  // Transform the point clouds into the specified output frame
213  if (output_frame_ != in1.header.frame_id)
214  {
215  if (!pcl_ros::transformPointCloud (output_frame_, in1, *in1_t, tf_))
216  {
217  NODELET_ERROR ("[%s::combineClouds] Error converting first input dataset from %s to %s.", getName ().c_str (), in1.header.frame_id.c_str (), output_frame_.c_str ());
218  return;
219  }
220  }
221  else
222  {
223  in1_t = boost::make_shared<PointCloud2> (in1);
224  }
225 
226  if (output_frame_ != in2.header.frame_id)
227  {
228  if (!pcl_ros::transformPointCloud (output_frame_, in2, *in2_t, tf_))
229  {
230  NODELET_ERROR ("[%s::combineClouds] Error converting second input dataset from %s to %s.", getName ().c_str (), in2.header.frame_id.c_str (), output_frame_.c_str ());
231  return;
232  }
233  }
234  else
235  {
236  in2_t = boost::make_shared<PointCloud2> (in2);
237  }
238 
239  // Concatenate the results
240  pcl::concatenatePointCloud (*in1_t, *in2_t, out);
241  // Copy header
242  out.header.stamp = in1.header.stamp;
243 }
244 
246 void
248  const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2,
249  const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4,
250  const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6,
251  const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8)
252 {
253  PointCloud2::Ptr out1 (new PointCloud2 ());
254  PointCloud2::Ptr out2 (new PointCloud2 ());
256  if (in3 && in3->width * in3->height > 0)
257  {
259  out1 = out2;
260  if (in4 && in4->width * in4->height > 0)
261  {
263  if (in5 && in5->width * in5->height > 0)
264  {
266  out1 = out2;
267  if (in6 && in6->width * in6->height > 0)
268  {
270  if (in7 && in7->width * in7->height > 0)
271  {
273  out1 = out2;
274  if (in8 && in8->width * in8->height > 0)
275  {
277  }
278  }
279  }
280  }
281  }
282  }
283  pub_output_.publish (boost::make_shared<PointCloud2> (*out1));
284 }
285 
288 
XmlRpc::XmlRpcValue::size
int size() const
NODELET_FATAL
#define NODELET_FATAL(...)
concatenate_data.h
PointCloudConcatenateDataSynchronizer
pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer
Definition: concatenate_data.cpp:286
NODELET_ERROR
#define NODELET_ERROR(...)
nodelet_topic_tools::NodeletLazy::pnh_
boost::shared_ptr< ros::NodeHandle > pnh_
message_filters::Synchronizer
pcl_ros::PointCloudConcatenateDataSynchronizer::onInit
void onInit()
Definition: concatenate_data.cpp:46
pcl_ros::PointCloudConcatenateDataSynchronizer::maximum_queue_size_
int maximum_queue_size_
The maximum number of messages that we can store in the queue.
Definition: concatenate_data.h:87
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
pcl_ros::PointCloudConcatenateDataSynchronizer::output_frame_
std::string output_frame_
Output TF frame the concatenated points should be transformed to.
Definition: concatenate_data.h:96
pcl_ros::PointCloudConcatenateDataSynchronizer::input_topics_
XmlRpc::XmlRpcValue input_topics_
Input point cloud topics.
Definition: concatenate_data.h:99
pcl_ros::PointCloudConcatenateDataSynchronizer::approximate_sync_
bool approximate_sync_
True if we use an approximate time synchronizer versus an exact one (false by default).
Definition: concatenate_data.h:90
pcl_ros::PointCloudConcatenateDataSynchronizer::input_callback
void input_callback(const PointCloud2ConstPtr &input)
Input point cloud callback. Because we want to use the same synchronizer object, we push back empty e...
Definition: concatenate_data.h:119
pcl_ros::PointCloudConcatenateDataSynchronizer::PointCloud2
sensor_msgs::PointCloud2 PointCloud2
Definition: concatenate_data.h:63
pcl_ros::PointCloudConcatenateDataSynchronizer::unsubscribe
void unsubscribe()
Definition: concatenate_data.cpp:196
message_filters::Subscriber< PointCloud2 >
transforms.h
pcl_ros::PointCloudConcatenateDataSynchronizer::input
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.
Definition: concatenate_data.cpp:247
message_filters::sync_policies::ApproximateTime
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
nodelet_topic_tools::NodeletLazy::onInit
virtual void onInit()
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::concatenatePointCloud
bool concatenatePointCloud(const sensor_msgs::PointCloud2 &cloud1, const sensor_msgs::PointCloud2 &cloud2, sensor_msgs::PointCloud2 &cloud_out)
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds
void combineClouds(const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out)
Definition: concatenate_data.cpp:206
d
d
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
XmlRpc::XmlRpcValue::getType
const Type & getType() const
XmlRpc::XmlRpcValue::TypeArray
TypeArray
nodelet::Nodelet
nodelet_topic_tools::NodeletLazy::onInitPostProcess
virtual void onInitPostProcess()
pcl_ros::PointCloudConcatenateDataSynchronizer
PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set ...
Definition: concatenate_data.h:60
pcl_ros::PointCloudConcatenateDataSynchronizer::pub_output_
ros::Publisher pub_output_
The output PointCloud publisher.
Definition: concatenate_data.h:84
class_list_macros.hpp
message_filters::sync_policies::ExactTime
pcl_ros::PointCloudConcatenateDataSynchronizer::subscribe
void subscribe()
Definition: concatenate_data.cpp:92
pcl_conversions.h


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