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_ros/MsgConversion.h>
51 #include <rtabmap/utilite/UConversion.h>
52 #include <rtabmap/core/util3d_filtering.h>
53 
54 namespace rtabmap_ros
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 = 5;
103  int count = 2;
104  bool approx=true;
105  double approxSyncMaxInterval = 0.0;
106  pnh.param("queue_size", queueSize, queueSize);
107  pnh.param("frame_id", frameId_, frameId_);
108  pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
109  pnh.param("approx_sync", approx, approx);
110  pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
111  pnh.param("count", count, count);
112  pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
113  pnh.param("xyz_output", xyzOutput_, xyzOutput_);
114 
115  cloudSub_1_.subscribe(nh, "cloud1", 1);
116  cloudSub_2_.subscribe(nh, "cloud2", 1);
117 
118  std::string subscribedTopicsMsg;
119  if(count == 4)
120  {
121  cloudSub_3_.subscribe(nh, "cloud3", 1);
122  cloudSub_4_.subscribe(nh, "cloud4", 1);
123  if(approx)
124  {
125  approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
126  if(approxSyncMaxInterval > 0.0)
127  approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
128  approxSync4_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
129  }
130  else
131  {
132  exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
133  exactSync4_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
134  }
135  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
136  getName().c_str(),
137  approx?"approx":"exact",
138  approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
139  cloudSub_1_.getTopic().c_str(),
140  cloudSub_2_.getTopic().c_str(),
141  cloudSub_3_.getTopic().c_str(),
142  cloudSub_4_.getTopic().c_str());
143  }
144  else if(count == 3)
145  {
146  cloudSub_3_.subscribe(nh, "cloud3", 1);
147  if(approx)
148  {
149  approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
150  if(approxSyncMaxInterval > 0.0)
151  approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
152  approxSync3_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
153  }
154  else
155  {
156  exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
157  exactSync3_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
158  }
159  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
160  getName().c_str(),
161  approx?"approx":"exact",
162  approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
163  cloudSub_1_.getTopic().c_str(),
164  cloudSub_2_.getTopic().c_str(),
165  cloudSub_3_.getTopic().c_str());
166  }
167  else
168  {
169  if(approx)
170  {
171  approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
172  if(approxSyncMaxInterval > 0.0)
173  approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
174  approxSync2_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, boost::placeholders::_1, boost::placeholders::_2));
175  }
176  else
177  {
178  exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
179  exactSync2_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, boost::placeholders::_1, boost::placeholders::_2));
180  }
181  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
182  getName().c_str(),
183  approx?"approx":"exact",
184  approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
185  cloudSub_1_.getTopic().c_str(),
186  cloudSub_2_.getTopic().c_str());
187  }
188 
189  cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("combined_cloud", 1);
190 
191  warningThread_ = new boost::thread(boost::bind(&PointCloudAggregator::warningLoop, this, subscribedTopicsMsg, approx));
192  NODELET_INFO("%s", subscribedTopicsMsg.c_str());
193  }
194 
195  void clouds4_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
196  const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
197  const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3,
198  const sensor_msgs::PointCloud2ConstPtr & cloudMsg_4)
199  {
200  std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
201  clouds.push_back(cloudMsg_1);
202  clouds.push_back(cloudMsg_2);
203  clouds.push_back(cloudMsg_3);
204  clouds.push_back(cloudMsg_4);
205 
206  combineClouds(clouds);
207  }
208  void clouds3_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
209  const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
210  const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3)
211  {
212  std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
213  clouds.push_back(cloudMsg_1);
214  clouds.push_back(cloudMsg_2);
215  clouds.push_back(cloudMsg_3);
216 
217  combineClouds(clouds);
218  }
219  void clouds2_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
220  const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2)
221  {
222  std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
223  clouds.push_back(cloudMsg_1);
224  clouds.push_back(cloudMsg_2);
225 
226  combineClouds(clouds);
227  }
228  void combineClouds(const std::vector<sensor_msgs::PointCloud2ConstPtr> & cloudMsgs)
229  {
230  callbackCalled_ = true;
231  ROS_ASSERT(cloudMsgs.size() > 1);
232  if(cloudPub_.getNumSubscribers())
233  {
234  pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2);
235 
236  std::string frameId = frameId_;
237  if(!frameId.empty() && frameId.compare(cloudMsgs[0]->header.frame_id) != 0)
238  {
239  sensor_msgs::PointCloud2 tmp;
240  pcl_ros::transformPointCloud(frameId, *cloudMsgs[0], tmp, tfListener_);
241  pcl_conversions::toPCL(tmp, *output);
242  }
243  else
244  {
245  pcl_conversions::toPCL(*cloudMsgs[0], *output);
246  frameId = cloudMsgs[0]->header.frame_id;
247  }
248 
249  if(xyzOutput_ && !output->data.empty())
250  {
251  // convert only if not already XYZ cloud
252  bool hasField[4] = {false};
253  for(size_t i=0; i<output->fields.size(); ++i)
254  {
255  if(output->fields[i].name.compare("x") == 0)
256  {
257  hasField[0] = true;
258  }
259  else if(output->fields[i].name.compare("y") == 0)
260  {
261  hasField[1] = true;
262  }
263  else if(output->fields[i].name.compare("z") == 0)
264  {
265  hasField[2] = true;
266  }
267  else
268  {
269  hasField[3] = true; // other
270  break;
271  }
272  }
273  if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
274  {
275  // do nothing, already XYZ
276  }
277  else
278  {
279  pcl::PointCloud<pcl::PointXYZ> cloudxyz;
280  pcl::fromPCLPointCloud2(*output, cloudxyz);
281  pcl::toPCLPointCloud2(cloudxyz, *output);
282  }
283  }
284 
285  for(unsigned int i=1; i<cloudMsgs.size(); ++i)
286  {
287  rtabmap::Transform cloudDisplacement;
288  if(!fixedFrameId_.empty() &&
289  cloudMsgs[0]->header.stamp != cloudMsgs[i]->header.stamp)
290  {
291  // approx sync
292  cloudDisplacement = rtabmap_ros::getTransform(
293  frameId, //sourceTargetFrame
294  fixedFrameId_, //fixedFrame
295  cloudMsgs[i]->header.stamp, //stampSource
296  cloudMsgs[0]->header.stamp, //stampTarget
297  tfListener_,
298  waitForTransformDuration_);
299  }
300 
301  pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
302  if(frameId.compare(cloudMsgs[i]->header.frame_id) != 0)
303  {
304  sensor_msgs::PointCloud2 tmp;
305  pcl_ros::transformPointCloud(frameId, *cloudMsgs[i], tmp, tfListener_);
306  if(!cloudDisplacement.isNull())
307  {
308  sensor_msgs::PointCloud2 tmp2;
309  pcl_ros::transformPointCloud(cloudDisplacement.toEigen4f(), tmp, tmp2);
310  pcl_conversions::toPCL(tmp2, *cloud2);
311  }
312  else
313  {
314  pcl_conversions::toPCL(tmp, *cloud2);
315  }
316 
317  }
318  else
319  {
320  if(!cloudDisplacement.isNull())
321  {
322  sensor_msgs::PointCloud2 tmp;
323  pcl_ros::transformPointCloud(cloudDisplacement.toEigen4f(), *cloudMsgs[i], tmp);
324  pcl_conversions::toPCL(tmp, *cloud2);
325  }
326  else
327  {
328  pcl_conversions::toPCL(*cloudMsgs[i], *cloud2);
329  }
330  }
331 
332  if(!cloud2->is_dense)
333  {
334  // remove nans
335  cloud2 = rtabmap::util3d::removeNaNFromPointCloud(cloud2);
336  }
337 
338  if(xyzOutput_ && !cloud2->data.empty())
339  {
340  // convert only if not already XYZ cloud
341  bool hasField[4] = {false};
342  for(size_t i=0; i<cloud2->fields.size(); ++i)
343  {
344  if(cloud2->fields[i].name.compare("x") == 0)
345  {
346  hasField[0] = true;
347  }
348  else if(cloud2->fields[i].name.compare("y") == 0)
349  {
350  hasField[1] = true;
351  }
352  else if(cloud2->fields[i].name.compare("z") == 0)
353  {
354  hasField[2] = true;
355  }
356  else
357  {
358  hasField[3] = true; // other
359  break;
360  }
361  }
362  if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
363  {
364  // do nothing, already XYZ
365  }
366  else
367  {
368  pcl::PointCloud<pcl::PointXYZ> cloudxyz;
369  pcl::fromPCLPointCloud2(*cloud2, cloudxyz);
370  pcl::toPCLPointCloud2(cloudxyz, *cloud2);
371  }
372  }
373 
374  if(output->data.empty())
375  {
376  output = cloud2;
377  }
378  else if(!cloud2->data.empty())
379  {
380 
381  if(output->fields.size() != cloud2->fields.size())
382  {
383  ROS_WARN("%s: Input topics don't have all the "
384  "same number of fields (cloud1=%d, cloud%d=%d), concatenation "
385  "may fails. You can enable \"xyz_output\" option "
386  "to convert all inputs to XYZ.",
387  getName().c_str(),
388  (int)output->fields.size(),
389  i+1,
390  (int)output->fields.size());
391  }
392 
393  pcl::PCLPointCloud2::Ptr tmp_output(new pcl::PCLPointCloud2);
394 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
395  pcl::concatenate(*output, *cloud2, *tmp_output);
396 #else
397  pcl::concatenatePointCloud(*output, *cloud2, *tmp_output);
398 #endif
399  //Make sure row_step is the sum of both
400  tmp_output->row_step = tmp_output->width * tmp_output->point_step;
401  output = tmp_output;
402  }
403  }
404 
405  sensor_msgs::PointCloud2 rosCloud;
406  pcl_conversions::moveFromPCL(*output, rosCloud);
407  rosCloud.header.stamp = cloudMsgs[0]->header.stamp;
408  rosCloud.header.frame_id = frameId;
409  cloudPub_.publish(rosCloud);
410  }
411  }
412 
413  void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
414  {
415  ros::Duration r(5.0);
416  while(!callbackCalled_)
417  {
418  r.sleep();
419  if(!callbackCalled_)
420  {
421  ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
422  "published (\"$ rostopic hz my_topic\") and the timestamps in their "
423  "header are set. %s%s",
424  getName().c_str(),
425  approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
426  "topics should have all the exact timestamp for the callback to be called.",
427  subscribedTopicsMsg.c_str());
428  }
429  }
430  }
431 
432  boost::thread * warningThread_;
433  bool callbackCalled_;
434 
435  typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSync4Policy;
436  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproxSync4Policy;
437  typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSync3Policy;
438  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproxSync3Policy;
439  typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSync2Policy;
440  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproxSync2Policy;
441  message_filters::Synchronizer<ExactSync4Policy>* exactSync4_;
442  message_filters::Synchronizer<ApproxSync4Policy>* approxSync4_;
443  message_filters::Synchronizer<ExactSync3Policy>* exactSync3_;
444  message_filters::Synchronizer<ApproxSync3Policy>* approxSync3_;
445  message_filters::Synchronizer<ExactSync2Policy>* exactSync2_;
446  message_filters::Synchronizer<ApproxSync2Policy>* approxSync2_;
447  message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_1_;
448  message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_2_;
449  message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_3_;
450  message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_4_;
451 
452  ros::Publisher cloudPub_;
453 
454  std::string frameId_;
455  std::string fixedFrameId_;
456  double waitForTransformDuration_;
457  bool xyzOutput_;
458  tf::TransformListener tfListener_;
459 };
460 
461 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet);
462 }
463 
rtabmap_ros::PointCloudAggregator::exactSync4_
message_filters::Synchronizer< ExactSync4Policy > * exactSync4_
Definition: point_cloud_aggregator.cpp:441
message_filters::Synchronizer
rtabmap_ros::PointCloudAggregator::callbackCalled_
bool callbackCalled_
Definition: point_cloud_aggregator.cpp:433
rtabmap_ros::PointCloudAggregator::cloudSub_2_
message_filters::Subscriber< sensor_msgs::PointCloud2 > cloudSub_2_
Definition: point_cloud_aggregator.cpp:448
uFormat
std::string uFormat(const char *fmt,...)
transform_listener.h
ros::NodeHandle
rtabmap_ros::PointCloudAggregator::xyzOutput_
bool xyzOutput_
Definition: point_cloud_aggregator.cpp:457
subscriber_filter.h
rtabmap_ros::PointCloudAggregator::cloudPub_
ros::Publisher cloudPub_
Definition: point_cloud_aggregator.cpp:452
message_filters::Subscriber::getTopic
std::string getTopic() const
rtabmap_ros::PointCloudAggregator::cloudSub_3_
message_filters::Subscriber< sensor_msgs::PointCloud2 > cloudSub_3_
Definition: point_cloud_aggregator.cpp:449
message_filters::sync_policies::ExactTime
nodelet::Nodelet::getNodeHandle
ros::NodeHandle & getNodeHandle() const
rtabmap_ros::PointCloudAggregator::ExactSync3Policy
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > ExactSync3Policy
Definition: point_cloud_aggregator.cpp:437
image_transport.h
rtabmap::util3d::removeNaNFromPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
rtabmap_ros::PointCloudAggregator::exactSync2_
message_filters::Synchronizer< ExactSync2Policy > * exactSync2_
Definition: point_cloud_aggregator.cpp:445
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
rtabmap_ros::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:208
rtabmap_ros
Definition: CommonDataSubscriber.h:57
ROS_WARN
#define ROS_WARN(...)
rtabmap_ros::PointCloudAggregator::onInit
virtual void onInit()
Definition: point_cloud_aggregator.cpp:97
rtabmap_ros::getTransform
rtabmap::Transform getTransform(const std::string &fromFrameId, const std::string &toFrameId, const ros::Time &stamp, tf::TransformListener &listener, double waitForTransform)
Definition: MsgConversion.cpp:1733
rtabmap_ros::PointCloudAggregator::approxSync3_
message_filters::Synchronizer< ApproxSync3Policy > * approxSync3_
Definition: point_cloud_aggregator.cpp:444
rtabmap_ros::PointCloudAggregator::warningThread_
boost::thread * warningThread_
Definition: point_cloud_aggregator.cpp:432
rtabmap_ros::PointCloudAggregator::waitForTransformDuration_
double waitForTransformDuration_
Definition: point_cloud_aggregator.cpp:456
exact_time.h
nodelet::Nodelet::getName
const std::string & getName() const
class_list_macros.hpp
rtabmap_ros::PointCloudAggregator::combineClouds
void combineClouds(const std::vector< sensor_msgs::PointCloud2ConstPtr > &cloudMsgs)
Definition: point_cloud_aggregator.cpp:228
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
rtabmap_ros::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:195
rtabmap_ros::PointCloudAggregator::~PointCloudAggregator
virtual ~PointCloudAggregator()
Definition: point_cloud_aggregator.cpp:79
rtabmap_ros::PointCloudAggregator::fixedFrameId_
std::string fixedFrameId_
Definition: point_cloud_aggregator.cpp:455
rtabmap_ros::PointCloudAggregator::ApproxSync2Policy
message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > ApproxSync2Policy
Definition: point_cloud_aggregator.cpp:440
util3d_filtering.h
rtabmap_ros::PointCloudAggregator::exactSync3_
message_filters::Synchronizer< ExactSync3Policy > * exactSync3_
Definition: point_cloud_aggregator.cpp:443
pcl_conversions.h
approximate_time.h
pcl_conversions::toPCL
void toPCL(const ros::Time &stamp, std::uint64_t &pcl_stamp)
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
rtabmap_ros::PointCloudAggregator::ExactSync2Policy
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > ExactSync2Policy
Definition: point_cloud_aggregator.cpp:439
rtabmap_ros::PointCloudAggregator::PointCloudAggregator
PointCloudAggregator()
Definition: point_cloud_aggregator.cpp:66
pcl::concatenatePointCloud
bool concatenatePointCloud(const sensor_msgs::PointCloud2 &cloud1, const sensor_msgs::PointCloud2 &cloud2, sensor_msgs::PointCloud2 &cloud_out)
rtabmap::Transform::isNull
bool isNull() const
patrol.frameId
string frameId
Definition: patrol.py:11
false
#define false
message_filters::sync_policies::ApproximateTime
NODELET_INFO
#define NODELET_INFO(...)
rtabmap_ros::PointCloudAggregator::frameId_
std::string frameId_
Definition: point_cloud_aggregator.cpp:454
message_filters::Subscriber::subscribe
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)
rtabmap_ros::PointCloudAggregator::tfListener_
tf::TransformListener tfListener_
Definition: point_cloud_aggregator.cpp:458
pcl_ros::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
rtabmap_ros::PointCloudAggregator
Definition: point_cloud_aggregator.cpp:63
rtabmap_ros::PointCloudAggregator::ApproxSync3Policy
message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > ApproxSync3Policy
Definition: point_cloud_aggregator.cpp:438
rtabmap::Transform::toEigen4f
Eigen::Matrix4f toEigen4f() const
header
const std::string header
rtabmap_ros::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:435
rtabmap_ros::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet)
ros::Duration::sleep
bool sleep() const
pcl_conversions::moveFromPCL
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
message_filters::Subscriber< sensor_msgs::PointCloud2 >
ROS_ASSERT
#define ROS_ASSERT(cond)
rtabmap_ros::PointCloudAggregator::clouds2_callback
void clouds2_callback(const sensor_msgs::PointCloud2ConstPtr &cloudMsg_1, const sensor_msgs::PointCloud2ConstPtr &cloudMsg_2)
Definition: point_cloud_aggregator.cpp:219
rtabmap_ros::PointCloudAggregator::approxSync2_
message_filters::Synchronizer< ApproxSync2Policy > * approxSync2_
Definition: point_cloud_aggregator.cpp:446
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
r
r
rtabmap_ros::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:436
rtabmap_ros::PointCloudAggregator::warningLoop
void warningLoop(const std::string &subscribedTopicsMsg, bool approxSync)
Definition: point_cloud_aggregator.cpp:413
rtabmap_ros::PointCloudAggregator::cloudSub_4_
message_filters::Subscriber< sensor_msgs::PointCloud2 > cloudSub_4_
Definition: point_cloud_aggregator.cpp:450
rtabmap_ros::PointCloudAggregator::cloudSub_1_
message_filters::Subscriber< sensor_msgs::PointCloud2 > cloudSub_1_
Definition: point_cloud_aggregator.cpp:447
rtabmap_ros::PointCloudAggregator::approxSync4_
message_filters::Synchronizer< ApproxSync4Policy > * approxSync4_
Definition: point_cloud_aggregator.cpp:442

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40