mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +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)
|
add_definitions(-DWITH_OCTOMAP)
|
||||||
ENDIF(octomap_ros_FOUND)
|
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)
|
add_dependencies(rtabmap rtabmap_generate_messages_cpp)
|
||||||
target_link_libraries(rtabmap rtabmap_ros ${Libraries})
|
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 <sensor_msgs/image_encodings.h>
|
||||||
#include <cv_bridge/cv_bridge.h>
|
#include <cv_bridge/cv_bridge.h>
|
||||||
#include <nav_msgs/Path.h>
|
#include <nav_msgs/Path.h>
|
||||||
#include <nav_msgs/OccupancyGrid.h>
|
|
||||||
#include <std_msgs/Int32MultiArray.h>
|
#include <std_msgs/Int32MultiArray.h>
|
||||||
#include <std_msgs/Bool.h>
|
#include <std_msgs/Bool.h>
|
||||||
|
|
||||||
#include <visualization_msgs/MarkerArray.h>
|
#include <visualization_msgs/MarkerArray.h>
|
||||||
|
|
||||||
#include <rtabmap/utilite/ULogger.h>
|
|
||||||
#include <rtabmap/utilite/UTimer.h>
|
#include <rtabmap/utilite/UTimer.h>
|
||||||
#include <rtabmap/utilite/UStl.h>
|
#include <rtabmap/utilite/UDirectory.h>
|
||||||
#include <rtabmap/utilite/UConversion.h>
|
|
||||||
#include <rtabmap/utilite/UFile.h>
|
#include <rtabmap/utilite/UFile.h>
|
||||||
#include <rtabmap/core/util3d_mapping.h>
|
#include <rtabmap/utilite/UConversion.h>
|
||||||
#include <rtabmap/core/util3d_filtering.h>
|
#include <rtabmap/utilite/UStl.h>
|
||||||
#include <rtabmap/core/util3d_transforms.h>
|
|
||||||
#include <rtabmap/core/util3d_conversions.h>
|
#include <rtabmap/core/util3d_conversions.h>
|
||||||
#include <rtabmap/core/util2d.h>
|
#include <rtabmap/core/util3d_transforms.h>
|
||||||
#include <rtabmap/core/Graph.h>
|
|
||||||
#include <rtabmap/core/Memory.h>
|
#include <rtabmap/core/Memory.h>
|
||||||
|
|
||||||
#include <pcl_conversions/pcl_conversions.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>
|
#include <image_geometry/stereo_camera_model.h>
|
||||||
|
|
||||||
#ifdef WITH_OCTOMAP
|
#ifdef WITH_OCTOMAP
|
||||||
#include <octomap/octomap.h>
|
|
||||||
#include <octomap_ros/conversions.h>
|
|
||||||
#include <octomap_msgs/Octomap.h>
|
|
||||||
#include <octomap_msgs/conversions.h>
|
#include <octomap_msgs/conversions.h>
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
@@ -94,19 +87,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
|||||||
databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
|
databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
|
||||||
waitForTransform_(false),
|
waitForTransform_(false),
|
||||||
useActionForGoal_(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()),
|
mapToOdom_(rtabmap::Transform::getIdentity()),
|
||||||
depthSync_(0),
|
depthSync_(0),
|
||||||
depthScanSync_(0),
|
depthScanSync_(0),
|
||||||
@@ -166,27 +146,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
|||||||
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
|
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
|
||||||
pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
|
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());
|
ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str());
|
||||||
if(!odomFrameId_.empty())
|
if(!odomFrameId_.empty())
|
||||||
{
|
{
|
||||||
@@ -201,11 +160,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
|||||||
mapGraphPub_ = nh.advertise<rtabmap_ros::Graph>("graph", 1);
|
mapGraphPub_ = nh.advertise<rtabmap_ros::Graph>("graph", 1);
|
||||||
labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 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
|
// planning topics
|
||||||
goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
|
goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
|
||||||
goalGlobalSub_ = nh.subscribe("goal_global", 1, &CoreWrapper::goalGlobalCallback, this);
|
goalGlobalSub_ = nh.subscribe("goal_global", 1, &CoreWrapper::goalGlobalCallback, this);
|
||||||
@@ -1130,30 +1084,13 @@ void CoreWrapper::process(
|
|||||||
this->publishStats(stamp);
|
this->publishStats(stamp);
|
||||||
std::map<int, rtabmap::Transform> filteredPoses;
|
std::map<int, rtabmap::Transform> filteredPoses;
|
||||||
|
|
||||||
filteredPoses = this->updateMapCaches(rtabmap_.getLocalOptimizedPoses(),
|
filteredPoses = mapsManager_.updateMapCaches(
|
||||||
cloudMapPub_.getNumSubscribers() != 0,
|
rtabmap_.getLocalOptimizedPoses(),
|
||||||
projMapPub_.getNumSubscribers() != 0,
|
rtabmap_.getMemory(),
|
||||||
gridMapPub_.getNumSubscribers() != 0);
|
false,
|
||||||
this->publishMaps(filteredPoses, stamp);
|
false,
|
||||||
|
false);
|
||||||
// clear memory if no one subscribed
|
mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_);
|
||||||
if(mapCacheCleanup_)
|
|
||||||
{
|
|
||||||
if(cloudMapPub_.getNumSubscribers() == 0)
|
|
||||||
{
|
|
||||||
clouds_.clear();
|
|
||||||
}
|
|
||||||
|
|
||||||
if(projMapPub_.getNumSubscribers() == 0)
|
|
||||||
{
|
|
||||||
projMaps_.clear();
|
|
||||||
}
|
|
||||||
|
|
||||||
if(gridMapPub_.getNumSubscribers() == 0)
|
|
||||||
{
|
|
||||||
gridMaps_.clear();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// update goal if planning is enabled
|
// update goal if planning is enabled
|
||||||
if(!currentMetricGoal_.isNull())
|
if(!currentMetricGoal_.isNull())
|
||||||
@@ -1391,9 +1328,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
|
|||||||
lastPose_.setIdentity();
|
lastPose_.setIdentity();
|
||||||
currentMetricGoal_.setNull();
|
currentMetricGoal_.setNull();
|
||||||
latestNodeWasReached_ = false;
|
latestNodeWasReached_ = false;
|
||||||
clouds_.clear();
|
mapsManager_.clear();
|
||||||
projMaps_.clear();
|
|
||||||
gridMaps_.clear();
|
|
||||||
return true;
|
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)
|
bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
|
||||||
{
|
{
|
||||||
std::map<int, rtabmap::Transform> filteredPoses;
|
std::map<int, rtabmap::Transform> filteredPoses;
|
||||||
filteredPoses = this->updateMapCaches(rtabmap_.getLocalOptimizedPoses(), false, true, false);
|
filteredPoses = mapsManager_.updateMapCaches(
|
||||||
if(filteredPoses.size() && projMaps_.size())
|
rtabmap_.getLocalOptimizedPoses(),
|
||||||
|
rtabmap_.getMemory(),
|
||||||
|
false,
|
||||||
|
true,
|
||||||
|
false);
|
||||||
|
if(filteredPoses.size())
|
||||||
{
|
{
|
||||||
// create the projection map
|
// create the projection map
|
||||||
float xMin=0.0f, yMin=0.0f;
|
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
|
||||||
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
|
cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize);
|
||||||
filteredPoses,
|
|
||||||
projMaps_,
|
|
||||||
gridCellSize_,
|
|
||||||
xMin, yMin,
|
|
||||||
gridSize_,
|
|
||||||
gridEroded_);
|
|
||||||
|
|
||||||
// clear memory if no one subscribed
|
|
||||||
if(mapCacheCleanup_ && projMapPub_.getNumSubscribers() == 0)
|
|
||||||
{
|
|
||||||
projMaps_.clear();
|
|
||||||
}
|
|
||||||
|
|
||||||
if(!pixels.empty())
|
if(!pixels.empty())
|
||||||
{
|
{
|
||||||
//init
|
//init
|
||||||
res.map.info.resolution = gridCellSize_;
|
res.map.info.resolution = gridCellSize;
|
||||||
res.map.info.origin.position.x = 0.0;
|
res.map.info.origin.position.x = 0.0;
|
||||||
res.map.info.origin.position.y = 0.0;
|
res.map.info.origin.position.y = 0.0;
|
||||||
res.map.info.origin.position.z = 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)
|
bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
|
||||||
{
|
{
|
||||||
std::map<int, rtabmap::Transform> filteredPoses;
|
std::map<int, rtabmap::Transform> filteredPoses;
|
||||||
filteredPoses = this->updateMapCaches(rtabmap_.getLocalOptimizedPoses(), false, false, true);
|
filteredPoses = mapsManager_.updateMapCaches(
|
||||||
if(filteredPoses.size() && gridMaps_.size())
|
rtabmap_.getLocalOptimizedPoses(),
|
||||||
|
rtabmap_.getMemory(),
|
||||||
|
false,
|
||||||
|
false,
|
||||||
|
true);
|
||||||
|
if(filteredPoses.size())
|
||||||
{
|
{
|
||||||
// create the projection map
|
// create the grid map
|
||||||
float xMin=0.0f, yMin=0.0f;
|
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
|
||||||
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
|
cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize);
|
||||||
filteredPoses,
|
|
||||||
gridMaps_,
|
|
||||||
gridCellSize_,
|
|
||||||
xMin, yMin,
|
|
||||||
gridSize_,
|
|
||||||
gridEroded_);
|
|
||||||
|
|
||||||
// clear memory if no one subscribed
|
|
||||||
if(mapCacheCleanup_ && gridMapPub_.getNumSubscribers() == 0)
|
|
||||||
{
|
|
||||||
gridMaps_.clear();
|
|
||||||
}
|
|
||||||
|
|
||||||
if(!pixels.empty())
|
if(!pixels.empty())
|
||||||
{
|
{
|
||||||
//init
|
//init
|
||||||
res.map.info.resolution = gridCellSize_;
|
res.map.info.resolution = gridCellSize;
|
||||||
res.map.info.origin.position.x = 0.0;
|
res.map.info.origin.position.x = 0.0;
|
||||||
res.map.info.origin.position.y = 0.0;
|
res.map.info.origin.position.y = 0.0;
|
||||||
res.map.info.origin.position.z = 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() ||
|
if(mapDataPub_.getNumSubscribers() ||
|
||||||
mapGraphPub_.getNumSubscribers() ||
|
mapGraphPub_.getNumSubscribers() ||
|
||||||
(!req.graphOnly && (cloudMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers() || gridMapPub_.getNumSubscribers())))
|
!req.graphOnly)
|
||||||
{
|
{
|
||||||
std::map<int, Signature> signatures;
|
std::map<int, Signature> signatures;
|
||||||
std::map<int, Transform> poses;
|
std::map<int, Transform> poses;
|
||||||
@@ -1739,42 +1660,19 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
|
|||||||
std::map<int, Transform> filteredPoses;
|
std::map<int, Transform> filteredPoses;
|
||||||
if(signatures.size())
|
if(signatures.size())
|
||||||
{
|
{
|
||||||
filteredPoses = this->updateMapCaches(poses,
|
filteredPoses = mapsManager_.updateMapCaches(
|
||||||
cloudMapPub_.getNumSubscribers() != 0,
|
poses,
|
||||||
projMapPub_.getNumSubscribers() != 0,
|
rtabmap_.getMemory(),
|
||||||
gridMapPub_.getNumSubscribers() != 0,
|
true,
|
||||||
signatures);
|
true,
|
||||||
}
|
true,
|
||||||
else if(mapFilterRadius_ > 0.0)
|
signatures);
|
||||||
{
|
|
||||||
// filter nodes
|
|
||||||
double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
|
|
||||||
filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
filteredPoses = poses;
|
filteredPoses = mapsManager_.getFilteredPoses(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();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
mapsManager_.publishMaps(filteredPoses, now, mapFrameId_);
|
||||||
}
|
}
|
||||||
|
|
||||||
if(labelsPub_.getNumSubscribers())
|
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)
|
void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
|
||||||
{
|
{
|
||||||
if(!currentMetricGoal_.isNull())
|
if(!currentMetricGoal_.isNull())
|
||||||
@@ -2605,59 +2100,6 @@ void CoreWrapper::publishLocalPath(const ros::Time & stamp)
|
|||||||
}
|
}
|
||||||
|
|
||||||
#ifdef WITH_OCTOMAP
|
#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(
|
bool CoreWrapper::octomapBinaryCallback(
|
||||||
octomap_msgs::GetOctomap::Request &req,
|
octomap_msgs::GetOctomap::Request &req,
|
||||||
octomap_msgs::GetOctomap::Response &res)
|
octomap_msgs::GetOctomap::Response &res)
|
||||||
@@ -2666,7 +2108,10 @@ bool CoreWrapper::octomapBinaryCallback(
|
|||||||
res.map.header.frame_id = mapFrameId_;
|
res.map.header.frame_id = mapFrameId_;
|
||||||
res.map.header.stamp = ros::Time::now();
|
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);
|
bool success = octree != 0 && octree->size() && octomap_msgs::binaryMapToMsg(*octree, res.map);
|
||||||
if(octree)
|
if(octree)
|
||||||
{
|
{
|
||||||
@@ -2683,7 +2128,10 @@ bool CoreWrapper::octomapFullCallback(
|
|||||||
res.map.header.frame_id = mapFrameId_;
|
res.map.header.frame_id = mapFrameId_;
|
||||||
res.map.header.stamp = ros::Time::now();
|
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);
|
bool success = octree != 0 && octree->size() && octomap_msgs::fullMapToMsg(*octree, res.map);
|
||||||
if(octree)
|
if(octree)
|
||||||
{
|
{
|
||||||
|
|||||||
+3
-42
@@ -36,8 +36,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|||||||
#include <tf/transform_listener.h>
|
#include <tf/transform_listener.h>
|
||||||
#include <tf2_ros/transform_broadcaster.h>
|
#include <tf2_ros/transform_broadcaster.h>
|
||||||
|
|
||||||
#include <cv_bridge/cv_bridge.h>
|
|
||||||
|
|
||||||
#include <std_msgs/Empty.h>
|
#include <std_msgs/Empty.h>
|
||||||
#include <std_msgs/Int32.h>
|
#include <std_msgs/Int32.h>
|
||||||
#include <sensor_msgs/PointCloud2.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/Odometry.h>
|
||||||
#include <nav_msgs/GetMap.h>
|
#include <nav_msgs/GetMap.h>
|
||||||
|
|
||||||
#include <rtabmap/core/Statistics.h>
|
|
||||||
#include <rtabmap/core/Parameters.h>
|
#include <rtabmap/core/Parameters.h>
|
||||||
#include <rtabmap/core/Rtabmap.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/SetGoal.h"
|
||||||
#include "rtabmap_ros/SetLabel.h"
|
#include "rtabmap_ros/SetLabel.h"
|
||||||
|
|
||||||
|
#include "MapsManager.h"
|
||||||
|
|
||||||
#include <message_filters/subscriber.h>
|
#include <message_filters/subscriber.h>
|
||||||
#include <message_filters/synchronizer.h>
|
#include <message_filters/synchronizer.h>
|
||||||
#include <message_filters/sync_policies/approximate_time.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/image_transport.h>
|
||||||
#include <image_transport/subscriber_filter.h>
|
#include <image_transport/subscriber_filter.h>
|
||||||
|
|
||||||
#include <pcl/point_types.h>
|
|
||||||
#include <pcl/point_cloud.h>
|
|
||||||
|
|
||||||
#ifdef WITH_OCTOMAP
|
#ifdef WITH_OCTOMAP
|
||||||
#include <octomap_msgs/GetOctomap.h>
|
#include <octomap_msgs/GetOctomap.h>
|
||||||
#endif
|
#endif
|
||||||
@@ -80,10 +76,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|||||||
#include <actionlib_msgs/GoalStatusArray.h>
|
#include <actionlib_msgs/GoalStatusArray.h>
|
||||||
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
|
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
|
||||||
|
|
||||||
namespace octomap{
|
|
||||||
class OcTree;
|
|
||||||
}
|
|
||||||
|
|
||||||
class CoreWrapper
|
class CoreWrapper
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
@@ -207,24 +199,13 @@ private:
|
|||||||
|
|
||||||
void publishLoop(double tfDelay);
|
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 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 publishCurrentGoal(const ros::Time & stamp);
|
||||||
void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
|
void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
|
||||||
void goalActiveCb();
|
void goalActiveCb();
|
||||||
void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
|
void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
|
||||||
void publishLocalPath(const ros::Time & stamp);
|
void publishLocalPath(const ros::Time & stamp);
|
||||||
|
|
||||||
#ifdef WITH_OCTOMAP
|
|
||||||
octomap::OcTree * createOctomap();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
rtabmap::Rtabmap rtabmap_;
|
rtabmap::Rtabmap rtabmap_;
|
||||||
bool paused_;
|
bool paused_;
|
||||||
@@ -244,35 +225,15 @@ private:
|
|||||||
bool waitForTransform_;
|
bool waitForTransform_;
|
||||||
bool useActionForGoal_;
|
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_;
|
rtabmap::Transform mapToOdom_;
|
||||||
boost::mutex mapToOdomMutex_;
|
boost::mutex mapToOdomMutex_;
|
||||||
|
|
||||||
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
|
MapsManager mapsManager_;
|
||||||
std::map<int, std::pair<cv::Mat, cv::Mat> > projMaps_; // <ground, obstacles>
|
|
||||||
std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <ground, obstacles>
|
|
||||||
|
|
||||||
ros::Publisher infoPub_;
|
ros::Publisher infoPub_;
|
||||||
ros::Publisher mapDataPub_;
|
ros::Publisher mapDataPub_;
|
||||||
ros::Publisher mapGraphPub_;
|
ros::Publisher mapGraphPub_;
|
||||||
ros::Publisher labelsPub_;
|
ros::Publisher labelsPub_;
|
||||||
ros::Publisher cloudMapPub_;
|
|
||||||
ros::Publisher projMapPub_;
|
|
||||||
ros::Publisher gridMapPub_;
|
|
||||||
|
|
||||||
//Planning stuff
|
//Planning stuff
|
||||||
ros::Subscriber goalSub_;
|
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