point_cloud_aggregator.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <ros/ros.h>
29 #include <pluginlib/class_list_macros.hpp>
30 #include <nodelet/nodelet.h>
31 
32 #include <pcl/point_cloud.h>
33 #include <pcl/point_types.h>
34 #include <pcl_conversions/pcl_conversions.h>
35 #include <pcl/io/pcd_io.h>
36 
37 #include <pcl_ros/transforms.h>
38 
39 #include <tf/transform_listener.h>
40 
41 #include <sensor_msgs/PointCloud2.h>
42 
43 #include <image_transport/image_transport.h>
44 #include <image_transport/subscriber_filter.h>
45 
46 #include <message_filters/sync_policies/approximate_time.h>
47 #include <message_filters/subscriber.h>
48 #include <message_filters/sync_policies/exact_time.h>
49 
50 #include <rtabmap_conversions/MsgConversion.h>
51 #include <rtabmap/utilite/UConversion.h>
52 #include <rtabmap/core/util3d_filtering.h>
53 
54 namespace rtabmap_util
55 {
56 
63 class PointCloudAggregator : public nodelet::Nodelet
64 {
65 public:
66  PointCloudAggregator() :
67  warningThread_(0),
68  callbackCalled_(false),
69  exactSync4_(0),
70  approxSync4_(0),
71  exactSync3_(0),
72  approxSync3_(0),
73  exactSync2_(0),
74  approxSync2_(0),
75  waitForTransformDuration_(0.1),
76  xyzOutput_(false)
77  {}
78 
79  virtual ~PointCloudAggregator()
80  {
81  delete exactSync4_;
82  delete approxSync4_;
83  delete exactSync3_;
84  delete approxSync3_;
85  delete exactSync2_;
86  delete approxSync2_;
87 
88  if(warningThread_)
89  {
90  callbackCalled_=true;
91  warningThread_->join();
92  delete warningThread_;
93  }
94  }
95 
96 private:
97  virtual void onInit()
98  {
99  ros::NodeHandle & nh = getNodeHandle();
100  ros::NodeHandle & pnh = getPrivateNodeHandle();
101 
102  int queueSize = 1;
103  int syncQueueSize = 5;
104  int count = 2;
105  bool approx=true;
106  double approxSyncMaxInterval = 0.0;
107  pnh.param("topic_queue_size", queueSize, queueSize);
108  if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
109  {
110  pnh.param("queue_size", syncQueueSize, syncQueueSize);
111  ROS_WARN("Parameter \"queue_size\" has been renamed "
112  "to \"sync_queue_size\" and will be removed "
113  "in future versions! The value (%d) is copied to "
114  "\"sync_queue_size\".", syncQueueSize);
115  }
116  else
117  {
118  pnh.param("sync_queue_size", syncQueueSize, syncQueueSize);
119  }
120  pnh.param("frame_id", frameId_, frameId_);
121  pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
122  pnh.param("approx_sync", approx, approx);
123  pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
124  pnh.param("count", count, count);
125  pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
126  pnh.param("xyz_output", xyzOutput_, xyzOutput_);
127 
128  cloudSub_1_.subscribe(nh, "cloud1", queueSize);
129  cloudSub_2_.subscribe(nh, "cloud2", queueSize);
130 
131  std::string subscribedTopicsMsg;
132  if(count == 4)
133  {
134  cloudSub_3_.subscribe(nh, "cloud3", queueSize);
135  cloudSub_4_.subscribe(nh, "cloud4", queueSize);
136  if(approx)
137  {
138  approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
139  if(approxSyncMaxInterval > 0.0)
140  approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
141  approxSync4_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
142  }
143  else
144  {
145  exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
146  exactSync4_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
147  }
148  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
149  getName().c_str(),
150  approx?"approx":"exact",
151  approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
152  cloudSub_1_.getTopic().c_str(),
153  cloudSub_2_.getTopic().c_str(),
154  cloudSub_3_.getTopic().c_str(),
155  cloudSub_4_.getTopic().c_str());
156  }
157  else if(count == 3)
158  {
159  cloudSub_3_.subscribe(nh, "cloud3", queueSize);
160  if(approx)
161  {
162  approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
163  if(approxSyncMaxInterval > 0.0)
164  approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
165  approxSync3_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
166  }
167  else
168  {
169  exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
170  exactSync3_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
171  }
172  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
173  getName().c_str(),
174  approx?"approx":"exact",
175  approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
176  cloudSub_1_.getTopic().c_str(),
177  cloudSub_2_.getTopic().c_str(),
178  cloudSub_3_.getTopic().c_str());
179  }
180  else
181  {
182  if(approx)
183  {
184  approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(syncQueueSize), cloudSub_1_, cloudSub_2_);
185  if(approxSyncMaxInterval > 0.0)
186  approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
187  approxSync2_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, boost::placeholders::_1, boost::placeholders::_2));
188  }
189  else
190  {
191  exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(syncQueueSize), cloudSub_1_, cloudSub_2_);
192  exactSync2_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, boost::placeholders::_1, boost::placeholders::_2));
193  }
194  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
195  getName().c_str(),
196  approx?"approx":"exact",
197  approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
198  cloudSub_1_.getTopic().c_str(),
199  cloudSub_2_.getTopic().c_str());
200  }
201 
202  cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("combined_cloud", 1);
203 
204  warningThread_ = new boost::thread(boost::bind(&PointCloudAggregator::warningLoop, this, subscribedTopicsMsg, approx));
205  NODELET_INFO("%s", subscribedTopicsMsg.c_str());
206  }
207 
208  void clouds4_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
209  const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
210  const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3,
211  const sensor_msgs::PointCloud2ConstPtr & cloudMsg_4)
212  {
213  std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
214  clouds.push_back(cloudMsg_1);
215  clouds.push_back(cloudMsg_2);
216  clouds.push_back(cloudMsg_3);
217  clouds.push_back(cloudMsg_4);
218 
219  combineClouds(clouds);
220  }
221  void clouds3_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
222  const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
223  const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3)
224  {
225  std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
226  clouds.push_back(cloudMsg_1);
227  clouds.push_back(cloudMsg_2);
228  clouds.push_back(cloudMsg_3);
229 
230  combineClouds(clouds);
231  }
232  void clouds2_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
233  const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2)
234  {
235  std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
236  clouds.push_back(cloudMsg_1);
237  clouds.push_back(cloudMsg_2);
238 
239  combineClouds(clouds);
240  }
241  void combineClouds(const std::vector<sensor_msgs::PointCloud2ConstPtr> & cloudMsgs)
242  {
243  callbackCalled_ = true;
244  ROS_ASSERT(cloudMsgs.size() > 1);
245  if(cloudPub_.getNumSubscribers())
246  {
247  pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2);
248 
249  std::string frameId = frameId_;
250  if(!frameId.empty() && frameId.compare(cloudMsgs[0]->header.frame_id) != 0)
251  {
252  sensor_msgs::PointCloud2 tmp;
253  pcl_ros::transformPointCloud(frameId, *cloudMsgs[0], tmp, tfListener_);
254  pcl_conversions::toPCL(tmp, *output);
255  }
256  else
257  {
258  pcl_conversions::toPCL(*cloudMsgs[0], *output);
259  frameId = cloudMsgs[0]->header.frame_id;
260  }
261 
262  if(xyzOutput_ && !output->data.empty())
263  {
264  // convert only if not already XYZ cloud
265  bool hasField[4] = {false};
266  for(size_t i=0; i<output->fields.size(); ++i)
267  {
268  if(output->fields[i].name.compare("x") == 0)
269  {
270  hasField[0] = true;
271  }
272  else if(output->fields[i].name.compare("y") == 0)
273  {
274  hasField[1] = true;
275  }
276  else if(output->fields[i].name.compare("z") == 0)
277  {
278  hasField[2] = true;
279  }
280  else
281  {
282  hasField[3] = true; // other
283  break;
284  }
285  }
286  if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
287  {
288  // do nothing, already XYZ
289  }
290  else
291  {
292  pcl::PointCloud<pcl::PointXYZ> cloudxyz;
293  pcl::fromPCLPointCloud2(*output, cloudxyz);
294  pcl::toPCLPointCloud2(cloudxyz, *output);
295  }
296  }
297 
298  for(unsigned int i=1; i<cloudMsgs.size(); ++i)
299  {
300  rtabmap::Transform cloudDisplacement;
301  if(!fixedFrameId_.empty() &&
302  cloudMsgs[0]->header.stamp != cloudMsgs[i]->header.stamp)
303  {
304  // approx sync
305  cloudDisplacement = rtabmap_conversions::getMovingTransform(
306  frameId, //sourceTargetFrame
307  fixedFrameId_, //fixedFrame
308  cloudMsgs[0]->header.stamp, //stampTarget
309  cloudMsgs[i]->header.stamp, //stampSource
310  tfListener_,
311  waitForTransformDuration_);
312  }
313 
314  pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
315  if(frameId.compare(cloudMsgs[i]->header.frame_id) != 0)
316  {
317  sensor_msgs::PointCloud2 tmp;
318  pcl_ros::transformPointCloud(frameId, *cloudMsgs[i], tmp, tfListener_);
319  if(!cloudDisplacement.isNull())
320  {
321  sensor_msgs::PointCloud2 tmp2;
322  pcl_ros::transformPointCloud(cloudDisplacement.toEigen4f(), tmp, tmp2);
323  pcl_conversions::toPCL(tmp2, *cloud2);
324  }
325  else
326  {
327  pcl_conversions::toPCL(tmp, *cloud2);
328  }
329 
330  }
331  else
332  {
333  if(!cloudDisplacement.isNull())
334  {
335  sensor_msgs::PointCloud2 tmp;
336  pcl_ros::transformPointCloud(cloudDisplacement.toEigen4f(), *cloudMsgs[i], tmp);
337  pcl_conversions::toPCL(tmp, *cloud2);
338  }
339  else
340  {
341  pcl_conversions::toPCL(*cloudMsgs[i], *cloud2);
342  }
343  }
344 
345  if(!cloud2->is_dense)
346  {
347  // remove nans
348  cloud2 = rtabmap::util3d::removeNaNFromPointCloud(cloud2);
349  }
350 
351  if(xyzOutput_ && !cloud2->data.empty())
352  {
353  // convert only if not already XYZ cloud
354  bool hasField[4] = {false};
355  for(size_t i=0; i<cloud2->fields.size(); ++i)
356  {
357  if(cloud2->fields[i].name.compare("x") == 0)
358  {
359  hasField[0] = true;
360  }
361  else if(cloud2->fields[i].name.compare("y") == 0)
362  {
363  hasField[1] = true;
364  }
365  else if(cloud2->fields[i].name.compare("z") == 0)
366  {
367  hasField[2] = true;
368  }
369  else
370  {
371  hasField[3] = true; // other
372  break;
373  }
374  }
375  if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
376  {
377  // do nothing, already XYZ
378  }
379  else
380  {
381  pcl::PointCloud<pcl::PointXYZ> cloudxyz;
382  pcl::fromPCLPointCloud2(*cloud2, cloudxyz);
383  pcl::toPCLPointCloud2(cloudxyz, *cloud2);
384  }
385  }
386 
387  if(output->data.empty())
388  {
389  output = cloud2;
390  }
391  else if(!cloud2->data.empty())
392  {
393 
394  if(output->fields.size() != cloud2->fields.size())
395  {
396  ROS_WARN("%s: Input topics don't have all the "
397  "same number of fields (cloud1=%d, cloud%d=%d), concatenation "
398  "may fails. You can enable \"xyz_output\" option "
399  "to convert all inputs to XYZ.",
400  getName().c_str(),
401  (int)output->fields.size(),
402  i+1,
403  (int)output->fields.size());
404  }
405 
406  pcl::PCLPointCloud2::Ptr tmp_output(new pcl::PCLPointCloud2);
407 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
408  pcl::concatenate(*output, *cloud2, *tmp_output);
409 #else
410  pcl::concatenatePointCloud(*output, *cloud2, *tmp_output);
411 #endif
412  //Make sure row_step is the sum of both
413  tmp_output->row_step = tmp_output->width * tmp_output->point_step;
414  output = tmp_output;
415  }
416  }
417 
418  sensor_msgs::PointCloud2 rosCloud;
419  pcl_conversions::moveFromPCL(*output, rosCloud);
420  rosCloud.header.stamp = cloudMsgs[0]->header.stamp;
421  rosCloud.header.frame_id = frameId;
422  cloudPub_.publish(rosCloud);
423  }
424  }
425 
426  void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
427  {
428  ros::Duration r(5.0);
429  while(!callbackCalled_)
430  {
431  r.sleep();
432  if(!callbackCalled_)
433  {
434  ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
435  "published (\"$ rostopic hz my_topic\") and the timestamps in their "
436  "header are set. %s%s",
437  getName().c_str(),
438  approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
439  "topics should have all the exact timestamp for the callback to be called.",
440  subscribedTopicsMsg.c_str());
441  }
442  }
443  }
444 
445  boost::thread * warningThread_;
446  bool callbackCalled_;
447 
448  typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSync4Policy;
449  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproxSync4Policy;
450  typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSync3Policy;
451  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproxSync3Policy;
452  typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSync2Policy;
453  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproxSync2Policy;
454  message_filters::Synchronizer<ExactSync4Policy>* exactSync4_;
455  message_filters::Synchronizer<ApproxSync4Policy>* approxSync4_;
456  message_filters::Synchronizer<ExactSync3Policy>* exactSync3_;
457  message_filters::Synchronizer<ApproxSync3Policy>* approxSync3_;
458  message_filters::Synchronizer<ExactSync2Policy>* exactSync2_;
459  message_filters::Synchronizer<ApproxSync2Policy>* approxSync2_;
460  message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_1_;
461  message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_2_;
462  message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_3_;
463  message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_4_;
464 
465  ros::Publisher cloudPub_;
466 
467  std::string frameId_;
468  std::string fixedFrameId_;
469  double waitForTransformDuration_;
470  bool xyzOutput_;
471  tf::TransformListener tfListener_;
472 };
473 
474 PLUGINLIB_EXPORT_CLASS(rtabmap_util::PointCloudAggregator, nodelet::Nodelet);
475 }
476 
rtabmap_util::PointCloudAggregator::clouds3_callback
void clouds3_callback(const sensor_msgs::PointCloud2ConstPtr &cloudMsg_1, const sensor_msgs::PointCloud2ConstPtr &cloudMsg_2, const sensor_msgs::PointCloud2ConstPtr &cloudMsg_3)
Definition: point_cloud_aggregator.cpp:221
rtabmap::Transform::toEigen4f
Eigen::Matrix4f toEigen4f() const
rtabmap_util::PointCloudAggregator::cloudPub_
ros::Publisher cloudPub_
Definition: point_cloud_aggregator.cpp:465
rtabmap_util::PointCloudAggregator::ApproxSync2Policy
message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > ApproxSync2Policy
Definition: point_cloud_aggregator.cpp:453
rtabmap_util::PointCloudAggregator::ExactSync2Policy
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > ExactSync2Policy
Definition: point_cloud_aggregator.cpp:452
ros::Publisher
nodelet::Nodelet::getNodeHandle
ros::NodeHandle & getNodeHandle() const
message_filters::Synchronizer
rtabmap_util::PointCloudAggregator::exactSync3_
message_filters::Synchronizer< ExactSync3Policy > * exactSync3_
Definition: point_cloud_aggregator.cpp:456
rtabmap_util::PointCloudAggregator::approxSync3_
message_filters::Synchronizer< ApproxSync3Policy > * approxSync3_
Definition: point_cloud_aggregator.cpp:457
rtabmap_util::PointCloudAggregator::clouds4_callback
void clouds4_callback(const sensor_msgs::PointCloud2ConstPtr &cloudMsg_1, const sensor_msgs::PointCloud2ConstPtr &cloudMsg_2, const sensor_msgs::PointCloud2ConstPtr &cloudMsg_3, const sensor_msgs::PointCloud2ConstPtr &cloudMsg_4)
Definition: point_cloud_aggregator.cpp:208
uFormat
std::string uFormat(const char *fmt,...)
ros.h
rtabmap_util::PointCloudAggregator::cloudSub_3_
message_filters::Subscriber< sensor_msgs::PointCloud2 > cloudSub_3_
Definition: point_cloud_aggregator.cpp:462
rtabmap_util::PointCloudAggregator::PointCloudAggregator
PointCloudAggregator()
Definition: point_cloud_aggregator.cpp:66
pcl_conversions::toPCL
void toPCL(const pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
count
Index count
patrol.frameId
string frameId
Definition: patrol.py:11
rtabmap_util::PointCloudAggregator::exactSync2_
message_filters::Synchronizer< ExactSync2Policy > * exactSync2_
Definition: point_cloud_aggregator.cpp:458
rtabmap_util::PointCloudAggregator::exactSync4_
message_filters::Synchronizer< ExactSync4Policy > * exactSync4_
Definition: point_cloud_aggregator.cpp:454
rtabmap_util
Definition: MapsManager.h:49
rtabmap::Transform::isNull
bool isNull() const
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
rtabmap_util::PointCloudAggregator::ExactSync4Policy
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > ExactSync4Policy
Definition: point_cloud_aggregator.cpp:448
rtabmap_util::PointCloudAggregator::frameId_
std::string frameId_
Definition: point_cloud_aggregator.cpp:467
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
transforms.h
rtabmap_util::PointCloudAggregator::combineClouds
void combineClouds(const std::vector< sensor_msgs::PointCloud2ConstPtr > &cloudMsgs)
Definition: point_cloud_aggregator.cpp:241
message_filters::Subscriber::getTopic
std::string getTopic() const
message_filters::Subscriber< sensor_msgs::PointCloud2 >
rtabmap_util::PointCloudAggregator::clouds2_callback
void clouds2_callback(const sensor_msgs::PointCloud2ConstPtr &cloudMsg_1, const sensor_msgs::PointCloud2ConstPtr &cloudMsg_2)
Definition: point_cloud_aggregator.cpp:232
rtabmap_util::PointCloudAggregator::warningLoop
void warningLoop(const std::string &subscribedTopicsMsg, bool approxSync)
Definition: point_cloud_aggregator.cpp:426
message_filters::sync_policies::ApproximateTime
subscriber_filter.h
rtabmap_util::PointCloudAggregator::tfListener_
tf::TransformListener tfListener_
Definition: point_cloud_aggregator.cpp:471
rtabmap_util::PointCloudAggregator::cloudSub_4_
message_filters::Subscriber< sensor_msgs::PointCloud2 > cloudSub_4_
Definition: point_cloud_aggregator.cpp:463
subscriber.h
rtabmap_util::PointCloudAggregator::callbackCalled_
bool callbackCalled_
Definition: point_cloud_aggregator.cpp:446
pcl::concatenatePointCloud
bool concatenatePointCloud(const sensor_msgs::PointCloud2 &cloud1, const sensor_msgs::PointCloud2 &cloud2, sensor_msgs::PointCloud2 &cloud_out)
rtabmap_util::PointCloudAggregator::onInit
virtual void onInit()
Definition: point_cloud_aggregator.cpp:97
rtabmap::util3d::removeNaNFromPointCloud
pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT removeNaNFromPointCloud(const pcl::PCLPointCloud2::Ptr &cloud)
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
ROS_WARN
#define ROS_WARN(...)
rtabmap_util::PointCloudAggregator::ApproxSync4Policy
message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > ApproxSync4Policy
Definition: point_cloud_aggregator.cpp:449
image_transport.h
rtabmap_util::PointCloudAggregator::ApproxSync3Policy
message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > ApproxSync3Policy
Definition: point_cloud_aggregator.cpp:451
util3d_filtering.h
exact_time.h
message_filters::Subscriber::subscribe
void subscribe()
NODELET_INFO
#define NODELET_INFO(...)
transform_listener.h
rtabmap::Transform
rtabmap_util::PointCloudAggregator::~PointCloudAggregator
virtual ~PointCloudAggregator()
Definition: point_cloud_aggregator.cpp:79
pcl_ros::transformPointCloud
void transformPointCloud(const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out)
rtabmap_conversions::getMovingTransform
rtabmap::Transform getMovingTransform(const std::string &movingFrame, const std::string &fixedFrame, const ros::Time &stampFrom, const ros::Time &stampTo, tf::TransformListener &listener, double waitForTransform)
rtabmap_util::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(rtabmap_util::DisparityToDepth, nodelet::Nodelet)
nodelet::Nodelet
rtabmap_util::PointCloudAggregator::approxSync2_
message_filters::Synchronizer< ApproxSync2Policy > * approxSync2_
Definition: point_cloud_aggregator.cpp:459
rtabmap_util::PointCloudAggregator::ExactSync3Policy
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > ExactSync3Policy
Definition: point_cloud_aggregator.cpp:450
nodelet.h
rtabmap_util::PointCloudAggregator::waitForTransformDuration_
double waitForTransformDuration_
Definition: point_cloud_aggregator.cpp:469
c_str
const char * c_str(Args &&...args)
class_list_macros.hpp
rtabmap_util::PointCloudAggregator::cloudSub_2_
message_filters::Subscriber< sensor_msgs::PointCloud2 > cloudSub_2_
Definition: point_cloud_aggregator.cpp:461
tf::TransformListener
nodelet::Nodelet::getName
const std::string & getName() const
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
rtabmap_util::PointCloudAggregator
Definition: point_cloud_aggregator.cpp:63
approximate_time.h
ros::Duration::sleep
bool sleep() const
false
#define false
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
rtabmap_util::PointCloudAggregator::fixedFrameId_
std::string fixedFrameId_
Definition: point_cloud_aggregator.cpp:468
rtabmap_util::PointCloudAggregator::warningThread_
boost::thread * warningThread_
Definition: point_cloud_aggregator.cpp:445
message_filters::sync_policies::ExactTime
header
const std::string header
rtabmap_util::PointCloudAggregator::xyzOutput_
bool xyzOutput_
Definition: point_cloud_aggregator.cpp:470
UConversion.h
ROS_ASSERT
#define ROS_ASSERT(cond)
MsgConversion.h
ros::Duration
i
int i
rtabmap_util::PointCloudAggregator::cloudSub_1_
message_filters::Subscriber< sensor_msgs::PointCloud2 > cloudSub_1_
Definition: point_cloud_aggregator.cpp:460
pcl_conversions::moveFromPCL
void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
rtabmap_util::PointCloudAggregator::approxSync4_
message_filters::Synchronizer< ApproxSync4Policy > * approxSync4_
Definition: point_cloud_aggregator.cpp:455
ros::NodeHandle
pcl_conversions.h

rtabmap_util
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:40:50