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

Rtabmap::computePath() added tolerance parameter to overwrite default RGBD/LocalRadius

Browse Source
This commit is contained in:
matlabbe
2018-07-09 15:11:19 -04:00
parent 41d5e11511
commit d41c15dbc7
2 changed files with 10 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -171,7 +171,7 @@ public:
int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
void clearPath(int status); // -1=failed 0=idle/executing 1=success
bool computePath(int targetNode, bool global);
bool computePath(const Transform & targetPose); // only in current optimized map
bool computePath(const Transform & targetPose, float tolerance = -1.0f); // only in current optimized map, tolerance (m) < 0 means RGBD/LocalRadius, 0 means infinite
const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
std::vector<std::pair<int, Transform> > getPathNextPoses() const;
std::vector<int> getPathNextNodes() const;

13
corelib/src/Rtabmap.cpp
View File

@@ -4124,8 +4124,13 @@ bool Rtabmap::computePath(int targetNode, bool global)
return false;
}
bool Rtabmap::computePath(const Transform & targetPose)
bool Rtabmap::computePath(const Transform & targetPose, float tolerance)
{
if(tolerance < 0.0f)
{
tolerance = _localRadius;
}
UINFO("Planning a path to pose %s ", targetPose.prettyPrint().c_str());
this->clearPath(0);
@@ -4178,7 +4183,7 @@ bool Rtabmap::computePath(const Transform & targetPose)
}
int nearestId;
if(!_lastLocalizationPose.isNull() && _lastLocalizationPose.getDistance(targetPose) < _localRadius)
if(!_lastLocalizationPose.isNull() && _lastLocalizationPose.getDistance(targetPose) < tolerance)
{
// target can be reached from the current node
nearestId = currentNode;
@@ -4190,10 +4195,10 @@ bool Rtabmap::computePath(const Transform & targetPose)
UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks());
if(nearestId > 0)
{
if(_localRadius != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > _localRadius)
if(tolerance != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > tolerance)
{
UWARN("Cannot plan farther than %f m from the graph! (distance=%f m from node %d)",
_localRadius, targetPose.getDistance(nodes.at(nearestId)), nearestId);
tolerance, targetPose.getDistance(nodes.at(nearestId)), nearestId);
}
else
{