mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
CoreWrapper.cpp: Moved map generation stuff in MapsManager class (that reduces a little the memory used for the compilation of CoreWrapper.cpp).
This commit is contained in:
Mathieu Labbe
+1
-1
@@ -180,7 +180,7 @@ SET(Libraries
|
||||
add_definitions(-DWITH_OCTOMAP)
|
||||
ENDIF(octomap_ros_FOUND)
|
||||
|
||||
add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
|
||||
add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp src/MapsManager.cpp)
|
||||
add_dependencies(rtabmap rtabmap_generate_messages_cpp)
|
||||
target_link_libraries(rtabmap rtabmap_ros ${Libraries})
|
||||
|
||||
|
||||
+52
-604
@@ -33,23 +33,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <nav_msgs/Path.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <std_msgs/Int32MultiArray.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/utilite/UDirectory.h>
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
#include <rtabmap/core/util3d_mapping.h>
|
||||
#include <rtabmap/core/util3d_filtering.h>
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
|
||||
#include <rtabmap/core/util3d_conversions.h>
|
||||
#include <rtabmap/core/util2d.h>
|
||||
#include <rtabmap/core/Graph.h>
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
#include <rtabmap/core/Memory.h>
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
@@ -58,9 +54,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <image_geometry/stereo_camera_model.h>
|
||||
|
||||
#ifdef WITH_OCTOMAP
|
||||
#include <octomap/octomap.h>
|
||||
#include <octomap_ros/conversions.h>
|
||||
#include <octomap_msgs/Octomap.h>
|
||||
#include <octomap_msgs/conversions.h>
|
||||
#endif
|
||||
|
||||
@@ -94,19 +87,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
||||
databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
|
||||
waitForTransform_(false),
|
||||
useActionForGoal_(false),
|
||||
cloudDecimation_(4),
|
||||
cloudMaxDepth_(4.0), // meters
|
||||
cloudVoxelSize_(0.05), // meters
|
||||
cloudOutputVoxelized_(false),
|
||||
projMaxGroundAngle_(45.0), // degrees
|
||||
projMinClusterSize_(20),
|
||||
projMaxHeight_(2.0), // meters
|
||||
gridCellSize_(0.05), // meters
|
||||
gridSize_(0), // meters
|
||||
gridEroded_(false),
|
||||
mapFilterRadius_(0.5),
|
||||
mapFilterAngle_(30.0), // degrees
|
||||
mapCacheCleanup_(true),
|
||||
mapToOdom_(rtabmap::Transform::getIdentity()),
|
||||
depthSync_(0),
|
||||
depthScanSync_(0),
|
||||
@@ -166,27 +146,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
||||
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
|
||||
pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
|
||||
|
||||
// cloud map stuff
|
||||
pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
|
||||
pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
|
||||
pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
|
||||
pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_);
|
||||
|
||||
//projection map stuff
|
||||
pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_);
|
||||
pnh.param("proj_min_cluster_size", projMinClusterSize_, projMinClusterSize_);
|
||||
pnh.param("proj_max_height", projMaxHeight_, projMaxHeight_);
|
||||
|
||||
// common grid map stuff
|
||||
pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m
|
||||
pnh.param("grid_size", gridSize_, gridSize_); // m
|
||||
pnh.param("grid_eroded", gridEroded_, gridEroded_);
|
||||
|
||||
// common map stuff
|
||||
pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
|
||||
pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
|
||||
pnh.param("map_cache_cleanup", mapCacheCleanup_, mapCacheCleanup_);
|
||||
|
||||
ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str());
|
||||
if(!odomFrameId_.empty())
|
||||
{
|
||||
@@ -201,11 +160,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
||||
mapGraphPub_ = nh.advertise<rtabmap_ros::Graph>("graph", 1);
|
||||
labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 1);
|
||||
|
||||
// mapping topics
|
||||
cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1);
|
||||
projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1);
|
||||
gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
|
||||
|
||||
// planning topics
|
||||
goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
|
||||
goalGlobalSub_ = nh.subscribe("goal_global", 1, &CoreWrapper::goalGlobalCallback, this);
|
||||
@@ -1130,30 +1084,13 @@ void CoreWrapper::process(
|
||||
this->publishStats(stamp);
|
||||
std::map<int, rtabmap::Transform> filteredPoses;
|
||||
|
||||
filteredPoses = this->updateMapCaches(rtabmap_.getLocalOptimizedPoses(),
|
||||
cloudMapPub_.getNumSubscribers() != 0,
|
||||
projMapPub_.getNumSubscribers() != 0,
|
||||
gridMapPub_.getNumSubscribers() != 0);
|
||||
this->publishMaps(filteredPoses, stamp);
|
||||
|
||||
// clear memory if no one subscribed
|
||||
if(mapCacheCleanup_)
|
||||
{
|
||||
if(cloudMapPub_.getNumSubscribers() == 0)
|
||||
{
|
||||
clouds_.clear();
|
||||
}
|
||||
|
||||
if(projMapPub_.getNumSubscribers() == 0)
|
||||
{
|
||||
projMaps_.clear();
|
||||
}
|
||||
|
||||
if(gridMapPub_.getNumSubscribers() == 0)
|
||||
{
|
||||
gridMaps_.clear();
|
||||
}
|
||||
}
|
||||
filteredPoses = mapsManager_.updateMapCaches(
|
||||
rtabmap_.getLocalOptimizedPoses(),
|
||||
rtabmap_.getMemory(),
|
||||
false,
|
||||
false,
|
||||
false);
|
||||
mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_);
|
||||
|
||||
// update goal if planning is enabled
|
||||
if(!currentMetricGoal_.isNull())
|
||||
@@ -1391,9 +1328,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
|
||||
lastPose_.setIdentity();
|
||||
currentMetricGoal_.setNull();
|
||||
latestNodeWasReached_ = false;
|
||||
clouds_.clear();
|
||||
projMaps_.clear();
|
||||
gridMaps_.clear();
|
||||
mapsManager_.clear();
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -1550,29 +1485,22 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
|
||||
bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
|
||||
{
|
||||
std::map<int, rtabmap::Transform> filteredPoses;
|
||||
filteredPoses = this->updateMapCaches(rtabmap_.getLocalOptimizedPoses(), false, true, false);
|
||||
if(filteredPoses.size() && projMaps_.size())
|
||||
filteredPoses = mapsManager_.updateMapCaches(
|
||||
rtabmap_.getLocalOptimizedPoses(),
|
||||
rtabmap_.getMemory(),
|
||||
false,
|
||||
true,
|
||||
false);
|
||||
if(filteredPoses.size())
|
||||
{
|
||||
// create the projection map
|
||||
float xMin=0.0f, yMin=0.0f;
|
||||
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
|
||||
filteredPoses,
|
||||
projMaps_,
|
||||
gridCellSize_,
|
||||
xMin, yMin,
|
||||
gridSize_,
|
||||
gridEroded_);
|
||||
|
||||
// clear memory if no one subscribed
|
||||
if(mapCacheCleanup_ && projMapPub_.getNumSubscribers() == 0)
|
||||
{
|
||||
projMaps_.clear();
|
||||
}
|
||||
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
|
||||
cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize);
|
||||
|
||||
if(!pixels.empty())
|
||||
{
|
||||
//init
|
||||
res.map.info.resolution = gridCellSize_;
|
||||
res.map.info.resolution = gridCellSize;
|
||||
res.map.info.origin.position.x = 0.0;
|
||||
res.map.info.origin.position.y = 0.0;
|
||||
res.map.info.origin.position.z = 0.0;
|
||||
@@ -1600,29 +1528,22 @@ bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::
|
||||
bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
|
||||
{
|
||||
std::map<int, rtabmap::Transform> filteredPoses;
|
||||
filteredPoses = this->updateMapCaches(rtabmap_.getLocalOptimizedPoses(), false, false, true);
|
||||
if(filteredPoses.size() && gridMaps_.size())
|
||||
filteredPoses = mapsManager_.updateMapCaches(
|
||||
rtabmap_.getLocalOptimizedPoses(),
|
||||
rtabmap_.getMemory(),
|
||||
false,
|
||||
false,
|
||||
true);
|
||||
if(filteredPoses.size())
|
||||
{
|
||||
// create the projection map
|
||||
float xMin=0.0f, yMin=0.0f;
|
||||
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
|
||||
filteredPoses,
|
||||
gridMaps_,
|
||||
gridCellSize_,
|
||||
xMin, yMin,
|
||||
gridSize_,
|
||||
gridEroded_);
|
||||
|
||||
// clear memory if no one subscribed
|
||||
if(mapCacheCleanup_ && gridMapPub_.getNumSubscribers() == 0)
|
||||
{
|
||||
gridMaps_.clear();
|
||||
}
|
||||
// create the grid map
|
||||
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
|
||||
cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize);
|
||||
|
||||
if(!pixels.empty())
|
||||
{
|
||||
//init
|
||||
res.map.info.resolution = gridCellSize_;
|
||||
res.map.info.resolution = gridCellSize;
|
||||
res.map.info.origin.position.x = 0.0;
|
||||
res.map.info.origin.position.y = 0.0;
|
||||
res.map.info.origin.position.z = 0.0;
|
||||
@@ -1653,7 +1574,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
|
||||
|
||||
if(mapDataPub_.getNumSubscribers() ||
|
||||
mapGraphPub_.getNumSubscribers() ||
|
||||
(!req.graphOnly && (cloudMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers() || gridMapPub_.getNumSubscribers())))
|
||||
!req.graphOnly)
|
||||
{
|
||||
std::map<int, Signature> signatures;
|
||||
std::map<int, Transform> poses;
|
||||
@@ -1739,42 +1660,19 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
|
||||
std::map<int, Transform> filteredPoses;
|
||||
if(signatures.size())
|
||||
{
|
||||
filteredPoses = this->updateMapCaches(poses,
|
||||
cloudMapPub_.getNumSubscribers() != 0,
|
||||
projMapPub_.getNumSubscribers() != 0,
|
||||
gridMapPub_.getNumSubscribers() != 0,
|
||||
signatures);
|
||||
}
|
||||
else if(mapFilterRadius_ > 0.0)
|
||||
{
|
||||
// filter nodes
|
||||
double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
|
||||
filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
|
||||
filteredPoses = mapsManager_.updateMapCaches(
|
||||
poses,
|
||||
rtabmap_.getMemory(),
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
signatures);
|
||||
}
|
||||
else
|
||||
{
|
||||
filteredPoses = poses;
|
||||
}
|
||||
publishMaps(filteredPoses, now);
|
||||
|
||||
// clear memory if no one subscribed
|
||||
if(mapCacheCleanup_)
|
||||
{
|
||||
if(cloudMapPub_.getNumSubscribers() == 0)
|
||||
{
|
||||
clouds_.clear();
|
||||
}
|
||||
|
||||
if(projMapPub_.getNumSubscribers() == 0)
|
||||
{
|
||||
projMaps_.clear();
|
||||
}
|
||||
|
||||
if(gridMapPub_.getNumSubscribers() == 0)
|
||||
{
|
||||
gridMaps_.clear();
|
||||
}
|
||||
filteredPoses = mapsManager_.getFilteredPoses(poses);
|
||||
}
|
||||
mapsManager_.publishMaps(filteredPoses, now, mapFrameId_);
|
||||
}
|
||||
|
||||
if(labelsPub_.getNumSubscribers())
|
||||
@@ -2073,409 +1971,6 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
|
||||
}
|
||||
}
|
||||
|
||||
std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
|
||||
const std::map<int, rtabmap::Transform> & poses,
|
||||
bool updateCloud,
|
||||
bool updateProj,
|
||||
bool updateGrid,
|
||||
const std::map<int, Signature> & signatures)
|
||||
{
|
||||
UDEBUG("Updating map caches...");
|
||||
|
||||
if(!rtabmap_.getMemory() && signatures.size() == 0)
|
||||
{
|
||||
ROS_FATAL("Memory not initialized!?");
|
||||
return std::map<int, rtabmap::Transform>();
|
||||
}
|
||||
|
||||
std::map<int, rtabmap::Transform> filteredPoses;
|
||||
|
||||
// update cache
|
||||
if(updateCloud || updateProj || updateGrid)
|
||||
{
|
||||
// filter nodes
|
||||
if(mapFilterRadius_ > 0.0)
|
||||
{
|
||||
double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
|
||||
filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
|
||||
}
|
||||
else
|
||||
{
|
||||
filteredPoses = poses;
|
||||
}
|
||||
|
||||
|
||||
for(std::map<int, Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
|
||||
{
|
||||
if(!iter->second.isNull())
|
||||
{
|
||||
Signature data;
|
||||
bool rgbDepthRequired = updateCloud && !uContains(clouds_, iter->first);
|
||||
bool depthRequired = updateProj && !uContains(projMaps_, iter->first);
|
||||
bool scanRequired = updateGrid && !uContains(gridMaps_, iter->first);
|
||||
if(rgbDepthRequired ||
|
||||
depthRequired ||
|
||||
scanRequired)
|
||||
{
|
||||
if(signatures.size())
|
||||
{
|
||||
std::map<int, Signature>::const_iterator findIter = signatures.find(iter->first);
|
||||
if(findIter != signatures.end())
|
||||
{
|
||||
data = findIter->second;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
data = rtabmap_.getMemory()->getSignatureDataConst(iter->first);
|
||||
}
|
||||
}
|
||||
|
||||
if(data.id() > 0)
|
||||
{
|
||||
Transform localTransform = data.getLocalTransform();
|
||||
if(!localTransform.isNull())
|
||||
{
|
||||
// Which data should we decompress?
|
||||
cv::Mat image, depth, scan;
|
||||
data.uncompressDataConst(rgbDepthRequired?&image:0, rgbDepthRequired||depthRequired?&depth:0, scanRequired?&scan:0);
|
||||
if(!depth.empty() &&
|
||||
depth.type() == CV_8UC1 &&
|
||||
image.empty() &&
|
||||
!rgbDepthRequired)
|
||||
{
|
||||
// Stereo detected, we should uncompress left image too
|
||||
data.uncompressDataConst(&image, 0, 0);
|
||||
}
|
||||
float fx = data.getFx();
|
||||
float fy = data.getFy();
|
||||
float cx = data.getCx();
|
||||
float cy = data.getCy();
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
|
||||
if(rgbDepthRequired)
|
||||
{
|
||||
if(!image.empty() &&
|
||||
!depth.empty() &&
|
||||
fx > 0.0f && fy > 0.0f &&
|
||||
cx >= 0.0f && cy >= 0.0f)
|
||||
{
|
||||
if(depth.type() == CV_8UC1)
|
||||
{
|
||||
cloudRGB = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloudDecimation_);
|
||||
}
|
||||
else
|
||||
{
|
||||
cloudRGB = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloudDecimation_);
|
||||
}
|
||||
|
||||
if(cloudRGB->size() && cloudMaxDepth_ > 0)
|
||||
{
|
||||
cloudRGB = util3d::passThrough(cloudRGB, "z", 0, cloudMaxDepth_);
|
||||
}
|
||||
if(cloudRGB->size() && cloudVoxelSize_ > 0)
|
||||
{
|
||||
cloudRGB = util3d::voxelize(cloudRGB, cloudVoxelSize_);
|
||||
}
|
||||
if(cloudRGB->size())
|
||||
{
|
||||
cloudRGB = util3d::transformPointCloud(cloudRGB, localTransform);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first);
|
||||
}
|
||||
}
|
||||
else if(depthRequired)
|
||||
{
|
||||
if( !depth.empty() &&
|
||||
fx > 0.0f && fy > 0.0f &&
|
||||
cx >= 0.0f && cy >= 0.0f)
|
||||
{
|
||||
if(depth.type() == CV_8UC1)
|
||||
{
|
||||
if(!image.empty())
|
||||
{
|
||||
cv::Mat leftMono;
|
||||
if(image.channels() == 3)
|
||||
{
|
||||
cv::cvtColor(image, leftMono, CV_BGR2GRAY);
|
||||
}
|
||||
else
|
||||
{
|
||||
leftMono = image;
|
||||
}
|
||||
cloudXYZ = rtabmap::util3d::cloudFromDisparity(
|
||||
util2d::disparityFromStereoImages(leftMono, depth),
|
||||
cx, cy,
|
||||
fx, fy,
|
||||
cloudDecimation_);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
cloudXYZ = util3d::cloudFromDepth(depth, cx, cy, fx, fy, cloudDecimation_);
|
||||
}
|
||||
|
||||
if(cloudXYZ.get())
|
||||
{
|
||||
if(cloudXYZ->size() && cloudMaxDepth_ > 0)
|
||||
{
|
||||
cloudXYZ = util3d::passThrough(cloudXYZ, "z", 0, cloudMaxDepth_);
|
||||
}
|
||||
if(cloudXYZ->size() && gridCellSize_ > 0)
|
||||
{
|
||||
// use gridCellSize since this cloud is only for the projection map
|
||||
cloudXYZ = util3d::voxelize(cloudXYZ, gridCellSize_);
|
||||
}
|
||||
if(cloudXYZ->size())
|
||||
{
|
||||
cloudXYZ = util3d::transformPointCloud(cloudXYZ, localTransform);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Left stereo image was empty! (node=%d)", iter->first);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first);
|
||||
}
|
||||
}
|
||||
|
||||
if(cloudRGB.get())
|
||||
{
|
||||
clouds_.insert(std::make_pair(iter->first, cloudRGB));
|
||||
}
|
||||
|
||||
if(depthRequired)
|
||||
{
|
||||
cv::Mat ground, obstacles;
|
||||
if(cloudRGB.get())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloudRGB;
|
||||
if(cloudClipped->size() && projMaxHeight_ > 0)
|
||||
{
|
||||
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
|
||||
}
|
||||
if(cloudClipped->size())
|
||||
{
|
||||
cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_);
|
||||
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
|
||||
}
|
||||
}
|
||||
else if(cloudXYZ.get())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudClipped = cloudXYZ;
|
||||
if(cloudClipped->size() && projMaxHeight_ > 0)
|
||||
{
|
||||
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
|
||||
}
|
||||
if(cloudClipped->size())
|
||||
{
|
||||
util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
|
||||
}
|
||||
}
|
||||
projMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
|
||||
}
|
||||
|
||||
if(scanRequired)
|
||||
{
|
||||
cv::Mat ground, obstacles;
|
||||
util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_);
|
||||
gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Local transform detected for node %d", iter->first);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Pose null for node %d", iter->first);
|
||||
}
|
||||
}
|
||||
|
||||
// cleanup not used nodes
|
||||
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=clouds_.begin();
|
||||
iter!=clouds_.end();)
|
||||
{
|
||||
if(!uContains(poses, iter->first))
|
||||
{
|
||||
clouds_.erase(iter++);
|
||||
}
|
||||
else
|
||||
{
|
||||
++iter;
|
||||
}
|
||||
}
|
||||
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=projMaps_.begin();
|
||||
iter!=projMaps_.end();)
|
||||
{
|
||||
if(!uContains(poses, iter->first))
|
||||
{
|
||||
projMaps_.erase(iter++);
|
||||
}
|
||||
else
|
||||
{
|
||||
++iter;
|
||||
}
|
||||
}
|
||||
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=gridMaps_.begin();
|
||||
iter!=gridMaps_.end();)
|
||||
{
|
||||
if(!uContains(poses, iter->first))
|
||||
{
|
||||
gridMaps_.erase(iter++);
|
||||
}
|
||||
else
|
||||
{
|
||||
++iter;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return filteredPoses;
|
||||
}
|
||||
|
||||
void CoreWrapper::publishMaps(
|
||||
const std::map<int, rtabmap::Transform> & poses,
|
||||
const ros::Time & stamp)
|
||||
{
|
||||
UDEBUG("Publishing maps...");
|
||||
|
||||
// publish maps
|
||||
if(cloudMapPub_.getNumSubscribers())
|
||||
{
|
||||
// generate the assembled cloud!
|
||||
UTimer time;
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
|
||||
int count = 0;
|
||||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first);
|
||||
if(jter != clouds_.end())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
|
||||
*assembledCloud+=*transformed;
|
||||
++count;
|
||||
}
|
||||
}
|
||||
|
||||
if(assembledCloud->size())
|
||||
{
|
||||
if(cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
|
||||
{
|
||||
assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_);
|
||||
}
|
||||
ROS_INFO("Assembled %d clouds (%fs)", count, time.ticks());
|
||||
|
||||
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
|
||||
pcl::toROSMsg(*assembledCloud, *cloudMsg);
|
||||
cloudMsg->header.stamp = stamp;
|
||||
cloudMsg->header.frame_id = mapFrameId_;
|
||||
cloudMapPub_.publish(cloudMsg);
|
||||
}
|
||||
else if(poses.size())
|
||||
{
|
||||
ROS_WARN("Cloud map is empty! (clouds=%d)", (int)clouds_.size());
|
||||
}
|
||||
}
|
||||
|
||||
if(projMapPub_.getNumSubscribers())
|
||||
{
|
||||
// create the projection map
|
||||
float xMin=0.0f, yMin=0.0f;
|
||||
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
|
||||
poses,
|
||||
projMaps_,
|
||||
gridCellSize_,
|
||||
xMin, yMin,
|
||||
gridSize_,
|
||||
gridEroded_);
|
||||
|
||||
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;
|
||||
|
||||
projMapPub_.publish(map);
|
||||
}
|
||||
else if(poses.size())
|
||||
{
|
||||
ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size());
|
||||
}
|
||||
}
|
||||
|
||||
if(gridMapPub_.getNumSubscribers())
|
||||
{
|
||||
// create the grid map
|
||||
float xMin=0.0f, yMin=0.0f;
|
||||
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
|
||||
poses,
|
||||
gridMaps_,
|
||||
gridCellSize_,
|
||||
xMin, yMin,
|
||||
gridSize_,
|
||||
gridEroded_);
|
||||
|
||||
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;
|
||||
|
||||
gridMapPub_.publish(map);
|
||||
}
|
||||
else if(poses.size())
|
||||
{
|
||||
ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
|
||||
{
|
||||
if(!currentMetricGoal_.isNull())
|
||||
@@ -2605,59 +2100,6 @@ void CoreWrapper::publishLocalPath(const ros::Time & stamp)
|
||||
}
|
||||
|
||||
#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 * CoreWrapper::createOctomap()
|
||||
{
|
||||
octomap::OcTree * octree = new octomap::OcTree(gridCellSize_);
|
||||
UTimer time;
|
||||
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
|
||||
if(clouds_.size() == 0)
|
||||
{
|
||||
poses = this->updateMapCaches(poses, true, false, false);
|
||||
}
|
||||
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();
|
||||
}
|
||||
return octree;
|
||||
}
|
||||
|
||||
bool CoreWrapper::octomapBinaryCallback(
|
||||
octomap_msgs::GetOctomap::Request &req,
|
||||
octomap_msgs::GetOctomap::Response &res)
|
||||
@@ -2666,7 +2108,10 @@ bool CoreWrapper::octomapBinaryCallback(
|
||||
res.map.header.frame_id = mapFrameId_;
|
||||
res.map.header.stamp = ros::Time::now();
|
||||
|
||||
octomap::OcTree * octree = createOctomap();
|
||||
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
|
||||
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false);
|
||||
|
||||
octomap::OcTree * octree = mapsManager_.createOctomap(poses);
|
||||
bool success = octree != 0 && octree->size() && octomap_msgs::binaryMapToMsg(*octree, res.map);
|
||||
if(octree)
|
||||
{
|
||||
@@ -2683,7 +2128,10 @@ bool CoreWrapper::octomapFullCallback(
|
||||
res.map.header.frame_id = mapFrameId_;
|
||||
res.map.header.stamp = ros::Time::now();
|
||||
|
||||
octomap::OcTree * octree = createOctomap();
|
||||
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
|
||||
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false);
|
||||
|
||||
octomap::OcTree * octree = mapsManager_.createOctomap(poses);
|
||||
bool success = octree != 0 && octree->size() && octomap_msgs::fullMapToMsg(*octree, res.map);
|
||||
if(octree)
|
||||
{
|
||||
|
||||
+3
-42
@@ -36,8 +36,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <tf/transform_listener.h>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
#include <std_msgs/Empty.h>
|
||||
#include <std_msgs/Int32.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
@@ -47,7 +45,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <nav_msgs/GetMap.h>
|
||||
|
||||
#include <rtabmap/core/Statistics.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/core/Rtabmap.h>
|
||||
|
||||
@@ -57,6 +54,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap_ros/SetGoal.h"
|
||||
#include "rtabmap_ros/SetLabel.h"
|
||||
|
||||
#include "MapsManager.h"
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
@@ -65,9 +64,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <image_transport/subscriber_filter.h>
|
||||
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
|
||||
#ifdef WITH_OCTOMAP
|
||||
#include <octomap_msgs/GetOctomap.h>
|
||||
#endif
|
||||
@@ -80,10 +76,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <actionlib_msgs/GoalStatusArray.h>
|
||||
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
|
||||
|
||||
namespace octomap{
|
||||
class OcTree;
|
||||
}
|
||||
|
||||
class CoreWrapper
|
||||
{
|
||||
public:
|
||||
@@ -207,24 +199,13 @@ private:
|
||||
|
||||
void publishLoop(double tfDelay);
|
||||
|
||||
std::map<int, rtabmap::Transform> updateMapCaches(const std::map<int, rtabmap::Transform> & poses,
|
||||
bool updateCloud,
|
||||
bool updateProj,
|
||||
bool updateGrid,
|
||||
const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
|
||||
|
||||
void publishStats(const ros::Time & stamp);
|
||||
void publishMaps(const std::map<int, rtabmap::Transform> & poses, const ros::Time & stamp);
|
||||
void publishCurrentGoal(const ros::Time & stamp);
|
||||
void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
|
||||
void goalActiveCb();
|
||||
void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
|
||||
void publishLocalPath(const ros::Time & stamp);
|
||||
|
||||
#ifdef WITH_OCTOMAP
|
||||
octomap::OcTree * createOctomap();
|
||||
#endif
|
||||
|
||||
private:
|
||||
rtabmap::Rtabmap rtabmap_;
|
||||
bool paused_;
|
||||
@@ -244,35 +225,15 @@ private:
|
||||
bool waitForTransform_;
|
||||
bool useActionForGoal_;
|
||||
|
||||
// mapping stuff
|
||||
int cloudDecimation_;
|
||||
double cloudMaxDepth_;
|
||||
double cloudVoxelSize_;
|
||||
bool cloudOutputVoxelized_;
|
||||
double projMaxGroundAngle_;
|
||||
int projMinClusterSize_;
|
||||
double projMaxHeight_;
|
||||
double gridCellSize_;
|
||||
double gridSize_;
|
||||
bool gridEroded_;
|
||||
double mapFilterRadius_;
|
||||
double mapFilterAngle_;
|
||||
bool mapCacheCleanup_;
|
||||
|
||||
rtabmap::Transform mapToOdom_;
|
||||
boost::mutex mapToOdomMutex_;
|
||||
|
||||
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
|
||||
std::map<int, std::pair<cv::Mat, cv::Mat> > projMaps_; // <ground, obstacles>
|
||||
std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <ground, obstacles>
|
||||
MapsManager mapsManager_;
|
||||
|
||||
ros::Publisher infoPub_;
|
||||
ros::Publisher mapDataPub_;
|
||||
ros::Publisher mapGraphPub_;
|
||||
ros::Publisher labelsPub_;
|
||||
ros::Publisher cloudMapPub_;
|
||||
ros::Publisher projMapPub_;
|
||||
ros::Publisher gridMapPub_;
|
||||
|
||||
//Planning stuff
|
||||
ros::Subscriber goalSub_;
|
||||
|
||||
@@ -0,0 +1,593 @@
|
||||
/*
|
||||
* MapsManager.cpp
|
||||
*
|
||||
* Created on: 2015-05-14
|
||||
* Author: mathieu
|
||||
*/
|
||||
|
||||
#include "MapsManager.h"
|
||||
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
#include <rtabmap/core/util3d_mapping.h>
|
||||
#include <rtabmap/core/util3d_filtering.h>
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
#include <rtabmap/core/util2d.h>
|
||||
#include <rtabmap/core/Memory.h>
|
||||
#include <rtabmap/core/Graph.h>
|
||||
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
#ifdef WITH_OCTOMAP
|
||||
#include <octomap/octomap.h>
|
||||
#endif
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
MapsManager::MapsManager() :
|
||||
cloudDecimation_(4),
|
||||
cloudMaxDepth_(4.0), // meters
|
||||
cloudVoxelSize_(0.05), // meters
|
||||
cloudOutputVoxelized_(false),
|
||||
projMaxGroundAngle_(45.0), // degrees
|
||||
projMinClusterSize_(20),
|
||||
projMaxHeight_(2.0), // meters
|
||||
gridCellSize_(0.05), // meters
|
||||
gridSize_(0), // meters
|
||||
gridEroded_(false),
|
||||
mapFilterRadius_(0.5),
|
||||
mapFilterAngle_(30.0), // degrees
|
||||
mapCacheCleanup_(true)
|
||||
{
|
||||
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
|
||||
// cloud map stuff
|
||||
pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
|
||||
pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
|
||||
pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
|
||||
pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_);
|
||||
|
||||
//projection map stuff
|
||||
pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_);
|
||||
pnh.param("proj_min_cluster_size", projMinClusterSize_, projMinClusterSize_);
|
||||
pnh.param("proj_max_height", projMaxHeight_, projMaxHeight_);
|
||||
|
||||
// common grid map stuff
|
||||
pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m
|
||||
pnh.param("grid_size", gridSize_, gridSize_); // m
|
||||
pnh.param("grid_eroded", gridEroded_, gridEroded_);
|
||||
|
||||
// common map stuff
|
||||
pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
|
||||
pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
|
||||
pnh.param("map_mapsManager_cleanup", mapCacheCleanup_, mapCacheCleanup_);
|
||||
|
||||
// mapping topics
|
||||
cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1);
|
||||
projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1);
|
||||
gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
|
||||
}
|
||||
|
||||
MapsManager::~MapsManager() {
|
||||
clear();
|
||||
}
|
||||
|
||||
void MapsManager::clear()
|
||||
{
|
||||
clouds_.clear();
|
||||
projMaps_.clear();
|
||||
gridMaps_.clear();
|
||||
}
|
||||
|
||||
std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Transform> & poses)
|
||||
{
|
||||
if(mapFilterRadius_ > 0.0)
|
||||
{
|
||||
// filter nodes
|
||||
double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
|
||||
return rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
|
||||
}
|
||||
return std::map<int, Transform>();
|
||||
}
|
||||
|
||||
std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
|
||||
const std::map<int, rtabmap::Transform> & poses,
|
||||
const rtabmap::Memory * memory,
|
||||
bool updateCloud,
|
||||
bool updateProj,
|
||||
bool updateGrid,
|
||||
const std::map<int, rtabmap::Signature> & signatures)
|
||||
{
|
||||
if(!updateCloud && !updateProj && !updateGrid)
|
||||
{
|
||||
// all false, udpate only those where we have subscribers
|
||||
updateCloud = cloudMapPub_.getNumSubscribers() != 0;
|
||||
updateProj = projMapPub_.getNumSubscribers() != 0;
|
||||
updateGrid = gridMapPub_.getNumSubscribers() != 0;
|
||||
}
|
||||
|
||||
UDEBUG("Updating map caches...");
|
||||
|
||||
if(!memory && signatures.size() == 0)
|
||||
{
|
||||
ROS_FATAL("Memory should not be null!?");
|
||||
return std::map<int, rtabmap::Transform>();
|
||||
}
|
||||
|
||||
std::map<int, rtabmap::Transform> filteredPoses;
|
||||
|
||||
// update cache
|
||||
if(updateCloud || updateProj || updateGrid)
|
||||
{
|
||||
// filter nodes
|
||||
if(mapFilterRadius_ > 0.0)
|
||||
{
|
||||
double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
|
||||
filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
|
||||
}
|
||||
else
|
||||
{
|
||||
filteredPoses = poses;
|
||||
}
|
||||
|
||||
|
||||
for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
|
||||
{
|
||||
if(!iter->second.isNull())
|
||||
{
|
||||
rtabmap::Signature data;
|
||||
bool rgbDepthRequired = updateCloud && !uContains(clouds_, iter->first);
|
||||
bool depthRequired = updateProj && !uContains(projMaps_, iter->first);
|
||||
bool scanRequired = updateGrid && !uContains(gridMaps_, iter->first);
|
||||
if(rgbDepthRequired ||
|
||||
depthRequired ||
|
||||
scanRequired)
|
||||
{
|
||||
if(signatures.size())
|
||||
{
|
||||
std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
|
||||
if(findIter != signatures.end())
|
||||
{
|
||||
data = findIter->second;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
data = memory->getSignatureDataConst(iter->first);
|
||||
}
|
||||
}
|
||||
|
||||
if(data.id() > 0)
|
||||
{
|
||||
rtabmap::Transform localTransform = data.getLocalTransform();
|
||||
if(!localTransform.isNull())
|
||||
{
|
||||
// Which data should we decompress?
|
||||
cv::Mat image, depth, scan;
|
||||
data.uncompressDataConst(rgbDepthRequired?&image:0, rgbDepthRequired||depthRequired?&depth:0, scanRequired?&scan:0);
|
||||
if(!depth.empty() &&
|
||||
depth.type() == CV_8UC1 &&
|
||||
image.empty() &&
|
||||
!rgbDepthRequired)
|
||||
{
|
||||
// Stereo detected, we should uncompress left image too
|
||||
data.uncompressDataConst(&image, 0, 0);
|
||||
}
|
||||
float fx = data.getFx();
|
||||
float fy = data.getFy();
|
||||
float cx = data.getCx();
|
||||
float cy = data.getCy();
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
|
||||
if(rgbDepthRequired)
|
||||
{
|
||||
if(!image.empty() &&
|
||||
!depth.empty() &&
|
||||
fx > 0.0f && fy > 0.0f &&
|
||||
cx >= 0.0f && cy >= 0.0f)
|
||||
{
|
||||
if(depth.type() == CV_8UC1)
|
||||
{
|
||||
cloudRGB = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloudDecimation_);
|
||||
}
|
||||
else
|
||||
{
|
||||
cloudRGB = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloudDecimation_);
|
||||
}
|
||||
|
||||
if(cloudRGB->size() && cloudMaxDepth_ > 0)
|
||||
{
|
||||
cloudRGB = util3d::passThrough(cloudRGB, "z", 0, cloudMaxDepth_);
|
||||
}
|
||||
if(cloudRGB->size() && cloudVoxelSize_ > 0)
|
||||
{
|
||||
cloudRGB = util3d::voxelize(cloudRGB, cloudVoxelSize_);
|
||||
}
|
||||
if(cloudRGB->size())
|
||||
{
|
||||
cloudRGB = util3d::transformPointCloud(cloudRGB, localTransform);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first);
|
||||
}
|
||||
}
|
||||
else if(depthRequired)
|
||||
{
|
||||
if( !depth.empty() &&
|
||||
fx > 0.0f && fy > 0.0f &&
|
||||
cx >= 0.0f && cy >= 0.0f)
|
||||
{
|
||||
if(depth.type() == CV_8UC1)
|
||||
{
|
||||
if(!image.empty())
|
||||
{
|
||||
cv::Mat leftMono;
|
||||
if(image.channels() == 3)
|
||||
{
|
||||
cv::cvtColor(image, leftMono, CV_BGR2GRAY);
|
||||
}
|
||||
else
|
||||
{
|
||||
leftMono = image;
|
||||
}
|
||||
cloudXYZ = rtabmap::util3d::cloudFromDisparity(
|
||||
util2d::disparityFromStereoImages(leftMono, depth),
|
||||
cx, cy,
|
||||
fx, fy,
|
||||
cloudDecimation_);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
cloudXYZ = util3d::cloudFromDepth(depth, cx, cy, fx, fy, cloudDecimation_);
|
||||
}
|
||||
|
||||
if(cloudXYZ.get())
|
||||
{
|
||||
if(cloudXYZ->size() && cloudMaxDepth_ > 0)
|
||||
{
|
||||
cloudXYZ = util3d::passThrough(cloudXYZ, "z", 0, cloudMaxDepth_);
|
||||
}
|
||||
if(cloudXYZ->size() && gridCellSize_ > 0)
|
||||
{
|
||||
// use gridCellSize since this cloud is only for the projection map
|
||||
cloudXYZ = util3d::voxelize(cloudXYZ, gridCellSize_);
|
||||
}
|
||||
if(cloudXYZ->size())
|
||||
{
|
||||
cloudXYZ = util3d::transformPointCloud(cloudXYZ, localTransform);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Left stereo image was empty! (node=%d)", iter->first);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first);
|
||||
}
|
||||
}
|
||||
|
||||
if(cloudRGB.get())
|
||||
{
|
||||
clouds_.insert(std::make_pair(iter->first, cloudRGB));
|
||||
}
|
||||
|
||||
if(depthRequired)
|
||||
{
|
||||
cv::Mat ground, obstacles;
|
||||
if(cloudRGB.get())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloudRGB;
|
||||
if(cloudClipped->size() && projMaxHeight_ > 0)
|
||||
{
|
||||
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
|
||||
}
|
||||
if(cloudClipped->size())
|
||||
{
|
||||
cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_);
|
||||
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
|
||||
}
|
||||
}
|
||||
else if(cloudXYZ.get())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudClipped = cloudXYZ;
|
||||
if(cloudClipped->size() && projMaxHeight_ > 0)
|
||||
{
|
||||
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
|
||||
}
|
||||
if(cloudClipped->size())
|
||||
{
|
||||
util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
|
||||
}
|
||||
}
|
||||
projMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
|
||||
}
|
||||
|
||||
if(scanRequired)
|
||||
{
|
||||
cv::Mat ground, obstacles;
|
||||
util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_);
|
||||
gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Local transform detected for node %d", iter->first);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Pose null for node %d", iter->first);
|
||||
}
|
||||
}
|
||||
|
||||
// cleanup not used nodes
|
||||
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=clouds_.begin();
|
||||
iter!=clouds_.end();)
|
||||
{
|
||||
if(!uContains(poses, iter->first))
|
||||
{
|
||||
clouds_.erase(iter++);
|
||||
}
|
||||
else
|
||||
{
|
||||
++iter;
|
||||
}
|
||||
}
|
||||
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=projMaps_.begin();
|
||||
iter!=projMaps_.end();)
|
||||
{
|
||||
if(!uContains(poses, iter->first))
|
||||
{
|
||||
projMaps_.erase(iter++);
|
||||
}
|
||||
else
|
||||
{
|
||||
++iter;
|
||||
}
|
||||
}
|
||||
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=gridMaps_.begin();
|
||||
iter!=gridMaps_.end();)
|
||||
{
|
||||
if(!uContains(poses, iter->first))
|
||||
{
|
||||
gridMaps_.erase(iter++);
|
||||
}
|
||||
else
|
||||
{
|
||||
++iter;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return filteredPoses;
|
||||
}
|
||||
|
||||
void MapsManager::publishMaps(
|
||||
const std::map<int, rtabmap::Transform> & poses,
|
||||
const ros::Time & stamp,
|
||||
const std::string & mapFrameId)
|
||||
{
|
||||
UDEBUG("Publishing maps...");
|
||||
|
||||
// publish maps
|
||||
if(cloudMapPub_.getNumSubscribers())
|
||||
{
|
||||
// generate the assembled cloud!
|
||||
UTimer time;
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
|
||||
int count = 0;
|
||||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first);
|
||||
if(jter != clouds_.end())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
|
||||
*assembledCloud+=*transformed;
|
||||
++count;
|
||||
}
|
||||
}
|
||||
|
||||
if(assembledCloud->size())
|
||||
{
|
||||
if(cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
|
||||
{
|
||||
assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_);
|
||||
}
|
||||
ROS_INFO("Assembled %d clouds (%fs)", count, time.ticks());
|
||||
|
||||
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
|
||||
pcl::toROSMsg(*assembledCloud, *cloudMsg);
|
||||
cloudMsg->header.stamp = stamp;
|
||||
cloudMsg->header.frame_id = mapFrameId;
|
||||
cloudMapPub_.publish(cloudMsg);
|
||||
}
|
||||
else if(poses.size())
|
||||
{
|
||||
ROS_WARN("Cloud map is empty! (clouds=%d)", (int)clouds_.size());
|
||||
}
|
||||
}
|
||||
else if(mapCacheCleanup_)
|
||||
{
|
||||
clouds_.clear();
|
||||
}
|
||||
|
||||
if(projMapPub_.getNumSubscribers())
|
||||
{
|
||||
// create the projection map
|
||||
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
|
||||
cv::Mat pixels = this->generateProjMap(poses, xMin, yMin, gridCellSize);
|
||||
|
||||
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;
|
||||
|
||||
projMapPub_.publish(map);
|
||||
}
|
||||
else if(poses.size())
|
||||
{
|
||||
ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size());
|
||||
}
|
||||
}
|
||||
else if(mapCacheCleanup_)
|
||||
{
|
||||
projMaps_.clear();
|
||||
}
|
||||
|
||||
if(gridMapPub_.getNumSubscribers())
|
||||
{
|
||||
// create the grid map
|
||||
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
|
||||
cv::Mat pixels = this->generateGridMap(poses, xMin, yMin, gridCellSize);
|
||||
|
||||
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;
|
||||
|
||||
gridMapPub_.publish(map);
|
||||
}
|
||||
else if(poses.size())
|
||||
{
|
||||
ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size());
|
||||
}
|
||||
}
|
||||
else if(mapCacheCleanup_)
|
||||
{
|
||||
gridMaps_.clear();
|
||||
}
|
||||
}
|
||||
|
||||
cv::Mat MapsManager::generateProjMap(
|
||||
const std::map<int, rtabmap::Transform> & poses,
|
||||
float & xMin,
|
||||
float & yMin,
|
||||
float & gridCellSize)
|
||||
{
|
||||
gridCellSize = gridCellSize_;
|
||||
return util3d::create2DMapFromOccupancyLocalMaps(
|
||||
poses,
|
||||
projMaps_,
|
||||
gridCellSize_,
|
||||
xMin, yMin,
|
||||
gridSize_,
|
||||
gridEroded_);
|
||||
}
|
||||
|
||||
cv::Mat MapsManager::generateGridMap(
|
||||
const std::map<int, rtabmap::Transform> & poses,
|
||||
float & xMin,
|
||||
float & yMin,
|
||||
float & gridCellSize)
|
||||
{
|
||||
gridCellSize = gridCellSize_;
|
||||
return util3d::create2DMapFromOccupancyLocalMaps(
|
||||
poses,
|
||||
gridMaps_,
|
||||
gridCellSize_,
|
||||
xMin, yMin,
|
||||
gridSize_,
|
||||
gridEroded_);
|
||||
}
|
||||
|
||||
#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();
|
||||
}
|
||||
return octree;
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,90 @@
|
||||
/*
|
||||
* MapsManager.h
|
||||
*
|
||||
* Created on: 2015-05-14
|
||||
* Author: mathieu
|
||||
*/
|
||||
|
||||
#ifndef MAPSMANAGER_H_
|
||||
#define MAPSMANAGER_H_
|
||||
|
||||
#include <rtabmap/core/Signature.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <ros/time.h>
|
||||
#include <ros/publisher.h>
|
||||
|
||||
namespace octomap{
|
||||
class OcTree;
|
||||
}
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class Memory;
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
class MapsManager {
|
||||
public:
|
||||
MapsManager();
|
||||
virtual ~MapsManager();
|
||||
void clear();
|
||||
|
||||
std::map<int, rtabmap::Transform> getFilteredPoses(
|
||||
const std::map<int, rtabmap::Transform> & poses);
|
||||
|
||||
std::map<int, rtabmap::Transform> updateMapCaches(
|
||||
const std::map<int, rtabmap::Transform> & poses,
|
||||
const rtabmap::Memory * memory,
|
||||
bool updateCloud,
|
||||
bool updateProj,
|
||||
bool updateGrid,
|
||||
const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
|
||||
|
||||
void publishMaps(
|
||||
const std::map<int, rtabmap::Transform> & poses,
|
||||
const ros::Time & stamp,
|
||||
const std::string & mapFrameId);
|
||||
|
||||
cv::Mat generateProjMap(
|
||||
const std::map<int, rtabmap::Transform> & filteredPoses,
|
||||
float & xMin,
|
||||
float & yMin,
|
||||
float & gridCellSize);
|
||||
|
||||
cv::Mat generateGridMap(
|
||||
const std::map<int, rtabmap::Transform> & filteredPoses,
|
||||
float & xMin,
|
||||
float & yMin,
|
||||
float & gridCellSize);
|
||||
|
||||
#ifdef WITH_OCTOMAP
|
||||
octomap::OcTree * createOctomap(const std::map<int, rtabmap::Transform> & poses);
|
||||
#endif
|
||||
|
||||
private:
|
||||
// mapping stuff
|
||||
int cloudDecimation_;
|
||||
double cloudMaxDepth_;
|
||||
double cloudVoxelSize_;
|
||||
bool cloudOutputVoxelized_;
|
||||
double projMaxGroundAngle_;
|
||||
int projMinClusterSize_;
|
||||
double projMaxHeight_;
|
||||
double gridCellSize_;
|
||||
double gridSize_;
|
||||
bool gridEroded_;
|
||||
double mapFilterRadius_;
|
||||
double mapFilterAngle_;
|
||||
bool mapCacheCleanup_;
|
||||
|
||||
ros::Publisher cloudMapPub_;
|
||||
ros::Publisher projMapPub_;
|
||||
ros::Publisher gridMapPub_;
|
||||
|
||||
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
|
||||
std::map<int, std::pair<cv::Mat, cv::Mat> > projMaps_; // <ground, obstacles>
|
||||
std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <ground, obstacles>
|
||||
};
|
||||
|
||||
#endif /* MAPSMANAGER_H_ */
|
||||
Reference in New Issue
Block a user