i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Octomap: added frontier output topic

Browse Source
This commit is contained in:
Louis Petit
2019-12-17 17:32:51 -05:00
parent f242e1879b
commit 71294263b8
2 changed files with 24 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/MapsManager.h
+1
View File
@@ -103,6 +103,7 @@ private:
ros::Publisher octoMapPubBin_;
ros::Publisher octoMapPubFull_;
ros::Publisher octoMapCloud_;
ros::Publisher octoMapFrontierCloud_;
ros::Publisher octoMapGroundCloud_;
ros::Publisher octoMapObstacleCloud_;
ros::Publisher octoMapEmptySpace_;
src/MapsManager.cpp
+23 -1
View File
@@ -188,6 +188,8 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
latched_.insert(std::make_pair((void*)&octoMapPubFull_, false));
octoMapCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_occupied_space", 1, latching_);
latched_.insert(std::make_pair((void*)&octoMapCloud_, false));
octoMapFrontierCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_global_frontier_space", 1, latching_);
latched_.insert(std::make_pair((void*)&octoMapFrontierCloud_, false));
octoMapObstacleCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_obstacles", 1, latching_);
latched_.insert(std::make_pair((void*)&octoMapObstacleCloud_, false));
octoMapGroundCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_ground", 1, latching_);
@@ -413,6 +415,7 @@ bool MapsManager::hasSubscribers() const
octoMapPubBin_.getNumSubscribers() != 0 ||
octoMapPubFull_.getNumSubscribers() != 0 ||
octoMapCloud_.getNumSubscribers() != 0 ||
octoMapFrontierCloud_.getNumSubscribers() != 0 ||
octoMapObstacleCloud_.getNumSubscribers() != 0 ||
octoMapGroundCloud_.getNumSubscribers() != 0 ||
octoMapEmptySpace_.getNumSubscribers() != 0 ||
@@ -445,6 +448,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
octoMapPubBin_.getNumSubscribers() != 0 ||
octoMapPubFull_.getNumSubscribers() != 0 ||
octoMapCloud_.getNumSubscribers() != 0 ||
octoMapFrontierCloud_.getNumSubscribers() != 0 ||
octoMapObstacleCloud_.getNumSubscribers() != 0 ||
octoMapGroundCloud_.getNumSubscribers() != 0 ||
octoMapEmptySpace_.getNumSubscribers() != 0 ||
@@ -1178,6 +1182,7 @@ void MapsManager::publishMaps(
(octoMapPubBin_.getNumSubscribers() && !latched_.at(&octoMapPubBin_)) ||
(octoMapPubFull_.getNumSubscribers() && !latched_.at(&octoMapPubFull_)) ||
(octoMapCloud_.getNumSubscribers() && !latched_.at(&octoMapCloud_)) ||
(octoMapFrontierCloud_.getNumSubscribers() && !latched_.at(&octoMapFrontierCloud_)) ||
(octoMapObstacleCloud_.getNumSubscribers() && !latched_.at(&octoMapObstacleCloud_)) ||
(octoMapGroundCloud_.getNumSubscribers() && !latched_.at(&octoMapGroundCloud_)) ||
(octoMapEmptySpace_.getNumSubscribers() && !latched_.at(&octoMapEmptySpace_)) ||
@@ -1202,15 +1207,17 @@ void MapsManager::publishMaps(
latched_.at(&octoMapPubFull_) = true;
}
if(octoMapCloud_.getNumSubscribers() ||
octoMapFrontierCloud_.getNumSubscribers() ||
octoMapObstacleCloud_.getNumSubscribers() ||
octoMapGroundCloud_.getNumSubscribers() ||
octoMapEmptySpace_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::IndicesPtr obstacleIndices(new std::vector<int>);
pcl::IndicesPtr frontierIndices(new std::vector<int>);
pcl::IndicesPtr emptyIndices(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());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get(), groundIndices.get(), true, frontierIndices.get());
if(octoMapCloud_.getNumSubscribers())
{
@@ -1223,6 +1230,16 @@ void MapsManager::publishMaps(
octoMapCloud_.publish(msg);
latched_.at(&octoMapCloud_) = true;
}
if(octoMapFrontierCloud_.getNumSubscribers())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudFrontier;
pcl::copyPointCloud(*cloud, *frontierIndices, cloudFrontier);
pcl::toROSMsg(cloudFrontier, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapFrontierCloud_.publish(msg);
latched_.at(&octoMapFrontierCloud_) = true;
}
if(octoMapObstacleCloud_.getNumSubscribers())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudObstacles;
@@ -1302,6 +1319,7 @@ void MapsManager::publishMaps(
octoMapPubBin_.getNumSubscribers() == 0 &&
octoMapPubFull_.getNumSubscribers() == 0 &&
octoMapCloud_.getNumSubscribers() == 0 &&
octoMapFrontierCloud_.getNumSubscribers() == 0 &&
octoMapObstacleCloud_.getNumSubscribers() == 0 &&
octoMapGroundCloud_.getNumSubscribers() == 0 &&
octoMapEmptySpace_.getNumSubscribers() == 0 &&
@@ -1322,6 +1340,10 @@ void MapsManager::publishMaps(
{
latched_.at(&octoMapCloud_) = false;
}
if(octoMapFrontierCloud_.getNumSubscribers() == 0)
{
latched_.at(&octoMapFrontierCloud_) = false;
}
if(octoMapObstacleCloud_.getNumSubscribers() == 0)
{
latched_.at(&octoMapObstacleCloud_) = false;