point_cloud_assembler.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 #include <pcl/filters/voxel_grid.h>
37 #include <pcl/filters/radius_outlier_removal.h>
38 
39 #include <pcl_ros/transforms.h>
40 
41 #include <tf/transform_listener.h>
42 
43 #include <nav_msgs/Odometry.h>
44 #include <sensor_msgs/PointCloud2.h>
45 #include <sensor_msgs/point_cloud2_iterator.h>
46 
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_ros/OdomInfo.h>
52 #include <rtabmap/core/util3d.h>
53 #include <rtabmap/core/util3d_filtering.h>
54 #include <rtabmap/core/Version.h>
55 
56 namespace rtabmap_ros
57 {
58 
67 class PointCloudAssembler : public nodelet::Nodelet
68 {
69 public:
70  PointCloudAssembler() :
71  warningThread_(0),
72  callbackCalled_(false),
73  exactSync_(0),
74  exactInfoSync_(0),
75  maxClouds_(0),
76  assemblingTime_(0),
77  skipClouds_(0),
78  cloudsSkipped_(0),
79  circularBuffer_(false),
80  linearUpdate_(0),
81  angularUpdate_(0),
82  waitForTransformDuration_(0.1),
83  rangeMin_(0),
84  rangeMax_(0),
85  voxelSize_(0),
86  noiseRadius_(0),
87  noiseMinNeighbors_(5),
88  removeZ_(false),
89  fixedFrameId_("odom"),
90  frameId_("")
91  {}
92 
93  virtual ~PointCloudAssembler()
94  {
95  delete exactSync_;
96  delete exactInfoSync_;
97 
98  if(warningThread_)
99  {
100  callbackCalled_=true;
101  warningThread_->join();
102  delete warningThread_;
103  }
104  }
105 
106 private:
107  virtual void onInit()
108  {
109  ros::NodeHandle & nh = getNodeHandle();
110  ros::NodeHandle & pnh = getPrivateNodeHandle();
111 
112  int queueSize = 5;
113  bool subscribeOdomInfo = false;
114 
115  pnh.param("queue_size", queueSize, queueSize);
116  pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
117  pnh.param("frame_id", frameId_, frameId_);
118  pnh.param("max_clouds", maxClouds_, maxClouds_);
119  pnh.param("assembling_time", assemblingTime_, assemblingTime_);
120  pnh.param("skip_clouds", skipClouds_, skipClouds_);
121  pnh.param("circular_buffer", circularBuffer_, circularBuffer_);
122  pnh.param("linear_update", linearUpdate_, linearUpdate_);
123  pnh.param("angular_update", angularUpdate_, angularUpdate_);
124  pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
125  pnh.param("range_min", rangeMin_, rangeMin_);
126  pnh.param("range_max", rangeMax_, rangeMax_);
127  pnh.param("voxel_size", voxelSize_, voxelSize_);
128  pnh.param("noise_radius", noiseRadius_, noiseRadius_);
129  pnh.param("noise_min_neighbors", noiseMinNeighbors_, noiseMinNeighbors_);
130  pnh.param("remove_z", removeZ_, removeZ_);
131  pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
132  ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0);
133 
134  ROS_INFO("%s: queue_size=%d", getName().c_str(), queueSize);
135  ROS_INFO("%s: fixed_frame_id=%s", getName().c_str(), fixedFrameId_.c_str());
136  ROS_INFO("%s: frame_id=%s", getName().c_str(), frameId_.c_str());
137  ROS_INFO("%s: max_clouds=%d", getName().c_str(), maxClouds_);
138  ROS_INFO("%s: assembling_time=%fs", getName().c_str(), assemblingTime_);
139  ROS_INFO("%s: skip_clouds=%d", getName().c_str(), skipClouds_);
140  ROS_INFO("%s: circular_buffer=%s", getName().c_str(), circularBuffer_?"true":"false");
141  ROS_INFO("%s: linear_update=%f m", getName().c_str(), linearUpdate_);
142  ROS_INFO("%s: angular_update=%f rad", getName().c_str(), angularUpdate_);
143  ROS_INFO("%s: wait_for_transform_duration=%f", getName().c_str(), waitForTransformDuration_);
144  ROS_INFO("%s: range_min=%f", getName().c_str(), rangeMin_);
145  ROS_INFO("%s: range_max=%f", getName().c_str(), rangeMax_);
146  ROS_INFO("%s: voxel_size=%fm", getName().c_str(), voxelSize_);
147  ROS_INFO("%s: noise_radius=%fm", getName().c_str(), noiseRadius_);
148  ROS_INFO("%s: noise_min_neighbors=%d", getName().c_str(), noiseMinNeighbors_);
149  ROS_INFO("%s: remove_z=%s", getName().c_str(), removeZ_?"true":"false");
150 
151  if(maxClouds_==0 && assemblingTime_ ==0.0)
152  {
153  ROS_ERROR("point_cloud_assembler: max_cloud or assembling_time parameters should be set!");
154  exit(-1);
155  }
156 
157  cloudsSkipped_ = skipClouds_;
158 
159  std::string subscribedTopicsMsg;
160  if(!fixedFrameId_.empty())
161  {
162  cloudSub_ = nh.subscribe("cloud", queueSize, &PointCloudAssembler::callbackCloud, this);
163  subscribedTopicsMsg = uFormat("\n%s subscribed to %s",
164  getName().c_str(),
165  cloudSub_.getTopic().c_str());
166  }
167  else if(subscribeOdomInfo)
168  {
169  syncCloudSub_.subscribe(nh, "cloud", 1);
170  syncOdomSub_.subscribe(nh, "odom", 1);
171  syncOdomInfoSub_.subscribe(nh, "odom_info", 1);
172  exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_);
173  exactInfoSync_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAssembler::callbackCloudOdomInfo, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
174  subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
175  getName().c_str(),
176  syncCloudSub_.getTopic().c_str(),
177  syncOdomSub_.getTopic().c_str(),
178  syncOdomInfoSub_.getTopic().c_str());
179 
180  warningThread_ = new boost::thread(boost::bind(&PointCloudAssembler::warningLoop, this, subscribedTopicsMsg));
181  }
182  else
183  {
184  syncCloudSub_.subscribe(nh, "cloud", 1);
185  syncOdomSub_.subscribe(nh, "odom", 1);
186  exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_);
187  exactSync_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAssembler::callbackCloudOdom, this, boost::placeholders::_1, boost::placeholders::_2));
188  subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
189  getName().c_str(),
190  syncCloudSub_.getTopic().c_str(),
191  syncOdomSub_.getTopic().c_str());
192 
193  warningThread_ = new boost::thread(boost::bind(&PointCloudAssembler::warningLoop, this, subscribedTopicsMsg));
194  }
195 
196  cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("assembled_cloud", 1);
197 
198  NODELET_INFO("%s", subscribedTopicsMsg.c_str());
199  }
200 
201  void callbackCloudOdom(
202  const sensor_msgs::PointCloud2ConstPtr & cloudMsg,
203  const nav_msgs::OdometryConstPtr & odomMsg)
204  {
205  callbackCalled_ = true;
206  rtabmap::Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
207  if(!odom.isNull())
208  {
209  fixedFrameId_ = odomMsg->header.frame_id;
210  callbackCloud(cloudMsg);
211  }
212  else
213  {
214  NODELET_WARN("Reseting point cloud assembler as null odometry has been received.");
215  clouds_.clear();
216  }
217  }
218 
219  void callbackCloudOdomInfo(
220  const sensor_msgs::PointCloud2ConstPtr & cloudMsg,
221  const nav_msgs::OdometryConstPtr & odomMsg,
222  const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg)
223  {
224  callbackCalled_ = true;
225  rtabmap::Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
226  if(!odom.isNull())
227  {
228  if(odomInfoMsg->keyFrameAdded)
229  {
230  fixedFrameId_ = odomMsg->header.frame_id;
231  callbackCloud(cloudMsg);
232  }
233  else
234  {
235  NODELET_INFO("Skipping non keyframe...");
236  }
237  }
238  else
239  {
240  NODELET_WARN("Reseting point cloud assembler as null odometry has been received.");
241  clouds_.clear();
242  }
243  }
244 
245  sensor_msgs::PointCloud2 removeField(const sensor_msgs::PointCloud2 & input, const std::string & field)
246  {
247  sensor_msgs::PointCloud2 output;
248  int offset = 0;
249  std::vector<int> inputFieldIndex;
250  for(size_t i=0; i<input.fields.size(); ++i)
251  {
252  if(input.fields[i].name.compare(field) == 0)
253  {
254  continue;
255  }
256  else
257  {
258  sensor_msgs::PointField outputField = input.fields[i];
259  outputField.offset = offset;
260  offset += outputField.count * sizeOfPointField(outputField.datatype);
261  output.fields.push_back(outputField);
262  inputFieldIndex.push_back(i);
263  }
264  }
265  output.header = input.header;
266  output.height = input.height;
267  output.width = input.width;
268  output.is_bigendian = input.is_bigendian;
269  output.is_dense = input.is_dense;
270  output.point_step = offset;
271  output.row_step = output.width * output.point_step;
272  output.data.resize(output.height*output.row_step);
273  int total = output.height*output.width;
274  for(int i=0; i<total; ++i)
275  {
276  // for each point, copy fields
277  int oi = i*output.point_step;
278  int pi = i*input.point_step;
279  for(size_t j=0;j<output.fields.size(); ++j)
280  {
281  memcpy(&output.data[oi + output.fields[j].offset],
282  &input.data[pi + input.fields[inputFieldIndex[j]].offset],
283  output.fields[j].count * sizeOfPointField(output.fields[j].datatype));
284  }
285  }
286  return output;
287  }
288 
289  void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
290  {
291  if(cloudPub_.getNumSubscribers())
292  {
293  UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
294  uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
295 
296  if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_)
297  {
298  cloudsSkipped_ = 0;
299 
300  rtabmap::Transform pose = rtabmap_ros::getTransform(
301  fixedFrameId_, //fromFrame
302  cloudMsg->header.frame_id, //toFrame
303  cloudMsg->header.stamp,
304  tfListener_,
305  waitForTransformDuration_);
306 
307  if(pose.isNull())
308  {
309  ROS_ERROR("Cloud not transform all clouds! Resetting...");
310  clouds_.clear();
311  return;
312  }
313 
314  bool isMoving = true;
315  if(!previousPose_.isNull() && (linearUpdate_>0 || angularUpdate_>0))
316  {
317  rtabmap::Transform delta = previousPose_.inverse()*pose;
318  float roll, pitch, yaw;
319  delta.getEulerAngles(roll, pitch, yaw);
320  isMoving = fabs(delta.x()) > linearUpdate_ ||
321  fabs(delta.y()) > linearUpdate_ ||
322  fabs(delta.z()) > linearUpdate_ ||
323  (angularUpdate_>0.0f && (
324  fabs(roll) > angularUpdate_ ||
325  fabs(pitch) > angularUpdate_ ||
326  fabs(yaw) > angularUpdate_));
327  }
328 
329  pcl::PCLPointCloud2::Ptr newCloud(new pcl::PCLPointCloud2);
330  if(rangeMin_ > 0.0 || rangeMax_ > 0.0 || voxelSize_ > 0.0f)
331  {
332  pcl_conversions::toPCL(*cloudMsg, *newCloud);
333  rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*newCloud);
334  scan = rtabmap::util3d::commonFiltering(scan, 1, rangeMin_, rangeMax_, voxelSize_);
335 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
336  std::uint64_t stamp = newCloud->header.stamp;
337 #else
338  pcl::uint64_t stamp = newCloud->header.stamp;
339 #endif
340  newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, pose);
341  newCloud->header.stamp = stamp;
342  }
343  else
344  {
345  sensor_msgs::PointCloud2 output;
346  pcl_ros::transformPointCloud(pose.toEigen4f(), *cloudMsg, output);
347  pcl_conversions::toPCL(output, *newCloud);
348  }
349 
350  if(!newCloud->is_dense)
351  {
352  // remove nans
353  newCloud = rtabmap::util3d::removeNaNFromPointCloud(newCloud);
354  }
355 
356  clouds_.push_back(newCloud);
357 
358 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
359  bool reachedMaxSize =
360  ((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
361  ||
362  ((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<std::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
363 #else
364  bool reachedMaxSize =
365  ((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
366  ||
367  ((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<pcl::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
368 #endif
369 
370  if( circularBuffer_ || reachedMaxSize )
371  {
372  pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
373  for(std::list<pcl::PCLPointCloud2::Ptr>::iterator iter=clouds_.begin(); iter!=clouds_.end(); ++iter)
374  {
375  if(assembled->data.empty())
376  {
377  *assembled = *(*iter);
378  }
379  else
380  {
381  pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
382 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
383  pcl::concatenate(*assembled, *(*iter), *assembledTmp);
384 #else
385  pcl::concatenatePointCloud(*assembled, *(*iter), *assembledTmp);
386 #endif
387  //Make sure row_step is the sum of both
388  assembledTmp->row_step = assembled->row_step + (*iter)->row_step;
389  assembled = assembledTmp;
390  }
391  }
392 
393  sensor_msgs::PointCloud2 rosCloud;
394  if(voxelSize_>0.0)
395  {
396  // estimate if there would be an overflow
397  int x_idx=-1, y_idx=-1, z_idx=-1;
398  for (std::size_t d = 0; d < assembled->fields.size (); ++d)
399  {
400  if (assembled->fields[d].name.compare("x")==0)
401  x_idx = d;
402  if (assembled->fields[d].name.compare("y")==0)
403  y_idx = d;
404  if (assembled->fields[d].name.compare("z")==0)
405  z_idx = d;
406  }
407  bool overflow = false;
408  if(x_idx>=0 && y_idx>=0 && z_idx>=0) {
409  Eigen::Vector4f min_p, max_p;
410  pcl::getMinMax3D(assembled, x_idx, y_idx, z_idx, min_p, max_p);
411  float inverseVoxelSize = 1.0f/voxelSize_;
412  std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverseVoxelSize)+1;
413  std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverseVoxelSize)+1;
414  std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverseVoxelSize)+1;
415 
416  if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
417  {
418  overflow = true;
419  }
420  }
421  if(overflow)
422  {
423  rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*assembled);
424  scan = rtabmap::util3d::commonFiltering(scan, 1, 0, 0, voxelSize_);
425 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
426  std::uint64_t stamp = assembled->header.stamp;
427 #else
428  pcl::uint64_t stamp = assembled->header.stamp;
429 #endif
430  assembled = rtabmap::util3d::laserScanToPointCloud2(scan);
431  assembled->header.stamp = stamp;
432  }
433  else
434  {
435  pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
436  filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
437  filter.setInputCloud(assembled);
438  pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
439  filter.filter(*output);
440  assembled = output;
441  }
442  }
443  if(noiseRadius_>0.0 && noiseMinNeighbors_>0)
444  {
445  pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> filter;
446  filter.setRadiusSearch(noiseRadius_);
447  filter.setMinNeighborsInRadius(noiseMinNeighbors_);
448  filter.setInputCloud(assembled);
449  pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
450  filter.filter(*output);
451  assembled = output;
452  }
453 
454  pcl_conversions::moveFromPCL(*assembled, rosCloud);
455  rtabmap::Transform t = pose;
456  if(!frameId_.empty())
457  {
458  // transform in target frame_id instead of sensor frame
459  t = rtabmap_ros::getTransform(
460  fixedFrameId_, //fromFrame
461  frameId_, //toFrame
462  cloudMsg->header.stamp,
463  tfListener_,
464  waitForTransformDuration_);
465  if(t.isNull())
466  {
467  ROS_ERROR("Cloud not transform back assembled clouds in target frame \"%s\"! Resetting...", frameId_.c_str());
468  clouds_.clear();
469  return;
470  }
471  }
472  pcl_ros::transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud);
473 
474  if(removeZ_)
475  {
476  rosCloud = removeField(rosCloud, "z");
477  }
478 
479  rosCloud.header = cloudMsg->header;
480  if(!frameId_.empty())
481  {
482  rosCloud.header.frame_id = frameId_;
483  }
484  cloudPub_.publish(rosCloud);
485  if(circularBuffer_)
486  {
487  if(!isMoving)
488  {
489  clouds_.pop_back();
490  }
491  else
492  {
493  previousPose_ = pose;
494  if(reachedMaxSize)
495  {
496  clouds_.pop_front();
497  }
498  }
499  }
500  else
501  {
502  clouds_.clear();
503  previousPose_.setNull();
504  }
505  }
506  else if(!isMoving)
507  {
508  clouds_.pop_back();
509  }
510  else
511  {
512  previousPose_ = pose;
513  }
514  }
515  else
516  {
517  ++cloudsSkipped_;
518  }
519  }
520  }
521 
522  void warningLoop(const std::string & subscribedTopicsMsg)
523  {
524  ros::Duration r(5.0);
525  while(!callbackCalled_)
526  {
527  r.sleep();
528  if(!callbackCalled_)
529  {
530  ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
531  "published (\"$ rostopic hz my_topic\") and the timestamps in their "
532  "header are set. %s",
533  getName().c_str(),
534  subscribedTopicsMsg.c_str());
535  }
536  }
537  }
538 
539 private:
540  boost::thread * warningThread_;
541  bool callbackCalled_;
542 
543  ros::Subscriber cloudSub_;
544  ros::Publisher cloudPub_;
545 
546  typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> syncPolicy;
547  typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, rtabmap_ros::OdomInfo> syncInfoPolicy;
548  message_filters::Synchronizer<syncPolicy>* exactSync_;
549  message_filters::Synchronizer<syncInfoPolicy>* exactInfoSync_;
550  message_filters::Subscriber<sensor_msgs::PointCloud2> syncCloudSub_;
551  message_filters::Subscriber<nav_msgs::Odometry> syncOdomSub_;
552  message_filters::Subscriber<rtabmap_ros::OdomInfo> syncOdomInfoSub_;
553 
554  int maxClouds_;
555  int skipClouds_;
556  int cloudsSkipped_;
557  bool circularBuffer_;
558  double linearUpdate_;
559  double angularUpdate_;
560  double assemblingTime_;
561  double waitForTransformDuration_;
562  double rangeMin_;
563  double rangeMax_;
564  double voxelSize_;
565  double noiseRadius_;
566  int noiseMinNeighbors_;
567  bool removeZ_;
568  std::string fixedFrameId_;
569  std::string frameId_;
570  tf::TransformListener tfListener_;
571  rtabmap::Transform previousPose_;
572 
573  std::list<pcl::PCLPointCloud2::Ptr> clouds_;
574 };
575 
576 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAssembler, nodelet::Nodelet);
577 }
578 
rtabmap_ros::PointCloudAssembler::exactInfoSync_
message_filters::Synchronizer< syncInfoPolicy > * exactInfoSync_
Definition: point_cloud_assembler.cpp:549
setup.d
d
Definition: setup.py:5
message_filters::Synchronizer
roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap_ros::PointCloudAssembler::tfListener_
tf::TransformListener tfListener_
Definition: point_cloud_assembler.cpp:570
rtabmap_ros::PointCloudAssembler::PointCloudAssembler
PointCloudAssembler()
Definition: point_cloud_assembler.cpp:70
rtabmap_ros::PointCloudAssembler
Definition: point_cloud_assembler.cpp:67
rtabmap_ros::PointCloudAssembler::skipClouds_
int skipClouds_
Definition: point_cloud_assembler.cpp:555
rtabmap::Transform::getEulerAngles
void getEulerAngles(float &roll, float &pitch, float &yaw) const
uFormat
std::string uFormat(const char *fmt,...)
transform_listener.h
ros::Subscriber::getTopic
std::string getTopic() const
rtabmap_ros::PointCloudAssembler::removeField
sensor_msgs::PointCloud2 removeField(const sensor_msgs::PointCloud2 &input, const std::string &field)
Definition: point_cloud_assembler.cpp:245
ros::NodeHandle
NODELET_WARN
#define NODELET_WARN(...)
int64_t
detail::int64 int64_t
f
f
message_filters::Subscriber::getTopic
std::string getTopic() const
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
rtabmap_ros::PointCloudAssembler::syncOdomSub_
message_filters::Subscriber< nav_msgs::Odometry > syncOdomSub_
Definition: point_cloud_assembler.cpp:551
rtabmap_ros::PointCloudAssembler::removeZ_
bool removeZ_
Definition: point_cloud_assembler.cpp:567
rtabmap_ros::PointCloudAssembler::callbackCloud
void callbackCloud(const sensor_msgs::PointCloud2ConstPtr &cloudMsg)
Definition: point_cloud_assembler.cpp:289
message_filters::sync_policies::ExactTime
nodelet::Nodelet::getNodeHandle
ros::NodeHandle & getNodeHandle() const
rtabmap::util3d::removeNaNFromPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
rtabmap_ros::PointCloudAssembler::onInit
virtual void onInit()
Definition: point_cloud_assembler.cpp:107
rtabmap_ros::PointCloudAssembler::waitForTransformDuration_
double waitForTransformDuration_
Definition: point_cloud_assembler.cpp:561
rtabmap_ros::transformFromPoseMsg
rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose &msg, bool ignoreRotationIfNotSet=false)
Definition: MsgConversion.cpp:118
rtabmap_ros::PointCloudAssembler::cloudPub_
ros::Publisher cloudPub_
Definition: point_cloud_assembler.cpp:544
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap_ros::PointCloudAssembler::warningThread_
boost::thread * warningThread_
Definition: point_cloud_assembler.cpp:540
rtabmap_ros::PointCloudAssembler::circularBuffer_
bool circularBuffer_
Definition: point_cloud_assembler.cpp:557
rtabmap_ros
Definition: CommonDataSubscriber.h:57
ROS_WARN
#define ROS_WARN(...)
rtabmap_ros::PointCloudAssembler::angularUpdate_
double angularUpdate_
Definition: point_cloud_assembler.cpp:559
t
geometry_msgs::TransformStamped t
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::PointCloudAssembler::assemblingTime_
double assemblingTime_
Definition: point_cloud_assembler.cpp:560
exact_time.h
rtabmap_ros::PointCloudAssembler::clouds_
std::list< pcl::PCLPointCloud2::Ptr > clouds_
Definition: point_cloud_assembler.cpp:573
nodelet::Nodelet::getName
const std::string & getName() const
rtabmap_ros::PointCloudAssembler::callbackCalled_
bool callbackCalled_
Definition: point_cloud_assembler.cpp:541
rtabmap_ros::PointCloudAssembler::callbackCloudOdom
void callbackCloudOdom(const sensor_msgs::PointCloud2ConstPtr &cloudMsg, const nav_msgs::OdometryConstPtr &odomMsg)
Definition: point_cloud_assembler.cpp:201
class_list_macros.hpp
rtabmap::util3d::commonFiltering
LaserScan RTABMAP_EXP commonFiltering(const LaserScan &scan, int downsamplingStep, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalK=0, float normalRadius=0.0f, float groundNormalsUp=0.0f)
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::PointCloudAssembler::warningLoop
void warningLoop(const std::string &subscribedTopicsMsg)
Definition: point_cloud_assembler.cpp:522
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
util3d_filtering.h
rtabmap_ros::PointCloudAssembler::cloudsSkipped_
int cloudsSkipped_
Definition: point_cloud_assembler.cpp:556
ROS_INFO
#define ROS_INFO(...)
uint64_t
detail::uint64 uint64_t
pcl_conversions.h
pi
GLM_FUNC_DECL genType pi()
rtabmap_ros::PointCloudAssembler::voxelSize_
double voxelSize_
Definition: point_cloud_assembler.cpp:564
rtabmap_ros::PointCloudAssembler::noiseRadius_
double noiseRadius_
Definition: point_cloud_assembler.cpp:565
rtabmap_ros::PointCloudAssembler::syncCloudSub_
message_filters::Subscriber< sensor_msgs::PointCloud2 > syncCloudSub_
Definition: point_cloud_assembler.cpp:550
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
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::PointCloudAssembler::syncInfoPolicy
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, nav_msgs::Odometry, rtabmap_ros::OdomInfo > syncInfoPolicy
Definition: point_cloud_assembler.cpp:547
gt_tf_broadcaster.stamp
float stamp
Definition: gt_tf_broadcaster.py:28
rtabmap_ros::PointCloudAssembler::~PointCloudAssembler
virtual ~PointCloudAssembler()
Definition: point_cloud_assembler.cpp:93
rtabmap_ros::PointCloudAssembler::noiseMinNeighbors_
int noiseMinNeighbors_
Definition: point_cloud_assembler.cpp:566
rtabmap_ros::PointCloudAssembler::rangeMax_
double rangeMax_
Definition: point_cloud_assembler.cpp:563
rtabmap_ros::PointCloudAssembler::syncPolicy
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, nav_msgs::Odometry > syncPolicy
Definition: point_cloud_assembler.cpp:546
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
rtabmap_ros::PointCloudAssembler::linearUpdate_
double linearUpdate_
Definition: point_cloud_assembler.cpp:558
false
#define false
rtabmap_ros::PointCloudAssembler::syncOdomInfoSub_
message_filters::Subscriber< rtabmap_ros::OdomInfo > syncOdomInfoSub_
Definition: point_cloud_assembler.cpp:552
rtabmap_ros::PointCloudAssembler::rangeMin_
double rangeMin_
Definition: point_cloud_assembler.cpp:562
NODELET_INFO
#define NODELET_INFO(...)
max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
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)
pcl_ros::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
point_cloud2_iterator.h
rtabmap_ros::PointCloudAssembler::cloudSub_
ros::Subscriber cloudSub_
Definition: point_cloud_assembler.cpp:543
rtabmap_ros::PointCloudAssembler::callbackCloudOdomInfo
void callbackCloudOdomInfo(const sensor_msgs::PointCloud2ConstPtr &cloudMsg, const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_ros::OdomInfoConstPtr &odomInfoMsg)
Definition: point_cloud_assembler.cpp:219
rtabmap::Transform::toEigen4f
Eigen::Matrix4f toEigen4f() const
yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap_ros::PointCloudAssembler::previousPose_
rtabmap::Transform previousPose_
Definition: point_cloud_assembler.cpp:571
republish_scan.offset
offset
Definition: republish_scan.py:14
rtabmap_ros::PointCloudAssembler::exactSync_
message_filters::Synchronizer< syncPolicy > * exactSync_
Definition: point_cloud_assembler.cpp:548
rtabmap_ros::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet)
rtabmap_ros::PointCloudAssembler::frameId_
std::string frameId_
Definition: point_cloud_assembler.cpp:569
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::PointCloudAssembler::maxClouds_
int maxClouds_
Definition: point_cloud_assembler.cpp:554
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
r
r
ROS_ERROR
#define ROS_ERROR(...)
rtabmap_ros::PointCloudAssembler::fixedFrameId_
std::string fixedFrameId_
Definition: point_cloud_assembler.cpp:568
rtabmap::Transform::inverse
Transform inverse() const

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