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

Updated when computePath(...) returns true or false

Browse Source
This commit is contained in:
matlabbe
2015-09-23 12:11:18 -04:00
parent c60a808c75
commit fb69f770f9
1 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+8 -2
View File
@@ -3175,6 +3175,7 @@ bool Rtabmap::computePath(int targetNode, bool global)
} }
if(currentNode && targetNode) if(currentNode && targetNode)
{ {
std::list<std::pair<int, Transform> > path = graph::computePath( std::list<std::pair<int, Transform> > path = graph::computePath(
currentNode, currentNode,
targetNode, targetNode,
@@ -3198,6 +3199,7 @@ bool Rtabmap::computePath(int targetNode, bool global)
{ {
_path.clear(); _path.clear();
UWARN("Cannot compute a path!"); UWARN("Cannot compute a path!");
return false;
} }
else else
{ {
@@ -3222,9 +3224,11 @@ bool Rtabmap::computePath(int targetNode, bool global)
setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone()); setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone());
} }
updateGoalIndex(); updateGoalIndex();
return true;
} }
return _path.size()>0; return false;
} }
bool Rtabmap::computePath(const Transform & targetPose) bool Rtabmap::computePath(const Transform & targetPose)
@@ -3336,6 +3340,8 @@ bool Rtabmap::computePath(const Transform & targetPose)
_pathTransformToGoal = nodes.at(_path.back().first).inverse() * targetPose; _pathTransformToGoal = nodes.at(_path.back().first).inverse() * targetPose;
updateGoalIndex(); updateGoalIndex();
return true;
} }
} }
} }
@@ -3344,7 +3350,7 @@ bool Rtabmap::computePath(const Transform & targetPose)
UWARN("Nearest node not found in graph (size=%d) for pose %s", (int)nodes.size(), targetPose.prettyPrint().c_str()); UWARN("Nearest node not found in graph (size=%d) for pose %s", (int)nodes.size(), targetPose.prettyPrint().c_str());
} }
return _path.size()>0; return false;
} }
std::vector<std::pair<int, Transform> > Rtabmap::getPathNextPoses() const std::vector<std::pair<int, Transform> > Rtabmap::getPathNextPoses() const