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>
75 namespace ob = ompl::base;
76 namespace oc = ompl::control;
77 namespace ot = ompl::tools;
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...
Definition of a control path.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.