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

Added initial node to graph computed from A*

Browse Source
This commit is contained in:
Mathieu Labbe
2015-01-26 17:20:43 -05:00
parent f9fb6216d8
commit 47045002a9
2 changed files with 17 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/src/Graph.cpp
View File

@@ -719,6 +719,7 @@ std::vector<int> computePath(
path.push_front(currentNode.id());
currentNode = nodes.find(currentNode.fromId())->second;
}
path.push_front(startNode);
break;
}
@@ -743,7 +744,7 @@ std::vector<int> computePath(
float newCostSoFar = currentNode.costSoFar() + currentNode.distFrom(nodeIter->second.pose());
if(nodeIter->second.costSoFar() > newCostSoFar)
{
UERROR("newCostSoFar > previous cost (%f vs %f)", newCostSoFar, nodeIter->second.costSoFar());
UWARN("newCostSoFar > previous cost (%f vs %f)", newCostSoFar, nodeIter->second.costSoFar());
}
}
}