void benchmark(KoulesSetup& ks, ot::Benchmark::Request request,
const std::string& plannerName, const std::string& outputFile)
{
// Create a benchmark class
ompl::tools::Benchmark b(ks, "Koules");
b.addExperimentParameter("num_koules", "INTEGER", boost::lexical_cast<std::string>(
(ks.getStateSpace()->getDimension() - 5) / 4));
// Add the planner to evaluate
b.addPlanner(ks.getConfiguredPlannerInstance(plannerName));
// Start benchmark
b.benchmark(request);
// Save the results
b.saveResultsToFile(outputFile.c_str());
OMPL_INFORM("Output saved in %s", outputFile.c_str());
}
void benchmark(KoulesSetup& ks, ot::Benchmark::Request request,
const std::string& plannerName, const std::string& outputFile)
{
// Create a benchmark class
ompl::tools::Benchmark b(ks, "Koules experiment");
// Add the planner to evaluate
b.addPlanner(ks.getConfiguredPlannerInstance(plannerName));
// Start benchmark
b.benchmark(request);
// Save the results
b.saveResultsToFile(outputFile.c_str());
OMPL_INFORM("Output saved in %s", outputFile.c_str());
}
void plan(KoulesSetup& ks, double maxTime, const std::string& outputFile)
{
if (ks.solve(maxTime))
{
std::ofstream out(outputFile.c_str());
oc::PathControl path(ks.getSolutionPath());
path.interpolate();
if (!path.check())
OMPL_ERROR("Path is invalid");
writeParams(out);
path.printAsMatrix(out);
if (!ks.haveExactSolutionPath())
OMPL_INFORM("Solution is approximate. Distance to actual goal is %g",
ks.getProblemDefinition()->getSolutionDifference());
OMPL_INFORM("Output saved in %s", outputFile.c_str());
}
#if 0
// Get the planner data, save the ship's (x,y) coordinates to one file and
// the edge information to another file. This can be used for debugging
// purposes; plotting the tree of states might give you some idea of
// a planner's strategy.
ob::PlannerData pd(ks.getSpaceInformation());
ks.getPlannerData(pd);
std::ofstream vertexFile((outputFile + "-vertices").c_str()), edgeFile((outputFile + "-edges").c_str());
double* coords;
unsigned numVerts = pd.numVertices();
std::vector<unsigned int> edgeList;
for (unsigned int i = 0; i < numVerts; ++i)
{
coords = pd.getVertex(i).getState()->as<KoulesStateSpace::StateType>()->values;
vertexFile << coords[0] << ' ' << coords[1] << '\n';
pd.getEdges(i, edgeList);
for (unsigned int j = 0; j < edgeList.size(); ++j)
edgeFile << i << ' ' << edgeList[j] << '\n';
}
#endif
}
