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

Using Dijkstra for global planning for a significative performance boost (no need to optimize the graph before computing the path)

Browse Source
This commit is contained in:
matlabbe
2015-06-21 18:29:12 -04:00
parent bef408d4b9
commit a5efee20bc
11 changed files with 441 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/DBDriver.cpp
+27
View File
@@ -556,6 +556,33 @@ void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren) const
_dbSafeAccessMutex.unlock();
}
void DBDriver::getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks) const
{
_dbSafeAccessMutex.lock();
this->getAllLinksQuery(links, ignoreNullLinks);
_dbSafeAccessMutex.unlock();
// look in the trash
_trashesMutex.lock();
if(_trashSignatures.size())
{
for(std::map<int, Signature*>::const_iterator iter=_trashSignatures.begin(); iter!=_trashSignatures.end(); ++iter)
{
links.erase(iter->first);
for(std::multimap<int, Link>::const_iterator jter=iter->second->getLinks().begin();
jter!=iter->second->getLinks().end();
++jter)
{
if(!ignoreNullLinks || jter->second.isValid())
{
links.insert(std::make_pair(iter->first, jter->second));
}
}
}
}
_trashesMutex.unlock();
}
void DBDriver::getLastNodeId(int & id) const
{
// look in the trash