i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-11 20:19:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

MapsManager: added octomap_obstacles and octomap_ground output topics

Browse Source
This commit is contained in:
matlabbe
2018-03-28 19:02:31 -04:00
parent 07d3b027da
commit 6ec6387fc6
2 changed files with 36 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/MapsManager.h
+2
View File
@@ -97,6 +97,8 @@ private:
ros::Publisher octoMapPubBin_; ros::Publisher octoMapPubBin_;
ros::Publisher octoMapPubFull_; ros::Publisher octoMapPubFull_;
ros::Publisher octoMapCloud_; ros::Publisher octoMapCloud_;
ros::Publisher octoMapGroundCloud_;
ros::Publisher octoMapObstacleCloud_;
ros::Publisher octoMapEmptySpace_; ros::Publisher octoMapEmptySpace_;
ros::Publisher octoMapProj_; ros::Publisher octoMapProj_;
src/MapsManager.cpp
+34 -3
View File
@@ -152,6 +152,8 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
octoMapPubBin_ = nht->advertise<octomap_msgs::Octomap>("octomap_binary", 1, latch); octoMapPubBin_ = nht->advertise<octomap_msgs::Octomap>("octomap_binary", 1, latch);
octoMapPubFull_ = nht->advertise<octomap_msgs::Octomap>("octomap_full", 1, latch); octoMapPubFull_ = nht->advertise<octomap_msgs::Octomap>("octomap_full", 1, latch);
octoMapCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_occupied_space", 1, latch); octoMapCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_occupied_space", 1, latch);
octoMapObstacleCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_obstacles", 1, latch);
octoMapGroundCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_ground", 1, latch);
octoMapEmptySpace_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_empty_space", 1, latch); octoMapEmptySpace_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_empty_space", 1, latch);
octoMapProj_ = nht->advertise<nav_msgs::OccupancyGrid>("octomap_grid", 1, latch); octoMapProj_ = nht->advertise<nav_msgs::OccupancyGrid>("octomap_grid", 1, latch);
#endif #endif
@@ -319,6 +321,8 @@ bool MapsManager::hasSubscribers() const
octoMapPubBin_.getNumSubscribers() != 0 || octoMapPubBin_.getNumSubscribers() != 0 ||
octoMapPubFull_.getNumSubscribers() != 0 || octoMapPubFull_.getNumSubscribers() != 0 ||
octoMapCloud_.getNumSubscribers() != 0 || octoMapCloud_.getNumSubscribers() != 0 ||
octoMapObstacleCloud_.getNumSubscribers() != 0 ||
octoMapGroundCloud_.getNumSubscribers() != 0 ||
octoMapEmptySpace_.getNumSubscribers() != 0 || octoMapEmptySpace_.getNumSubscribers() != 0 ||
octoMapProj_.getNumSubscribers() != 0; octoMapProj_.getNumSubscribers() != 0;
} }
@@ -349,6 +353,8 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
octoMapPubBin_.getNumSubscribers() != 0 || octoMapPubBin_.getNumSubscribers() != 0 ||
octoMapPubFull_.getNumSubscribers() != 0 || octoMapPubFull_.getNumSubscribers() != 0 ||
octoMapCloud_.getNumSubscribers() != 0 || octoMapCloud_.getNumSubscribers() != 0 ||
octoMapObstacleCloud_.getNumSubscribers() != 0 ||
octoMapGroundCloud_.getNumSubscribers() != 0 ||
octoMapEmptySpace_.getNumSubscribers() != 0 || octoMapEmptySpace_.getNumSubscribers() != 0 ||
octoMapProj_.getNumSubscribers() != 0; octoMapProj_.getNumSubscribers() != 0;
@@ -1049,6 +1055,8 @@ void MapsManager::publishMaps(
if(octoMapPubBin_.getNumSubscribers() || if(octoMapPubBin_.getNumSubscribers() ||
octoMapPubFull_.getNumSubscribers() || octoMapPubFull_.getNumSubscribers() ||
octoMapCloud_.getNumSubscribers() || octoMapCloud_.getNumSubscribers() ||
octoMapObstacleCloud_.getNumSubscribers() ||
octoMapGroundCloud_.getNumSubscribers() ||
octoMapEmptySpace_.getNumSubscribers() || octoMapEmptySpace_.getNumSubscribers() ||
octoMapProj_.getNumSubscribers()) octoMapProj_.getNumSubscribers())
{ {
@@ -1068,21 +1076,44 @@ void MapsManager::publishMaps(
msg.header.stamp = stamp; msg.header.stamp = stamp;
octoMapPubFull_.publish(msg); octoMapPubFull_.publish(msg);
} }
if(octoMapCloud_.getNumSubscribers() || octoMapEmptySpace_.getNumSubscribers()) if(octoMapCloud_.getNumSubscribers() ||
octoMapObstacleCloud_.getNumSubscribers() ||
octoMapGroundCloud_.getNumSubscribers() ||
octoMapEmptySpace_.getNumSubscribers())
{ {
sensor_msgs::PointCloud2 msg; sensor_msgs::PointCloud2 msg;
pcl::IndicesPtr obstacleIndices(new std::vector<int>); pcl::IndicesPtr obstacleIndices(new std::vector<int>);
pcl::IndicesPtr emptyIndices(new std::vector<int>); pcl::IndicesPtr emptyIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get()); pcl::IndicesPtr groundIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get(), groundIndices.get());
if(octoMapCloud_.getNumSubscribers()) if(octoMapCloud_.getNumSubscribers())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudOccupiedSpace;
pcl::IndicesPtr indices = util3d::concatenate(obstacleIndices, groundIndices);
pcl::copyPointCloud(*cloud, *indices, cloudOccupiedSpace);
pcl::toROSMsg(cloudOccupiedSpace, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapCloud_.publish(msg);
}
if(octoMapObstacleCloud_.getNumSubscribers())
{ {
pcl::PointCloud<pcl::PointXYZRGB> cloudObstacles; pcl::PointCloud<pcl::PointXYZRGB> cloudObstacles;
pcl::copyPointCloud(*cloud, *obstacleIndices, cloudObstacles); pcl::copyPointCloud(*cloud, *obstacleIndices, cloudObstacles);
pcl::toROSMsg(cloudObstacles, msg); pcl::toROSMsg(cloudObstacles, msg);
msg.header.frame_id = mapFrameId; msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp; msg.header.stamp = stamp;
octoMapCloud_.publish(msg); octoMapObstacleCloud_.publish(msg);
}
if(octoMapGroundCloud_.getNumSubscribers())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudGround;
pcl::copyPointCloud(*cloud, *groundIndices, cloudGround);
pcl::toROSMsg(cloudGround, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapGroundCloud_.publish(msg);
} }
if(octoMapEmptySpace_.getNumSubscribers()) if(octoMapEmptySpace_.getNumSubscribers())
{ {