i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #369 from PrescilliaA/master

Browse Source 
added assemblingTime constraint for point cloud assembling
This commit is contained in:
matlabbe
2019-11-21 10:42:23 -05:00
committed by GitHub
parent 26b8fb5027 3e0c501c32
commit 1396c94d22
1 changed files with 9 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/point_cloud_assembler.cpp
+8 -3
View File
@@ -67,7 +67,8 @@ public:
warningThread_(0), warningThread_(0),
callbackCalled_(false), callbackCalled_(false),
exactSync_(0), exactSync_(0),
maxClouds_(1), maxClouds_(0),
assemblingTime_(0),
skipClouds_(0), skipClouds_(0),
cloudsSkipped_(0), cloudsSkipped_(0),
waitForTransformDuration_(0.1), waitForTransformDuration_(0.1),
@@ -100,12 +101,13 @@ private:
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_); pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
pnh.param("max_clouds", maxClouds_, maxClouds_); pnh.param("max_clouds", maxClouds_, maxClouds_);
pnh.param("assembling_time", assemblingTime_, assemblingTime_);
pnh.param("skip_clouds", skipClouds_, skipClouds_); pnh.param("skip_clouds", skipClouds_, skipClouds_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("range_min", rangeMin_, rangeMin_); pnh.param("range_min", rangeMin_, rangeMin_);
pnh.param("range_max", rangeMax_, rangeMax_); pnh.param("range_max", rangeMax_, rangeMax_);
pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("voxel_size", voxelSize_, voxelSize_);
ROS_ASSERT(maxClouds_>0); ROS_ASSERT(maxClouds_>=0 && assemblingTime_ >=0);
cloudsSkipped_ = skipClouds_; cloudsSkipped_ = skipClouds_;
@@ -166,7 +168,9 @@ private:
*cpy = *cloudMsg; *cpy = *cloudMsg;
clouds_.push_back(cpy); 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_ && assemblingTime_ != 0.0 )
{ {
pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2); pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*clouds_.back(), *assembled); pcl_conversions::toPCL(*clouds_.back(), *assembled);
@@ -270,6 +274,7 @@ private:
int maxClouds_; int maxClouds_;
int skipClouds_; int skipClouds_;
int cloudsSkipped_; int cloudsSkipped_;
double assemblingTime_;
double waitForTransformDuration_; double waitForTransformDuration_;
double rangeMin_; double rangeMin_;
double rangeMax_; double rangeMax_;