From cb57676af87868fb40e40eadaf569bf48ee9b911 Mon Sep 17 00:00:00 2001 From: Prescillia Date: Thu, 14 Nov 2019 15:21:48 -0500 Subject: [PATCH 1/2] changed assembling cloud condition from nb of clouds to sweep time --- src/nodelets/point_cloud_assembler.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/nodelets/point_cloud_assembler.cpp b/src/nodelets/point_cloud_assembler.cpp index 524bd492..9d9fa743 100644 --- a/src/nodelets/point_cloud_assembler.cpp +++ b/src/nodelets/point_cloud_assembler.cpp @@ -67,7 +67,8 @@ public: warningThread_(0), callbackCalled_(false), exactSync_(0), - maxClouds_(1), + maxClouds_(0), + assemblingTime_(0), skipClouds_(0), cloudsSkipped_(0), waitForTransformDuration_(0.1), @@ -100,12 +101,13 @@ private: pnh.param("queue_size", queueSize, queueSize); pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_); pnh.param("max_clouds", maxClouds_, maxClouds_); + pnh.param("assembling_time", assemblingTime_, assemblingTime_); pnh.param("skip_clouds", skipClouds_, skipClouds_); pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); pnh.param("range_min", rangeMin_, rangeMin_); pnh.param("range_max", rangeMax_, rangeMax_); pnh.param("voxel_size", voxelSize_, voxelSize_); - ROS_ASSERT(maxClouds_>0); + ROS_ASSERT(maxClouds_>=0); cloudsSkipped_ = skipClouds_; @@ -166,7 +168,8 @@ private: *cpy = *cloudMsg; clouds_.push_back(cpy); - if((int)clouds_.size() >= maxClouds_) + if( (int)clouds_.size() >= maxClouds_ && maxClouds_ != 0 + || (double)(*cpy).header.stamp.toSec() >= (double)clouds_[0]->header.stamp.toSec() + assemblingTime_ ) { pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2); pcl_conversions::toPCL(*clouds_.back(), *assembled); @@ -270,6 +273,7 @@ private: int maxClouds_; int skipClouds_; int cloudsSkipped_; + double assemblingTime_; double waitForTransformDuration_; double rangeMin_; double rangeMax_; From 3e0c501c32f5214511e9faf3fe534ec8e347b254 Mon Sep 17 00:00:00 2001 From: Prescillia Date: Thu, 21 Nov 2019 10:26:03 -0500 Subject: [PATCH 2/2] corrected assert and condition for assembling time --- src/nodelets/point_cloud_assembler.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/nodelets/point_cloud_assembler.cpp b/src/nodelets/point_cloud_assembler.cpp index 9d9fa743..36560576 100644 --- a/src/nodelets/point_cloud_assembler.cpp +++ b/src/nodelets/point_cloud_assembler.cpp @@ -107,7 +107,7 @@ private: pnh.param("range_min", rangeMin_, rangeMin_); pnh.param("range_max", rangeMax_, rangeMax_); pnh.param("voxel_size", voxelSize_, voxelSize_); - ROS_ASSERT(maxClouds_>=0); + ROS_ASSERT(maxClouds_>=0 && assemblingTime_ >=0); cloudsSkipped_ = skipClouds_; @@ -168,8 +168,9 @@ private: *cpy = *cloudMsg; clouds_.push_back(cpy); - if( (int)clouds_.size() >= maxClouds_ && maxClouds_ != 0 - || (double)(*cpy).header.stamp.toSec() >= (double)clouds_[0]->header.stamp.toSec() + assemblingTime_ ) + if( (int)clouds_.size() >= maxClouds_ && maxClouds_ != 0 + || + (double)(*cpy).header.stamp.toSec() >= (double)clouds_[0]->header.stamp.toSec() + assemblingTime_ && assemblingTime_ != 0.0 ) { pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2); pcl_conversions::toPCL(*clouds_.back(), *assembled); @@ -204,7 +205,7 @@ private: pcl::concatenatePointCloud(*assembled, *rtabmap::util3d::laserScanToPointCloud2(scan, t), *assembledTmp); } else - { + { sensor_msgs::PointCloud2 output; pcl_ros::transformPointCloud(t.toEigen4f(), *clouds_[i], output); pcl::PCLPointCloud2 output2;