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)
134 b.addExperimentParameter(
"num_koules",
"INTEGER", std::to_string(
135 (ks.getStateSpace()->getDimension() - 5) / 4));
137 b.addPlanner(ks.getConfiguredPlannerInstance(plannerName));
139 b.benchmark(request);
141 b.saveResultsToFile(outputFile.c_str());
142 OMPL_INFORM(
"Output saved in %s", outputFile.c_str());
145 int main(
int argc,
char **argv)
149 unsigned int numKoules, numRuns;
150 double maxTime, kouleVel;
151 std::string plannerName, outputFile;
152 po::options_description desc(
"Options");
154 (
"help",
"show help message")
155 (
"plan",
"solve the game of koules")
156 (
"benchmark",
"benchmark the game of koules")
157 (
"numkoules", po::value<unsigned int>(&numKoules)->default_value(3),
158 "start from <numkoules> koules")
159 (
"maxtime", po::value<double>(&maxTime)->default_value(10.),
160 "time limit in seconds")
161 (
"output", po::value<std::string>(&outputFile),
"output file name")
162 (
"numruns", po::value<unsigned int>(&numRuns)->default_value(10),
163 "number of runs for each planner in benchmarking mode")
164 (
"planner", po::value<std::string>(&plannerName)->default_value(
"kpiece"),
165 "planning algorithm to use (pdst, kpiece, sst, rrt, est, sycloprrt, or syclopest)")
166 (
"velocity", po::value<double>(&kouleVel)->default_value(0.),
167 "initial velocity of each koule")
170 po::variables_map vm;
171 po::store(po::parse_command_line(argc, argv, desc,
172 po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
175 KoulesSetup ks(numKoules, plannerName, kouleVel);
176 if ((vm.count(
"help") != 0u) || argc == 1)
178 std::cout <<
"Solve the games of Koules.\nSelect one of these two options:\n" 179 <<
"\"--plan\", or \"--benchmark\"\n\n" << desc <<
"\n";
183 if (outputFile.empty())
185 std::string prefix(vm.count(
"plan") != 0u ?
"koules_" :
"koulesBenchmark_");
186 outputFile = boost::str(boost::format(
"%1%%2%_%3%_%4%.dat")
187 % prefix % numKoules % plannerName % maxTime);
189 if (vm.count(
"plan") != 0u)
190 plan(ks, maxTime, outputFile);
191 else if (vm.count(
"benchmark") != 0u)
192 benchmark(ks, ot::Benchmark::Request(maxTime, 10000.0, numRuns),
193 plannerName, outputFile);
195 catch(std::exception& e) {
196 std::cerr <<
"Error: " << e.what() <<
"\n";
200 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.