Commit 929e15a0 authored by florent's avatar florent
Browse files

If direct path is solution of a problem, add direct path roadmap.

parent 0dedc5dd
...@@ -515,7 +515,7 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId) ...@@ -515,7 +515,7 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
ODEBUG2(":solveOneProblem: Problem solved with direct connection. "); ODEBUG2(":solveOneProblem: Problem solved with direct connection. ");
#if 0 #if 1
/* Add direct path to roadmap if not already included */ /* Add direct path to roadmap if not already included */
if (hppProblem.roadmapBuilder()) { if (hppProblem.roadmapBuilder()) {
CkwsRoadmapShPtr roadmap = hppProblem.roadmapBuilder()->roadmap(); CkwsRoadmapShPtr roadmap = hppProblem.roadmapBuilder()->roadmap();
...@@ -529,15 +529,22 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId) ...@@ -529,15 +529,22 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
/* If start and goal node are not in roadmap, add them. */ /* If start and goal node are not in roadmap, add them. */
if (!startNode) { if (!startNode) {
startNode = CkwsNode::create(*initConfig); startNode = CkwsNode::create(*initConfig);
roadmap->addNode(startNode); if (roadmap->addNode(startNode) != KD_OK) {
ODEBUG1(":solveOneProblem: failed to add start node in roadmap.");
startNode.reset();
}
} }
if (!goalNode) { if (!goalNode) {
goalNode = CkwsNode::create(*goalConfig); goalNode = CkwsNode::create(*goalConfig);
roadmap->addNode(goalNode); if (roadmap->addNode(goalNode) != KD_OK) {
ODEBUG1(":solveOneProblem: failed to add goal node in roadmap.");
goalNode.reset();
}
} }
ODEBUG2(":solveOneProblem: number of edges in roadmap after adding nodes = " << roadmap->countEdges()); ODEBUG2(":solveOneProblem: number of edges in roadmap after adding nodes = " << roadmap->countEdges());
if (startNode && goalNode) {
/* Add edge only if goal node is not accessible from initial node */ /* Add edge only if goal node is not accessible from initial node */
if (!startNode->hasTransitiveOutNode(goalNode)) { if (!startNode->hasTransitiveOutNode(goalNode)) {
CkwsEdgeShPtr edge=CkwsEdge::create (directPath); CkwsEdgeShPtr edge=CkwsEdge::create (directPath);
...@@ -548,6 +555,7 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId) ...@@ -548,6 +555,7 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
} }
ODEBUG2(":solveOneProblem: number of edges in roadmap after attempting at adding edge= " << roadmap->countEdges()); ODEBUG2(":solveOneProblem: number of edges in roadmap after attempting at adding edge= " << roadmap->countEdges());
} }
}
#endif #endif
kwsPath->appendDirectPath(directPath); kwsPath->appendDirectPath(directPath);
// Add the path to vector of paths of the problem. // Add the path to vector of paths of the problem.
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment