i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Planning: abort plan when nearest pose on the path is not found, only keep the latest virtual link on the current path index

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-12 13:29:00 -04:00
parent c697d27d86
commit b54ff8547e
1 changed files with 22 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
corelib/src/Rtabmap.cpp
View File

@@ -3139,6 +3139,26 @@ void Rtabmap::updateGoalIndex()
_memory->removeVirtualLinks(_path[i].first); _memory->removeVirtualLinks(_path[i].first);
} }
} }
// for the current index, only keep the newest virtual link
UASSERT(_pathCurrentIndex < _path.size());
const Signature * currentIndexS = _memory->getSignature(_path[_pathCurrentIndex].first);
UASSERT(currentIndexS != 0);
std::map<int, Link> links = currentIndexS->getLinks(); // make a copy
bool latestVirtualLinkFound = false;
for(std::map<int, Link>::reverse_iterator iter=links.rbegin(); iter!=links.rend(); ++iter)
{
if(iter->second.type() == Link::kVirtualClosure)
{
if(latestVirtualLinkFound)
{
_memory->removeLink(currentIndexS->id(), iter->first);
}
else
{
latestVirtualLinkFound = true;
}
}
}
// Make sure the next signatures on the path are linked together // Make sure the next signatures on the path are linked together
float distanceSoFar = 0.0f; float distanceSoFar = 0.0f;
@@ -3263,7 +3283,8 @@ void Rtabmap::updateGoalIndex()
} }
if(distance < 0) if(distance < 0)
{ {
UERROR("The nearest pose on the path not found!"); UERROR("The nearest pose on the path not found! Aborting the plan...");
this->clearPath();
} }
else else
{ {