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

move flood fill to octomap::uptade()

Browse Source
This commit is contained in:
chameau5050
2021-05-27 13:42:16 -04:00
parent 7239a4914e
commit 2473791891
1 changed files with 10 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MapsManager.cpp
+10 -5
View File
@@ -131,11 +131,12 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
#ifdef WITH_OCTOMAP_MSGS #ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP #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); pnh.param("octomap_frontier_flood_fill", octomap_frontier_flood_fill_, false);
pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_);
if(octomapTreeDepth_ > 16)
if(octomapTreeDepth_ > 16)
{ {
ROS_WARN("octomap_tree_depth maximum is 16"); ROS_WARN("octomap_tree_depth maximum is 16");
octomapTreeDepth_ = 16; octomapTreeDepth_ = 16;
@@ -146,6 +147,10 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
octomapTreeDepth_ = 16; octomapTreeDepth_ = 16;
} }
ROS_INFO("%s(maps): octomap_tree_depth = %d", name.c_str(), octomapTreeDepth_); 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
#endif #endif
@@ -1241,7 +1246,7 @@ void MapsManager::publishMaps(
pcl::IndicesPtr frontierIndices(new std::vector<int>); pcl::IndicesPtr frontierIndices(new std::vector<int>);
pcl::IndicesPtr emptyIndices(new std::vector<int>); pcl::IndicesPtr emptyIndices(new std::vector<int>);
pcl::IndicesPtr groundIndices(new std::vector<int>); pcl::IndicesPtr groundIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get(), groundIndices.get(), true, frontierIndices.get(),0,octomap_frontier_flood_fill_); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get(), groundIndices.get(), true, frontierIndices.get(),0);
if(octoMapCloud_.getNumSubscribers()) if(octoMapCloud_.getNumSubscribers())
{ {