Commit 8d530376 authored by florent's avatar florent
Browse files

Added some tests in ChppPlanner::solveOneProblem() for robustness purposes.

Test whether path is NULL even if roadmap builder return success. This should not
happen but seems to happen with ChppVisRdmBuilder (kwsPlus).
Test result of optimizer.
Copy path before inserting them in ChppProblem.
parent bfb460d0
43. Added some tests in ChppPlanner::solveOneProblem() for robustness purposes.
42. In ChppPlanner::solveOneProblem, if path is null after calling rdmBuilder return KD_ERROR.
41. Added a method to interrupt the roadmap builder during path planning process.
hppCore 1.4 released June 13, 2008
......
......@@ -461,12 +461,13 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
}
ChppProblem& hppProblem = hppProblemVector[problemId];
CkwsPathShPtr kwsPath;
CkppDeviceComponentShPtr hppDevice = hppProblem.getRobot();
if (!hppDevice)
return KD_ERROR ;
CkwsPathShPtr kwsPath = CkwsPath::create(hppDevice);
CkwsConfigShPtr initConfig = hppProblem.initConfig() ;
if (!initConfig)
return KD_ERROR ;
......@@ -529,10 +530,9 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
}
ODEBUG2(":solveOneProblem: number of edges in roadmap after attempting at adding edge= " << roadmap->countEdges());
}
kwsPath = CkwsPath::create(hppDevice);
kwsPath->appendDirectPath(directPath);
// Add the path to vector of paths of the problem.
hppProblem.addPath(kwsPath);
hppProblem.addPath(KIT_DYNAMIC_PTR_CAST(CkwsPath, kwsPath->clone()));
return KD_OK;
}
......@@ -552,20 +552,30 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
ODEBUG2(":solveOneProblem: ---- Problem NOT solved.----");
return KD_ERROR;
}
if (!kwsPath) {
ODEBUG1(":solveOneProblem: no path after successfully solving the problem");
ODEBUG1(":solveOneProblem: this should not happen.");
return KD_ERROR;
}
/*
Store path before optimization
*/
hppProblem.addPath(KIT_DYNAMIC_PTR_CAST(CkwsPath, kwsPath->clone()));
// optimizer for the path
if (hppProblem.pathOptimizer()) {
hppProblem.pathOptimizer()->optimizePath(kwsPath, hppProblem.roadmapBuilder()->penetration());
ODEBUG2(":solveOneProblem: path optimized with penetration "
<< hppProblem.roadmapBuilder()->penetration());
if (hppProblem.pathOptimizer()->optimizePath(kwsPath,
hppProblem.roadmapBuilder()->penetration())
== KD_OK) {
ODEBUG2(":solveOneProblem: path optimized with penetration "
<< hppProblem.roadmapBuilder()->penetration());
}
else {
ODEBUG1(":solveOneProblem: path optimization failed.");
}
} else {
ODEBUG1(":solveOneProblem: no Optimizer Defined ");
}
......@@ -573,7 +583,7 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
if (kwsPath) {
ODEBUG2(":solveOneProblem: number of direct path: "<<kwsPath->countDirectPaths());
// Add the path to vector of paths of the problem.
hppProblem.addPath(kwsPath);
hppProblem.addPath(KIT_DYNAMIC_PTR_CAST(CkwsPath, kwsPath->clone()));
}
} else {
ODEBUG1(":solveOneProblem: no roadmap builder");
......
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