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

fixed #447

Browse Source
This commit is contained in:
matlabbe
2020-08-02 12:54:24 -04:00
parent ae854dfb4c
commit 57dc95c9d7
1 changed files with 18 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+18 -2
View File
@@ -2792,6 +2792,10 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::
bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
{
// Make sure grid map cache is up to date (in case there is no subscriber on map topics)
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false);
// create the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
@@ -2820,11 +2824,19 @@ bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetM
res.map.header.stamp = ros::Time::now();
return true;
}
else
{
NODELET_WARN("rtabmap: The map is empty!");
}
return false;
}
bool CoreWrapper::getProbMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
{
// Make sure grid map cache is up to date (in case there is no subscriber on map topics)
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false);
// create the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = mapsManager_.getGridProbMap(xMin, yMin, gridCellSize);
@@ -2853,6 +2865,10 @@ bool CoreWrapper::getProbMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::
res.map.header.stamp = ros::Time::now();
return true;
}
else
{
NODELET_WARN("rtabmap: The map is empty!");
}
return false;
}
@@ -3812,7 +3828,7 @@ bool CoreWrapper::octomapBinaryCallback(
poses = nearestPoses;
}
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
@@ -3843,7 +3859,7 @@ bool CoreWrapper::octomapFullCallback(
poses = nearestPoses;
}
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);