66 #include "KoulesConfig.h"
67 #include "KoulesSetup.h"
68 #include "KoulesStateSpace.h"
69 #include <ompl/tools/benchmark/Benchmark.h>
70 #include <ompl/config.h>
71 #include <boost/program_options.hpp>
72 #include <boost/format.hpp>
78 namespace po = boost::program_options;
80 void writeParams(std::ostream& out)
82 out << sideLength <<
' ' << shipRadius <<
' ' << kouleRadius <<
' ' <<
' '
83 << propagationStepSize <<
' ' << shipAcceleration <<
' ' << shipRotVel <<
' '
84 << shipDelta <<
' ' << shipEps << std::endl;
87 void plan(KoulesSetup& ks,
double maxTime,
const std::string& outputFile)
89 if (ks.solve(maxTime))
91 std::ofstream out(outputFile.c_str());
97 path.printAsMatrix(out);
98 if (!ks.haveExactSolutionPath())
99 OMPL_INFORM(
"Solution is approximate. Distance to actual goal is %g",
100 ks.getProblemDefinition()->getSolutionDifference());
101 OMPL_INFORM(
"Output saved in %s", outputFile.c_str());
110 ks.getPlannerData(pd);
111 std::ofstream vertexFile((outputFile +
"-vertices").c_str()), edgeFile((outputFile +
"-edges").c_str());
113 unsigned numVerts = pd.numVertices();
114 std::vector<unsigned int> edgeList;
116 for (
unsigned int i = 0; i < numVerts; ++i)
118 coords = pd.getVertex(i).getState()->as<KoulesStateSpace::StateType>()->values;
119 vertexFile << coords[0] <<
' ' << coords[1] <<
'\n';
121 pd.getEdges(i, edgeList);
122 for (
unsigned int j = 0; j < edgeList.size(); ++j)
123 edgeFile << i <<
' ' << edgeList[j] <<
'\n';
129 void benchmark(KoulesSetup& ks, ot::Benchmark::Request request,
130 const std::string& plannerName,
const std::string& outputFile)
135 b.addPlanner(ks.getConfiguredPlannerInstance(plannerName));
137 b.benchmark(request);
139 b.saveResultsToFile(outputFile.c_str());
140 OMPL_INFORM(
"Output saved in %s", outputFile.c_str());
143 int main(
int argc,
char **argv)
147 unsigned int numKoules, numRuns;
148 double maxTime, kouleVel;
149 std::string plannerName, outputFile;
150 po::options_description desc(
"Options");
152 (
"help",
"show help message")
153 (
"plan",
"solve the game of koules")
154 (
"benchmark",
"benchmark the game of koules")
155 (
"numkoules", po::value<unsigned int>(&numKoules)->default_value(3),
156 "start from <numkoules> koules")
157 (
"maxtime", po::value<double>(&maxTime)->default_value(10.),
158 "time limit in seconds")
159 (
"output", po::value<std::string>(&outputFile),
"output file name")
160 (
"numruns", po::value<unsigned int>(&numRuns)->default_value(10),
161 "number of runs for each planner in benchmarking mode")
162 (
"planner", po::value<std::string>(&plannerName)->default_value(
"kpiece"),
163 "planning algorithm to use (pdst, kpiece, rrt, or est)")
164 (
"velocity", po::value<double>(&kouleVel)->default_value(0.),
165 "initial velocity of each koule")
168 po::variables_map vm;
169 po::store(po::parse_command_line(argc, argv, desc,
170 po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
173 KoulesSetup ks(numKoules, plannerName, kouleVel);
174 if (vm.count(
"help") || argc == 1)
176 std::cout <<
"Solve the games of Koules.\nSelect one of these two options:\n"
177 <<
"\"--plan\", or \"--benchmark\"\n\n" << desc <<
"\n";
181 if (outputFile.size() == 0)
183 std::string prefix(vm.count(
"plan") ?
"koules_" :
"koulesBenchmark_");
184 outputFile = boost::str(boost::format(
"%1%%2%_%3%_%4%.dat")
185 % prefix % numKoules % plannerName % maxTime);
187 if (vm.count(
"plan"))
188 plan(ks, maxTime, outputFile);
189 else if (vm.count(
"benchmark"))
190 benchmark(ks, ot::Benchmark::Request(maxTime, 10000.0, numRuns),
191 plannerName, outputFile);
193 catch(std::exception& e) {
194 std::cerr <<
"Error: " << e.what() <<
"\n";
198 std::cerr <<
"Exception of unknown type!\n";
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition of a control path.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.