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

point_cloud_assembler: added voxel_size parameter

Browse Source
This commit is contained in:
matlabbe
2019-09-15 10:08:08 -04:00
parent e6e7df5beb
commit 9ec95e1e0d
1 changed files with 21 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/point_cloud_assembler.cpp
+21 -2
View File
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_ros/transforms.h>
@@ -72,6 +73,7 @@ public:
waitForTransformDuration_(0.1),
rangeMin_(0),
rangeMax_(0),
voxelSize_(0),
fixedFrameId_("odom")
{}
@@ -102,6 +104,7 @@ private:
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);
cloudsSkipped_ = skipClouds_;
@@ -191,7 +194,10 @@ private:
pcl::PCLPointCloud2 output2;
pcl_conversions::toPCL(*clouds_[i], output2);
rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(output2);
scan = rtabmap::util3d::rangeFiltering(scan, rangeMin_, rangeMax_);
if(rangeMin_ > 0.0 || rangeMax_ > 0.0)
{
scan = rtabmap::util3d::rangeFiltering(scan, rangeMin_, rangeMax_);
}
pcl::concatenatePointCloud(*assembled, *rtabmap::util3d::laserScanToPointCloud2(scan, t), *assembledTmp);
}
else
@@ -207,7 +213,19 @@ private:
}
sensor_msgs::PointCloud2 rosCloud;
pcl_conversions::moveFromPCL(*assembled, rosCloud);
if(voxelSize_>0.0)
{
pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
filter.setInputCloud(assembled);
pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
filter.filter(*output);
pcl_conversions::moveFromPCL(*output, rosCloud);
}
else
{
pcl_conversions::moveFromPCL(*assembled, rosCloud);
}
rosCloud.header = cloudMsg->header;
cloudPub_.publish(rosCloud);
clouds_.clear();
@@ -255,6 +273,7 @@ private:
double waitForTransformDuration_;
double rangeMin_;
double rangeMax_;
double voxelSize_;
std::string fixedFrameId_;
tf::TransformListener tfListener_;