bool RagGraphPlanner::findGlobalTrajectory(const Controller::State &begin, const Controller::State &end, Controller::Trajectory &trajectory, Controller::Trajectory::iterator iter, const GenWorkspaceChainState* wend) {
CriticalSectionWrapper csw(csCommand);
#ifdef _HBGRAPHPLANNER_PERFMON
PerfTimer t;
#endif
#ifdef _HBGRAPHPLANNER_PERFMON
#ifdef _HBHEURISTIC_PERFMON
HBHeuristic::resetLog();
HBCollision::resetLog();
#endif
#ifdef _BOUNDS_PERFMON
Bounds::resetLog();
#endif
#endif
getCallbackDataSync()->syncCollisionBounds();
#ifdef _HBGRAPHPLANNER_PERFMON
t.reset();
#endif
// generate global graph only for the arm
context.debug("GraphPlanner::findGlobalTrajectory(): Enabled Uncertainty %s. disable hand planning...\n", getHBHeuristic()->enableUnc ? "ON" : "OFF");
disableHandPlanning();
// context.write("findGlobalTrajectory(): %s\n", hbplannerDebug(*this).c_str());
//context.write("GraphPlanner::findGlobalTrajectory(): %s\n", hbplannerConfigspaceDebug(*this).c_str());
//context.write("GraphPlanner::findGlobalTrajectory(): %s\n", hbplannerWorkspaceDebug(*this).c_str());
// generate global graph
pGlobalPathFinder->generateOnlineGraph(begin.cpos, end.cpos);
// find node path on global graph
globalPath.clear();
if (!pGlobalPathFinder->findPath(end.cpos, globalPath, globalPath.begin())) {
context.error("GlobalPathFinder::findPath(): unable to find global path\n");
return false;
}
#ifdef _HBGRAPHPLANNER_PERFMON
context.write(
"GlobalPathFinder::findPath(): time_elapsed = %f [sec], len = %d\n",
t.elapsed(), globalPath.size()
);
#endif
if (pLocalPathFinder != NULL) {
#ifdef _HBGRAPHPLANNER_PERFMON
t.reset();
#endif
PARAMETER_GUARD(Heuristic, Real, Scale, *pHeuristic);
for (U32 i = 0;;) {
localPath = globalPath;
if (localFind(begin.cpos, end.cpos, localPath))
break;
else if (++i > pathFinderDesc.numOfTrials) {
context.error("LocalPathFinder::findPath(): unable to find local path\n");
return false;
}
}
#ifdef _HBGRAPHPLANNER_PERFMON
context.write(
"LocalPathFinder::findPath(): time_elapsed = %f [sec], len = %d\n",
t.elapsed(), localPath.size()
);
#endif
// copy localPath
optimisedPath = localPath;
}
else {
// copy globalPath
optimisedPath = globalPath;
}
#ifdef _HBGRAPHPLANNER_PERFMON
#ifdef _HBHEURISTIC_PERFMON
// context.debug("Enabled Uncertainty %s\n", getHBHeuristic()->enableUnc ? "ON" : "OFF");
//HBHeuristic::writeLog(context, "PathFinder::find()");
HBCollision::writeLog(context, "PathFinder::find()");
#endif
#ifdef _BOUNDS_PERFMON
Bounds::writeLog(context, "PathFinder::find()");
#endif
#endif
#ifdef _HBGRAPHPLANNER_PERFMON
#ifdef _HBHEURISTIC_PERFMON
HBHeuristic::resetLog();
HBCollision::resetLog();
#endif
#ifdef _BOUNDS_PERFMON
Bounds::resetLog();
#endif
t.reset();
#endif
optimize(optimisedPath, begin.cacc, end.cacc);
#ifdef _HBGRAPHPLANNER_PERFMON
context.write(
"GraphPlanner::optimize(): time_elapsed = %f [sec], len = %d\n", t.elapsed(), optimisedPath.size()
);
#ifdef _HBHEURISTIC_PERFMON
HBHeuristic::writeLog(context, "GraphPlanner::optimize()");
HBCollision::writeLog(context, "GraphPlanner::optimize()");
#endif
#ifdef _BOUNDS_PERFMON
Bounds::writeLog(context, "GraphPlanner::optimize()");
#endif
#endif
#ifdef _HBGRAPHPLANNER_PERFMON
t.reset();
#endif
Controller::Trajectory::iterator iend = iter;
pProfile->create(optimisedPath.begin(), optimisedPath.end(), begin, end, trajectory, iter, iend);
pProfile->profile(trajectory, iter, iend);
#ifdef _HBGRAPHPLANNER_PERFMON
context.write(
"GraphPlanner::profile(): time_elapsed = %f [sec], len = %d\n",
t.elapsed(), trajectory.size()
);
#endif
getCallbackDataSync()->syncFindTrajectory(trajectory.begin(), trajectory.end(), wend);
return true;
}
