i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37: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
+9 -4
View File
@@ -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 && assemblingTime_ >=0);
cloudsSkipped_ = skipClouds_;
@@ -166,7 +168,9 @@ 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_ && assemblingTime_ != 0.0 )
{
pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*clouds_.back(), *assembled);
@@ -201,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;
@@ -270,6 +274,7 @@ private:
int maxClouds_;
int skipClouds_;
int cloudsSkipped_;
double assemblingTime_;
double waitForTransformDuration_;
double rangeMin_;
double rangeMax_;