i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-12 22:30:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added new octomap topics

Browse Source
This commit is contained in:
matlabbe
2016-06-28 19:02:01 -04:00
parent fd32e28c11
commit 980291ea57
7 changed files with 314 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+13 -13
View File
@@ -176,6 +176,19 @@ SET(rtabmap_ros_lib_src
)
ENDIF(costmap_2d_FOUND)
# If octomap is found, add definition
IF(octomap_ros_FOUND)
MESSAGE(STATUS "WITH octomap")
include_directories(
${octomap_ros_INCLUDE_DIRS}
)
SET(Libraries
${octomap_ros_LIBRARIES}
${Libraries}
)
ADD_DEFINITIONS("-DWITH_OCTOMAP_ROS")
ENDIF(octomap_ros_FOUND)
IF(QT4_FOUND OR Qt5_FOUND)
SET(Libraries
${Libraries}
@@ -244,19 +257,6 @@ IF(Qt5_FOUND)
ENDIF(Qt5_FOUND)
add_dependencies(rtabmap_ros ${${PROJECT_NAME}_EXPORTED_TARGETS})
# If octomap is found, add definition
IF(octomap_ros_FOUND)
MESSAGE(STATUS "WITH octomap")
include_directories(
${octomap_ros_INCLUDE_DIRS}
)
SET(Libraries
${octomap_ros_LIBRARIES}
${Libraries}
)
add_definitions(-DWITH_OCTOMAP)
ENDIF(octomap_ros_FOUND)
add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
target_link_libraries(rtabmap rtabmap_ros ${Libraries})
package.xml
-2
View File
@@ -39,7 +39,6 @@
<build_depend>move_base_msgs</build_depend>
<build_depend>costmap_2d</build_depend>
<build_depend>octomap_ros</build_depend>
<build_depend>octomap</build_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>roscpp</run_depend>
@@ -69,7 +68,6 @@
<run_depend>move_base_msgs</run_depend>
<run_depend>costmap_2d</run_depend>
<run_depend>octomap_ros</run_depend>
<run_depend>octomap</run_depend>
<build_depend>libpcl-all-dev</build_depend>
src/CoreWrapper.cpp
+30 -19
View File
@@ -51,13 +51,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_surface.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/OdometryEvent.h>
#include <rtabmap/core/Version.h>
#include <pcl_conversions/pcl_conversions.h>
#include <laser_geometry/laser_geometry.h>
#ifdef WITH_OCTOMAP
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
#include <octomap_msgs/conversions.h>
#include <rtabmap/core/OctoMap.h>
#endif
#endif
#define BAD_COVARIANCE 9999
@@ -411,9 +415,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
cancelGoalSrv_ = nh.advertiseService("cancel_goal", &CoreWrapper::cancelGoalCallback, this);
setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this);
#ifdef WITH_OCTOMAP
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this);
octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this);
#endif
#endif
//private services
setLogDebugSrv_ = pnh.advertiseService("log_debug", &CoreWrapper::setLogDebug, this);
@@ -1458,6 +1464,8 @@ void CoreWrapper::process(
if(rtabmap_.isIDsGenerated() || data.id() > 0)
{
double timeRtabmap = 0.0;
double timeUpdateMaps = 0.0;
double timePublishMaps = 0.0;
if(rtabmap_.process(data, odom, OdometryEvent::generateCovarianceMatrix(odomRotationalVariance, odomTransitionalVariance)))
{
timeRtabmap = timer.ticks();
@@ -1491,8 +1499,11 @@ void CoreWrapper::process(
false,
false,
false,
false,
tmpSignature);
timeUpdateMaps = timer.ticks();
mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_);
// update goal if planning is enabled
@@ -1564,17 +1575,20 @@ void CoreWrapper::process(
}
}
}
timePublishMaps = timer.ticks();
}
}
else
{
timeRtabmap = timer.ticks();
}
ROS_INFO("rtabmap: Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Pub=%.4fs (local map=%d, WM=%d)",
ROS_INFO("rtabmap: Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)",
rate_>0?1.0f/rate_:0,
rtabmap_.getTimeThreshold()/1000.0f,
timeRtabmap,
timer.ticks(),
timeUpdateMaps,
timePublishMaps,
(int)rtabmap_.getLocalOptimizedPoses().size(),
rtabmap_.getWMSize()+rtabmap_.getSTMSize());
}
@@ -1953,6 +1967,7 @@ bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::
false,
true,
false,
false,
false);
if(filteredPoses.size())
{
@@ -1997,6 +2012,7 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::
false,
false,
true,
false,
false);
if(filteredPoses.size())
{
@@ -2110,6 +2126,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
false,
false,
false,
false,
signatures);
}
else
@@ -2564,7 +2581,8 @@ void CoreWrapper::publishGlobalPath(const ros::Time & stamp)
}
}
#ifdef WITH_OCTOMAP
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
bool CoreWrapper::octomapBinaryCallback(
octomap_msgs::GetOctomap::Request &req,
octomap_msgs::GetOctomap::Response &res)
@@ -2574,14 +2592,10 @@ bool CoreWrapper::octomapBinaryCallback(
res.map.header.stamp = ros::Time::now();
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false, false);
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true);
octomap::OcTree * octree = mapsManager_.createOctomap(poses);
bool success = octree != 0 && octree->size() && octomap_msgs::binaryMapToMsg(*octree, res.map);
if(octree)
{
delete octree;
}
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
return success;
}
@@ -2594,17 +2608,14 @@ bool CoreWrapper::octomapFullCallback(
res.map.header.stamp = ros::Time::now();
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false, false);
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true);
octomap::OcTree * octree = mapsManager_.createOctomap(poses);
bool success = octree != 0 && octree->size() && octomap_msgs::fullMapToMsg(*octree, res.map);
if(octree)
{
delete octree;
}
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);
return success;
}
#endif
#endif
/**
* exclusive callbacks:
src/CoreWrapper.h
+3 -3
View File
@@ -65,7 +65,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#ifdef WITH_OCTOMAP
#ifdef WITH_OCTOMAP_ROS
#include <octomap_msgs/GetOctomap.h>
#endif
@@ -234,7 +234,7 @@ private:
bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res);
#ifdef WITH_OCTOMAP
#ifdef WITH_OCTOMAP_ROS
bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res);
bool octomapFullCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res);
#endif
@@ -457,7 +457,7 @@ private:
ros::ServiceServer cancelGoalSrv_;
ros::ServiceServer setLabelSrv_;
ros::ServiceServer listLabelsSrv_;
#ifdef WITH_OCTOMAP
#ifdef WITH_OCTOMAP_ROS
ros::ServiceServer octomapBinarySrv_;
ros::ServiceServer octomapFullSrv_;
#endif
src/MapAssemblerNode.cpp
+1
View File
@@ -101,6 +101,7 @@ public:
false,
false,
false,
false,
nodes_);
mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);
src/MapsManager.cpp
+257 -63
View File
@@ -17,14 +17,18 @@
#include <rtabmap/core/util2d.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Version.h>
#include <nav_msgs/OccupancyGrid.h>
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#ifdef WITH_OCTOMAP
#include <octomap/octomap.h>
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
#include <octomap_msgs/conversions.h>
#include <rtabmap/core/OctoMap.h>
#endif
#endif
using namespace rtabmap;
@@ -56,7 +60,8 @@ MapsManager::MapsManager(bool usePublicNamespace) :
mapFilterRadius_(0.0),
mapFilterAngle_(30.0), // degrees
mapCacheCleanup_(true),
negativePosesIgnored(false)
negativePosesIgnored(false),
octomap_(0)
{
ros::NodeHandle nh;
@@ -120,6 +125,12 @@ MapsManager::MapsManager(bool usePublicNamespace) :
pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_);
pnh.param("map_negative_poses_ignored", negativePosesIgnored, negativePosesIgnored);
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
octomap_ = new OctoMap(gridCellSize_);
#endif
#endif
// If true, the last message published on
// the map topics will be saved and sent to new subscribers when they
// connect
@@ -133,6 +144,15 @@ MapsManager::MapsManager(bool usePublicNamespace) :
projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
scanMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
octoMapPubBin_ = nh.advertise<octomap_msgs::Octomap>("octomap_binary", 1, latch);
octoMapPubFull_ = nh.advertise<octomap_msgs::Octomap>("octomap_full", 1, latch);
octoMapCloud_ = nh.advertise<sensor_msgs::PointCloud2>("octomap_cloud", 1, latch);
octoMapCloudGround_ = nh.advertise<sensor_msgs::PointCloud2>("octomap_cloud_ground", 1, latch);
octoMapProj_ = nh.advertise<nav_msgs::OccupancyGrid>("octomap_proj", 1, latch);
#endif
#endif
}
else
{
@@ -140,11 +160,30 @@ MapsManager::MapsManager(bool usePublicNamespace) :
projMapPub_ = pnh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
gridMapPub_ = pnh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
scanMapPub_ = pnh.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
octoMapPubBin_ = pnh.advertise<octomap_msgs::Octomap>("octomap_binary", 1, latch);
octoMapPubFull_ = pnh.advertise<octomap_msgs::Octomap>("octomap_full", 1, latch);
octoMapCloud_ = pnh.advertise<sensor_msgs::PointCloud2>("octomap_cloud", 1, latch);
octoMapCloudGround_ = pnh.advertise<sensor_msgs::PointCloud2>("octomap_cloud_ground", 1, latch);
octoMapProj_ = pnh.advertise<nav_msgs::OccupancyGrid>("octomap_proj", 1, latch);
#endif
#endif
}
}
MapsManager::~MapsManager() {
clear();
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
if(octomap_)
{
delete octomap_;
octomap_ = 0;
}
#endif
#endif
}
void MapsManager::clear()
@@ -153,6 +192,11 @@ void MapsManager::clear()
cameraModels_.clear();
projMaps_.clear();
gridMaps_.clear();
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
octomap_->clear();
#endif
#endif
}
bool MapsManager::hasSubscribers() const
@@ -160,7 +204,12 @@ bool MapsManager::hasSubscribers() const
return cloudMapPub_.getNumSubscribers() != 0 ||
projMapPub_.getNumSubscribers() != 0 ||
gridMapPub_.getNumSubscribers() != 0 ||
scanMapPub_.getNumSubscribers() != 0;
scanMapPub_.getNumSubscribers() != 0 ||
octoMapPubBin_.getNumSubscribers() != 0 ||
octoMapPubFull_.getNumSubscribers() != 0 ||
octoMapCloud_.getNumSubscribers() != 0 ||
octoMapCloudGround_.getNumSubscribers() != 0 ||
octoMapProj_.getNumSubscribers() != 0;
}
std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Transform> & poses)
@@ -181,6 +230,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
bool updateProj,
bool updateGrid,
bool updateScan,
bool updateOctomap,
const std::map<int, rtabmap::Signature> & signatures)
{
if(!updateCloud && !updateProj && !updateGrid && !updateScan)
@@ -190,8 +240,22 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
updateProj = projMapPub_.getNumSubscribers() != 0;
updateGrid = gridMapPub_.getNumSubscribers() != 0;
updateScan = scanMapPub_.getNumSubscribers() != 0;
updateOctomap =
octoMapPubBin_.getNumSubscribers() != 0 ||
octoMapPubFull_.getNumSubscribers() != 0 ||
octoMapCloud_.getNumSubscribers() != 0 ||
octoMapCloudGround_.getNumSubscribers() != 0 ||
octoMapProj_.getNumSubscribers() != 0;
}
#ifndef WITH_OCTOMAP_ROS
updateOctomap = false;
#endif
#ifndef RTABMAP_OCTOMAP
updateOctomap = false;
#endif
UDEBUG("Updating map caches...");
if(!memory && signatures.size() == 0)
@@ -203,7 +267,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
std::map<int, rtabmap::Transform> filteredPoses;
// update cache
if(updateCloud || updateProj || updateGrid || updateScan)
if(updateCloud || updateProj || updateGrid || updateScan || updateOctomap)
{
// filter nodes
if(mapFilterRadius_ > 0.0)
@@ -279,6 +343,18 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
bool gridRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first));
bool scanRequired = updateScan && (iter->first < 0 || !uContains(scans_, iter->first));
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
if(!rgbDepthRequired)
{
rgbDepthRequired = updateOctomap &&
(iter->first < 0 ||
octomap_->addedNodes().empty() ||
iter->first > octomap_->addedNodes().rbegin()->first);
}
#endif
#endif
if(rgbDepthRequired ||
depthRequired ||
scanRequired ||
@@ -398,9 +474,9 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
uInsert(cameraModels_, std::make_pair(iter->first, models));
}
if(depthRequired)
if(depthRequired || updateOctomap)
{
UDEBUG("Creating proj map for %d...", iter->first);
UDEBUG("Creating proj map / octomap for %d...", iter->first);
cv::Mat ground, obstacles;
if(cloudRGB.get())
{
@@ -420,10 +496,56 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
iter->second.getEulerAngles(roll, pitch, yaw);
cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0));
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_);
pcl::IndicesPtr groundIndices, obstaclesIndices;
util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
cloudClipped,
groundIndices,
obstaclesIndices,
20,
projMaxGroundAngle_*M_PI/180.0,
gridCellSize_*2.0f,
projMinClusterSize_,
projDetectFlatObstacles_,
projMaxGroundHeight_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(groundIndices->size())
{
pcl::copyPointCloud(*cloudClipped, *groundIndices, *groundCloud);
}
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloudClipped, *obstaclesIndices, *obstaclesCloud);
}
if(updateProj)
{
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
groundCloud,
obstaclesCloud,
ground,
obstacles,
gridCellSize_);
uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
}
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
if(updateOctomap)
{
Transform tinv = Transform(0,0,0, roll, pitch, 0).inverse();
groundCloud = util3d::transformPointCloud(groundCloud, tinv);
obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv);
octomap_->addToCache(iter->first, groundCloud, obstaclesCloud);
}
#endif
#endif
}
}
else if(cloudXYZ.get())
else if(updateProj && cloudXYZ.get())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudClipped = cloudXYZ;
if(cloudClipped->size() && projMaxObstaclesHeight_ > 0)
@@ -437,11 +559,28 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
iter->second.getEulerAngles(roll, pitch, yaw);
cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0));
UDEBUG("util3d::occupancy2DFromCloud3D()");
util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_);
pcl::IndicesPtr groundIndices, obstaclesIndices;
util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
cloudClipped,
groundIndices,
obstaclesIndices,
20,
projMaxGroundAngle_*M_PI/180.0,
gridCellSize_*2.0f,
projMinClusterSize_,
projDetectFlatObstacles_,
projMaxGroundHeight_);
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(
cloudClipped,
groundIndices,
obstaclesIndices,
ground,
obstacles,
gridCellSize_);
uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
}
}
uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
}
if(scanRequired || gridRequired)
@@ -502,6 +641,17 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
}
}
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
if(updateOctomap)
{
UTimer time;
octomap_->update(filteredPoses);
ROS_INFO("Octomap update time = %fs", time.ticks());
}
#endif
#endif
// cleanup not used nodes
UDEBUG("Cleanup not used nodes");
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=clouds_.begin();
@@ -684,6 +834,101 @@ void MapsManager::publishMaps(
clouds_.clear();
cameraModels_.clear();
}
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
if(octoMapPubBin_.getNumSubscribers() ||
octoMapPubFull_.getNumSubscribers() ||
octoMapCloud_.getNumSubscribers() ||
octoMapCloudGround_.getNumSubscribers() ||
octoMapProj_.getNumSubscribers())
{
if(octoMapPubBin_.getNumSubscribers())
{
octomap_msgs::Octomap msg;
octomap_msgs::binaryMapToMsg(*octomap_->octree(), msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapPubBin_.publish(msg);
}
if(octoMapPubFull_.getNumSubscribers())
{
octomap_msgs::Octomap msg;
octomap_msgs::binaryMapToMsg(*octomap_->octree(), msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapPubFull_.publish(msg);
}
if(octoMapCloud_.getNumSubscribers() || octoMapCloudGround_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::IndicesPtr obstacles(new std::vector<int>);
pcl::IndicesPtr ground(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(obstacles.get(), ground.get());
if(octoMapCloud_.getNumSubscribers())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudObstacles;
pcl::copyPointCloud(*cloud, *obstacles, cloudObstacles);
pcl::toROSMsg(cloudObstacles, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapCloud_.publish(msg);
}
if(octoMapCloudGround_.getNumSubscribers())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudGround;
pcl::copyPointCloud(*cloud, *ground, cloudGround);
pcl::toROSMsg(cloudGround, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapCloudGround_.publish(msg);
}
}
if(octoMapProj_.getNumSubscribers())
{
// create the projection map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = octomap_->createProjectionMap(xMin, yMin, gridCellSize, gridSize_);
if(!pixels.empty())
{
//init
nav_msgs::OccupancyGrid map;
map.info.resolution = gridCellSize;
map.info.origin.position.x = 0.0;
map.info.origin.position.y = 0.0;
map.info.origin.position.z = 0.0;
map.info.origin.orientation.x = 0.0;
map.info.origin.orientation.y = 0.0;
map.info.origin.orientation.z = 0.0;
map.info.origin.orientation.w = 1.0;
map.info.width = pixels.cols;
map.info.height = pixels.rows;
map.info.origin.position.x = xMin;
map.info.origin.position.y = yMin;
map.data.resize(map.info.width * map.info.height);
memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
map.header.frame_id = mapFrameId;
map.header.stamp = stamp;
octoMapProj_.publish(map);
}
else if(poses.size())
{
ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size());
}
}
}
else
{
octomap_->clear();
}
#endif
#endif
if(scanMapPub_.getNumSubscribers())
{
@@ -850,54 +1095,3 @@ cv::Mat MapsManager::generateGridMap(
return map;
}
#ifdef WITH_OCTOMAP
// returned OcTree must be deleted
// RTAB-Map optimizes the graph at almost each iteration, an octomap cannot
// be updated online. Only available on service. To have an "online" octomap published as a topic,
// you may want to subscribe an octomap_server to /rtabmap/cloud topic.
//
octomap::OcTree * MapsManager::createOctomap(const std::map<int, Transform> & poses)
{
octomap::OcTree * octree = new octomap::OcTree(gridCellSize_);
UTimer time;
for(std::map<int, Transform>::const_iterator posesIter = poses.begin(); posesIter!=poses.end(); ++posesIter)
{
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator cloudsIter = clouds_.find(posesIter->first);
if(cloudsIter != clouds_.end() && cloudsIter->second->size())
{
octomap::Pointcloud * scan = new octomap::Pointcloud();
//octomap::pointcloudPCLToOctomap(*cloudsIter->second, *scan); // Not anymore in Indigo!
scan->reserve(cloudsIter->second->size());
for(pcl::PointCloud<pcl::PointXYZRGB>::const_iterator it = cloudsIter->second->begin();
it != cloudsIter->second->end();
++it)
{
// Check if the point is invalid
if(pcl::isFinite(*it))
{
scan->push_back(it->x, it->y, it->z);
}
}
float x,y,z, r,p,w;
posesIter->second.getTranslationAndEulerAngles(x,y,z,r,p,w);
octomap::ScanNode node(scan, octomap::pose6d(x,y,z, r,p,w), posesIter->first);
octree->insertPointCloud(node, cloudMaxDepth_, true, true);
ROS_INFO("inserted %d pt=%d (%fs)", posesIter->first, (int)scan->size(), time.ticks());
}
}
octree->updateInnerOccupancy();
ROS_INFO("updated inner occupancy (%fs)", time.ticks());
// clear memory if no one subscribed
if(mapCacheCleanup_ && cloudMapPub_.getNumSubscribers() == 0)
{
clouds_.clear();
cameraModels_.clear();
}
return octree;
}
#endif
src/MapsManager.h
+10 -8
View File
@@ -14,12 +14,8 @@
#include <ros/time.h>
#include <ros/publisher.h>
namespace octomap{
class OcTree;
}
namespace rtabmap {
class OctoMap;
class Memory;
} // namespace rtabmap
@@ -41,6 +37,7 @@ public:
bool updateProj,
bool updateGrid,
bool updateScan,
bool updateOctomap,
const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
void publishMaps(
@@ -60,9 +57,7 @@ public:
float & yMin,
float & gridCellSize);
#ifdef WITH_OCTOMAP
octomap::OcTree * createOctomap(const std::map<int, rtabmap::Transform> & poses);
#endif
rtabmap::OctoMap * getOctomap() const {return octomap_;}
private:
// mapping stuff
@@ -98,12 +93,19 @@ private:
ros::Publisher projMapPub_;
ros::Publisher gridMapPub_;
ros::Publisher scanMapPub_;
ros::Publisher octoMapPubBin_;
ros::Publisher octoMapPubFull_;
ros::Publisher octoMapCloud_;
ros::Publisher octoMapCloudGround_;
ros::Publisher octoMapProj_;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
std::map<int, std::vector<rtabmap::CameraModel> > cameraModels_;
std::map<int, std::pair<cv::Mat, cv::Mat> > projMaps_; // <ground, obstacles>
std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <ground, obstacles>
rtabmap::OctoMap * octomap_;
};
#endif /* MAPSMANAGER_H_ */