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

0.17.0: Occupancy grid is automatically saved in database (#213). By default on localization, rtabmap will adjust the odometry to match the latest saved localization in the database (starting automatically from where the robot shut down, #220).

Browse Source
This commit is contained in:
matlabbe
2018-04-04 19:18:27 -04:00
parent dc29b74111
commit 75f1652194
5 changed files with 63 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+51 -47
View File
@@ -346,6 +346,13 @@ void CoreWrapper::onInit()
Parameters::kGridFromDepth().c_str());
parameters_.insert(ParametersPair(Parameters::kGridFromDepth(), "false"));
}
if((subscribeScan2d || subscribeScan3d) && parameters_.find(Parameters::kGridRangeMax()) == parameters_.end())
{
NODELET_INFO("Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan\" or \"subscribe_scan_cloud\" is true.",
Parameters::kGridRangeMax().c_str(),
Parameters::defaultGridRangeMax());
parameters_.insert(ParametersPair(Parameters::kGridRangeMax(), "0"));
}
int regStrategy = Parameters::defaultRegStrategy();
Parameters::parse(parameters_, Parameters::kRegStrategy(), regStrategy);
if(subscribeScan2d &&
@@ -465,6 +472,16 @@ void CoreWrapper::onInit()
// Init RTAB-Map
rtabmap_.init(parameters_, databasePath_);
if(rtabmap_.getMemory())
{
float xMin, yMin, gridCellSize;
cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
if(!map.empty())
{
mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses());
}
}
if(databasePath_.size() && rtabmap_.getMemory())
{
NODELET_INFO("rtabmap: Database version = \"%s\".", rtabmap_.getMemory()->getDatabaseVersion().c_str());
@@ -623,6 +640,17 @@ CoreWrapper::~CoreWrapper()
nh.deleteParam("is_rtabmap_paused");
printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str());
if(rtabmap_.getMemory())
{
// save the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
if(!pixels.empty())
{
rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize);
}
}
rtabmap_.close();
printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
}
@@ -2031,57 +2059,33 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::
bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
{
std::map<int, rtabmap::Transform> filteredPoses = rtabmap_.getLocalOptimizedPoses();
if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
// create the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
if(!pixels.empty())
{
std::map<int, Transform> nearestPoses;
std::vector<int> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_);
for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
if(pter != filteredPoses.end())
{
nearestPoses.insert(*pter);
}
}
filteredPoses = nearestPoses;
}
//init
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;
res.map.info.origin.orientation.x = 0.0;
res.map.info.origin.orientation.y = 0.0;
res.map.info.origin.orientation.z = 0.0;
res.map.info.origin.orientation.w = 1.0;
filteredPoses = mapsManager_.updateMapCaches(
filteredPoses,
rtabmap_.getMemory(),
true,
false);
if(filteredPoses.size())
{
// create the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = mapsManager_.getGridMap(filteredPoses, xMin, yMin, gridCellSize);
res.map.info.width = pixels.cols;
res.map.info.height = pixels.rows;
res.map.info.origin.position.x = xMin;
res.map.info.origin.position.y = yMin;
res.map.data.resize(res.map.info.width * res.map.info.height);
if(!pixels.empty())
{
//init
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;
res.map.info.origin.orientation.x = 0.0;
res.map.info.origin.orientation.y = 0.0;
res.map.info.origin.orientation.z = 0.0;
res.map.info.origin.orientation.w = 1.0;
memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
res.map.info.width = pixels.cols;
res.map.info.height = pixels.rows;
res.map.info.origin.position.x = xMin;
res.map.info.origin.position.y = yMin;
res.map.data.resize(res.map.info.width * res.map.info.height);
memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
res.map.header.frame_id = mapFrameId_;
res.map.header.stamp = ros::Time::now();
return true;
}
res.map.header.frame_id = mapFrameId_;
res.map.header.stamp = ros::Time::now();
return true;
}
return false;
}
src/MapsManager.cpp
+6 -2
View File
@@ -290,6 +290,11 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
#endif
}
void MapsManager::set2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, rtabmap::Transform> & poses)
{
occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses);
}
void MapsManager::clear()
{
gridMaps_.clear();
@@ -1195,7 +1200,7 @@ void MapsManager::publishMaps(
// create the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = this->getGridMap(poses, xMin, yMin, gridCellSize);
cv::Mat pixels = this->getGridMap(xMin, yMin, gridCellSize);
if(!pixels.empty())
{
@@ -1244,7 +1249,6 @@ void MapsManager::publishMaps(
}
cv::Mat MapsManager::getGridMap(
const std::map<int, rtabmap::Transform> & poses,
float & xMin,
float & yMin,
float & gridCellSize)