From 84ac70077cf870c4938f64fa8a18191b872cc321 Mon Sep 17 00:00:00 2001 From: MarcLeclercGit Date: Fri, 7 May 2021 10:52:43 -0400 Subject: [PATCH] add a flood fill filter --- include/rtabmap_ros/MapsManager.h | 1 + src/MapsManager.cpp | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/include/rtabmap_ros/MapsManager.h b/include/rtabmap_ros/MapsManager.h index a1092931..32bfa4dc 100644 --- a/include/rtabmap_ros/MapsManager.h +++ b/include/rtabmap_ros/MapsManager.h @@ -128,6 +128,7 @@ private: rtabmap::OctoMap * octomap_; int octomapTreeDepth_; + bool octomap_frontier_flood_fill_; bool octomapUpdated_; rtabmap::ParametersMap parameters_; diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index 7a830f38..78e49717 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -133,6 +133,8 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s #ifdef RTABMAP_OCTOMAP octomap_ = new OctoMap(occupancyGrid_->getCellSize(), 0.5, occupancyGrid_->isFullUpdate(), occupancyGrid_->getUpdateError()); pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_); + pnh.param("octomap_frontier_flood_fill", octomap_frontier_flood_fill_, false); + if(octomapTreeDepth_ > 16) { ROS_WARN("octomap_tree_depth maximum is 16"); @@ -1239,7 +1241,7 @@ void MapsManager::publishMaps( pcl::IndicesPtr frontierIndices(new std::vector); pcl::IndicesPtr emptyIndices(new std::vector); pcl::IndicesPtr groundIndices(new std::vector); - pcl::PointCloud::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get(), groundIndices.get(), true, frontierIndices.get()); + pcl::PointCloud::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get(), groundIndices.get(), true, frontierIndices.get(),0,octomap_frontier_flood_fill_); if(octoMapCloud_.getNumSubscribers()) {