From 24737918912c5952e4a1b99dc31bb548413bd53c Mon Sep 17 00:00:00 2001 From: chameau5050 Date: Thu, 27 May 2021 13:42:16 -0400 Subject: [PATCH] move flood fill to octomap::uptade() --- src/MapsManager.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index 78e49717..c9bc1d38 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -131,11 +131,12 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s #ifdef WITH_OCTOMAP_MSGS #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) + pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_); + + + if(octomapTreeDepth_ > 16) { ROS_WARN("octomap_tree_depth maximum is 16"); octomapTreeDepth_ = 16; @@ -146,6 +147,10 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s octomapTreeDepth_ = 16; } ROS_INFO("%s(maps): octomap_tree_depth = %d", name.c_str(), octomapTreeDepth_); + + + octomap_ = new OctoMap(occupancyGrid_->getCellSize(), 0.5, occupancyGrid_->isFullUpdate(), occupancyGrid_->getUpdateError(), octomap_frontier_flood_fill_, octomapTreeDepth_); + #endif #endif @@ -1241,7 +1246,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(),0,octomap_frontier_flood_fill_); + pcl::PointCloud::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get(), groundIndices.get(), true, frontierIndices.get(),0); if(octoMapCloud_.getNumSubscribers()) {