00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/tools/benchmark/Benchmark.h"
00038 #include "ompl/tools/benchmark/MachineSpecs.h"
00039 #include "ompl/util/Time.h"
00040 #include <boost/scoped_ptr.hpp>
00041 #include <boost/lexical_cast.hpp>
00042 #include <boost/progress.hpp>
00043 #include <fstream>
00044 #include <sstream>
00045
00047 namespace ompl
00048 {
00049 namespace tools
00050 {
00052 static std::string getResultsFilename(const Benchmark::CompleteExperiment &exp)
00053 {
00054 return "ompl_" + exp.host + "_" + boost::posix_time::to_iso_extended_string(exp.startTime) + ".log";
00055 }
00056
00058 static std::string getConsoleFilename(const Benchmark::CompleteExperiment &exp)
00059 {
00060 return "ompl_" + exp.host + "_" + boost::posix_time::to_iso_extended_string(exp.startTime) + ".console";
00061 }
00062
00063 static bool terminationCondition(const machine::MemUsage_t maxMem, const time::point &endTime)
00064 {
00065 if (time::now() < endTime && machine::getProcessMemoryUsage() < maxMem)
00066 return false;
00067 return true;
00068 }
00069
00070 class RunPlanner
00071 {
00072 public:
00073
00074 RunPlanner(const Benchmark *benchmark, bool useThreads)
00075 : benchmark_(benchmark), timeUsed_(0.0), memUsed_(0), useThreads_(useThreads)
00076 {
00077 }
00078
00079 void run(const base::PlannerPtr &planner, const machine::MemUsage_t memStart, const machine::MemUsage_t maxMem, const double maxTime)
00080 {
00081 if (!useThreads_)
00082 {
00083 runThread(planner, memStart + maxMem, time::seconds(maxTime));
00084 return;
00085 }
00086
00087 boost::thread t(boost::bind(&RunPlanner::runThread, this, planner, memStart + maxMem, time::seconds(maxTime)));
00088
00089
00090 if (!t.timed_join(time::seconds(maxTime * 1.25)))
00091 {
00092 status_ = base::PlannerStatus::CRASH;
00093
00094 std::stringstream es;
00095 es << "Planner " << benchmark_->getStatus().activePlanner << " did not complete run " << benchmark_->getStatus().activeRun
00096 << " within the specified amount of time (possible crash). Attempting to force termination of planning thread ..." << std::endl;
00097 std::cerr << es.str();
00098 logError(es.str().c_str());
00099
00100 t.interrupt();
00101 t.join();
00102
00103 std::string m = "Planning thread cancelled";
00104 std::cerr << m << std::endl;
00105 logError(m.c_str());
00106 }
00107
00108 if (memStart < memUsed_)
00109 memUsed_ -= memStart;
00110 else
00111 memUsed_ = 0;
00112 }
00113
00114 double getTimeUsed(void) const
00115 {
00116 return timeUsed_;
00117 }
00118
00119 machine::MemUsage_t getMemUsed(void) const
00120 {
00121 return memUsed_;
00122 }
00123
00124 base::PlannerStatus getStatus(void) const
00125 {
00126 return status_;
00127 }
00128
00129 private:
00130
00131 void runThread(const base::PlannerPtr &planner, const machine::MemUsage_t maxMem, const time::duration &maxDuration)
00132 {
00133 time::point timeStart = time::now();
00134
00135 try
00136 {
00137 base::PlannerTerminationConditionFn ptc = boost::bind(&terminationCondition, maxMem, time::now() + maxDuration);
00138 status_ = planner->solve(ptc, 0.1);
00139 }
00140 catch(std::runtime_error &e)
00141 {
00142 std::stringstream es;
00143 es << "There was an error executing planner " << benchmark_->getStatus().activePlanner << ", run = " << benchmark_->getStatus().activeRun << std::endl;
00144 es << "*** " << e.what() << std::endl;
00145 std::cerr << es.str();
00146 logError(es.str().c_str());
00147 }
00148
00149 timeUsed_ = time::seconds(time::now() - timeStart);
00150 memUsed_ = machine::getProcessMemoryUsage();
00151 }
00152
00153 const Benchmark *benchmark_;
00154 double timeUsed_;
00155 machine::MemUsage_t memUsed_;
00156 base::PlannerStatus status_;
00157 bool useThreads_;
00158 };
00159
00160 }
00161 }
00163
00164 bool ompl::tools::Benchmark::saveResultsToFile(const char *filename) const
00165 {
00166 bool result = false;
00167
00168 std::ofstream fout(filename);
00169 if (fout.good())
00170 {
00171 result = saveResultsToStream(fout);
00172 logInform("Results saved to '%s'", filename);
00173 }
00174 else
00175 {
00176
00177 if (getResultsFilename(exp_) != std::string(filename))
00178 result = saveResultsToFile();
00179
00180 logError("Unable to write results to '%s'", filename);
00181 }
00182 return result;
00183 }
00184
00185 bool ompl::tools::Benchmark::saveResultsToFile(void) const
00186 {
00187 std::string filename = getResultsFilename(exp_);
00188 return saveResultsToFile(filename.c_str());
00189 }
00190
00191 bool ompl::tools::Benchmark::saveResultsToStream(std::ostream &out) const
00192 {
00193 if (exp_.planners.empty())
00194 {
00195 logWarn("There is no experimental data to save");
00196 return false;
00197 }
00198
00199 if (!out.good())
00200 {
00201 logError("Unable to write to stream");
00202 return false;
00203 }
00204
00205 out << "Experiment " << (exp_.name.empty() ? "NO_NAME" : exp_.name) << std::endl;
00206 out << "Running on " << (exp_.host.empty() ? "UNKNOWN" : exp_.host) << std::endl;
00207 out << "Starting at " << boost::posix_time::to_iso_extended_string(exp_.startTime) << std::endl;
00208 out << "<<<|" << std::endl << exp_.setupInfo << "|>>>" << std::endl;
00209
00210 out << exp_.seed << " is the random seed" << std::endl;
00211 out << exp_.maxTime << " seconds per run" << std::endl;
00212 out << exp_.maxMem << " MB per run" << std::endl;
00213 out << exp_.runCount << " runs per planner" << std::endl;
00214 out << exp_.totalDuration << " seconds spent to collect the data" << std::endl;
00215
00216
00217 out << "1 enum type" << std::endl;
00218 out << "status";
00219 for (unsigned int i = 0 ; i < base::PlannerStatus::TYPE_COUNT ; ++i)
00220 out << '|' << base::PlannerStatus(static_cast<base::PlannerStatus::StatusType>(i)).asString();
00221 out << std::endl;
00222
00223 out << exp_.planners.size() << " planners" << std::endl;
00224
00225 for (unsigned int i = 0 ; i < exp_.planners.size() ; ++i)
00226 {
00227 out << exp_.planners[i].name << std::endl;
00228
00229
00230 std::vector<std::string> properties;
00231 for (std::map<std::string, std::string>::const_iterator mit = exp_.planners[i].common.begin() ;
00232 mit != exp_.planners[i].common.end() ; ++mit)
00233 properties.push_back(mit->first);
00234 std::sort(properties.begin(), properties.end());
00235
00236
00237 out << properties.size() << " common properties" << std::endl;
00238 for (unsigned int k = 0 ; k < properties.size() ; ++k)
00239 {
00240 std::map<std::string, std::string>::const_iterator it = exp_.planners[i].common.find(properties[k]);
00241 out << it->first << " = " << it->second << std::endl;
00242 }
00243
00244
00245 std::map<std::string, bool> propSeen;
00246 for (unsigned int j = 0 ; j < exp_.planners[i].runs.size() ; ++j)
00247 for (std::map<std::string, std::string>::const_iterator mit = exp_.planners[i].runs[j].begin() ;
00248 mit != exp_.planners[i].runs[j].end() ; ++mit)
00249 propSeen[mit->first] = true;
00250
00251 properties.clear();
00252
00253 for (std::map<std::string, bool>::iterator it = propSeen.begin() ; it != propSeen.end() ; ++it)
00254 properties.push_back(it->first);
00255 std::sort(properties.begin(), properties.end());
00256
00257
00258 out << properties.size() << " properties for each run" << std::endl;
00259 for (unsigned int j = 0 ; j < properties.size() ; ++j)
00260 out << properties[j] << std::endl;
00261
00262
00263 out << exp_.planners[i].runs.size() << " runs" << std::endl;
00264 for (unsigned int j = 0 ; j < exp_.planners[i].runs.size() ; ++j)
00265 {
00266 for (unsigned int k = 0 ; k < properties.size() ; ++k)
00267 {
00268 std::map<std::string, std::string>::const_iterator it = exp_.planners[i].runs[j].find(properties[k]);
00269 if (it != exp_.planners[i].runs[j].end())
00270 out << it->second;
00271 out << "; ";
00272 }
00273 out << std::endl;
00274 }
00275
00276 out << '.' << std::endl;
00277 }
00278 return true;
00279 }
00280
00281 void ompl::tools::Benchmark::benchmark(const Request &req)
00282 {
00283
00284 if (gsetup_)
00285 {
00286 if (!gsetup_->getSpaceInformation()->isSetup())
00287 gsetup_->getSpaceInformation()->setup();
00288 }
00289 else
00290 {
00291 if (!csetup_->getSpaceInformation()->isSetup())
00292 csetup_->getSpaceInformation()->setup();
00293 }
00294
00295 if (!(gsetup_ ? gsetup_->getGoal() : csetup_->getGoal()))
00296 {
00297 logError("No goal defined");
00298 return;
00299 }
00300
00301 if (planners_.empty())
00302 {
00303 logError("There are no planners to benchmark");
00304 return;
00305 }
00306
00307 status_.running = true;
00308 exp_.totalDuration = 0.0;
00309 exp_.maxTime = req.maxTime;
00310 exp_.maxMem = req.maxMem;
00311 exp_.runCount = req.runCount;
00312 exp_.host = machine::getHostname();
00313 exp_.seed = RNG::getSeed();
00314
00315 exp_.startTime = time::now();
00316
00317 logInform("Configuring planners ...");
00318
00319
00320 exp_.planners.clear();
00321 exp_.planners.resize(planners_.size());
00322
00323 const base::ProblemDefinitionPtr &pdef = gsetup_ ? gsetup_->getProblemDefinition() : csetup_->getProblemDefinition();
00324
00325 for (unsigned int i = 0 ; i < planners_.size() ; ++i)
00326 {
00327
00328 planners_[i]->setProblemDefinition(pdef);
00329 if (!planners_[i]->isSetup())
00330 planners_[i]->setup();
00331 exp_.planners[i].name = (gsetup_ ? "geometric_" : "control_") + planners_[i]->getName();
00332 logInform("Configured %s", exp_.planners[i].name.c_str());
00333 }
00334
00335 logInform("Done configuring planners.");
00336 logInform("Saving planner setup information ...");
00337
00338 std::stringstream setupInfo;
00339 if (gsetup_)
00340 gsetup_->print(setupInfo);
00341 else
00342 csetup_->print(setupInfo);
00343 setupInfo << std::endl << "Properties of benchmarked planners:" << std::endl;
00344 for (unsigned int i = 0 ; i < planners_.size() ; ++i)
00345 planners_[i]->printProperties(setupInfo);
00346
00347 exp_.setupInfo = setupInfo.str();
00348
00349 logInform("Done saving information");
00350
00351 logInform("Beginning benchmark");
00352 msg::OutputHandler *oh = msg::getOutputHandler();
00353 boost::scoped_ptr<msg::OutputHandlerFile> ohf;
00354 if (req.saveConsoleOutput)
00355 {
00356 ohf.reset(new msg::OutputHandlerFile(getConsoleFilename(exp_).c_str()));
00357 msg::useOutputHandler(ohf.get());
00358 }
00359 else
00360 msg::noOutputHandler();
00361 logInform("Beginning benchmark");
00362
00363 boost::scoped_ptr<boost::progress_display> progress;
00364 if (req.displayProgress)
00365 {
00366 std::cout << "Running experiment " << exp_.name << "." << std::endl;
00367 std::cout << "Each planner will be executed " << req.runCount << " times for at most " << req.maxTime << " seconds. Memory is limited at "
00368 << req.maxMem << "MB." << std::endl;
00369 progress.reset(new boost::progress_display(100, std::cout));
00370 }
00371
00372 machine::MemUsage_t memStart = machine::getProcessMemoryUsage();
00373 machine::MemUsage_t maxMemBytes = (machine::MemUsage_t)(req.maxMem * 1024 * 1024);
00374
00375 for (unsigned int i = 0 ; i < planners_.size() ; ++i)
00376 {
00377 status_.activePlanner = exp_.planners[i].name;
00378
00379 try
00380 {
00381 if (plannerSwitch_)
00382 {
00383 logInform("Executing planner-switch event for planner %s ...", status_.activePlanner.c_str());
00384 plannerSwitch_(planners_[i]);
00385 logInform("Completed execution of planner-switch event");
00386 }
00387 }
00388 catch(std::runtime_error &e)
00389 {
00390 std::stringstream es;
00391 es << "There was an error executing the planner-switch event for planner " << status_.activePlanner << std::endl;
00392 es << "*** " << e.what() << std::endl;
00393 std::cerr << es.str();
00394 logError(es.str().c_str());
00395 }
00396 if (gsetup_)
00397 gsetup_->setup();
00398 else
00399 csetup_->setup();
00400 planners_[i]->params().getParams(exp_.planners[i].common);
00401 planners_[i]->getSpaceInformation()->params().getParams(exp_.planners[i].common);
00402
00403
00404 for (unsigned int j = 0 ; j < req.runCount ; ++j)
00405 {
00406 status_.activeRun = j;
00407 status_.progressPercentage = (double)(100 * (req.runCount * i + j)) / (double)(planners_.size() * req.runCount);
00408
00409 if (req.displayProgress)
00410 while (status_.progressPercentage > progress->count())
00411 ++(*progress);
00412
00413 logInform("Preparing for run %d of %s", status_.activeRun, status_.activePlanner.c_str());
00414
00415
00416 try
00417 {
00418 planners_[i]->clear();
00419 if (gsetup_)
00420 {
00421 gsetup_->getProblemDefinition()->clearSolutionPaths();
00422 gsetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
00423 }
00424 else
00425 {
00426 csetup_->getProblemDefinition()->clearSolutionPaths();
00427 csetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
00428 }
00429 }
00430 catch(std::runtime_error &e)
00431 {
00432 std::stringstream es;
00433 es << "There was an error while preparing for run " << status_.activeRun << " of planner " << status_.activePlanner << std::endl;
00434 es << "*** " << e.what() << std::endl;
00435 std::cerr << es.str();
00436 logError(es.str().c_str());
00437 }
00438
00439
00440 try
00441 {
00442 if (preRun_)
00443 {
00444 logInform("Executing pre-run event for run %d of planner %s ...", status_.activeRun, status_.activePlanner.c_str());
00445 preRun_(planners_[i]);
00446 logInform("Completed execution of pre-run event");
00447 }
00448 }
00449 catch(std::runtime_error &e)
00450 {
00451 std::stringstream es;
00452 es << "There was an error executing the pre-run event for run " << status_.activeRun << " of planner " << status_.activePlanner << std::endl;
00453 es << "*** " << e.what() << std::endl;
00454 std::cerr << es.str();
00455 logError(es.str().c_str());
00456 }
00457
00458 RunPlanner rp(this, req.useThreads);
00459 rp.run(planners_[i], memStart, maxMemBytes, req.maxTime);
00460 bool solved = gsetup_ ? gsetup_->haveSolutionPath() : csetup_->haveSolutionPath();
00461
00462
00463 try
00464 {
00465 RunProperties run;
00466
00467 run["time REAL"] = boost::lexical_cast<std::string>(rp.getTimeUsed());
00468 run["memory REAL"] = boost::lexical_cast<std::string>((double)rp.getMemUsed() / (1024.0 * 1024.0));
00469 run["status ENUM"] = boost::lexical_cast<std::string>((int)static_cast<base::PlannerStatus::StatusType>(rp.getStatus()));
00470 if (gsetup_)
00471 {
00472 run["solved BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->haveExactSolutionPath());
00473 run["valid segment fraction REAL"] = boost::lexical_cast<std::string>(gsetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
00474 }
00475 else
00476 {
00477 run["solved BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->haveExactSolutionPath());
00478 run["valid segment fraction REAL"] = boost::lexical_cast<std::string>(csetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
00479 }
00480
00481 if (solved)
00482 {
00483 if (gsetup_)
00484 {
00485 run["approximate solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getProblemDefinition()->hasApproximateSolution());
00486 run["solution difference REAL"] = boost::lexical_cast<std::string>(gsetup_->getProblemDefinition()->getSolutionDifference());
00487 run["solution length REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().length());
00488 run["solution smoothness REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().smoothness());
00489 run["solution clearance REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().clearance());
00490 run["solution segments INTEGER"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().getStateCount() - 1);
00491 run["correct solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
00492
00493 unsigned int factor = gsetup_->getStateSpace()->getValidSegmentCountFactor();
00494 gsetup_->getStateSpace()->setValidSegmentCountFactor(factor * 4);
00495 run["correct solution strict BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
00496 gsetup_->getStateSpace()->setValidSegmentCountFactor(factor);
00497
00498
00499 time::point timeStart = time::now();
00500 gsetup_->simplifySolution();
00501 double timeUsed = time::seconds(time::now() - timeStart);
00502 run["simplification time REAL"] = boost::lexical_cast<std::string>(timeUsed);
00503 run["simplified solution length REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().length());
00504 run["simplified solution smoothness REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().smoothness());
00505 run["simplified solution clearance REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().clearance());
00506 run["simplified solution segments INTEGER"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().getStateCount() - 1);
00507 run["simplified correct solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
00508 gsetup_->getStateSpace()->setValidSegmentCountFactor(factor * 4);
00509 run["simplified correct solution strict BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
00510 gsetup_->getStateSpace()->setValidSegmentCountFactor(factor);
00511 }
00512 else
00513 {
00514 run["approximate solution BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->getProblemDefinition()->hasApproximateSolution());
00515 run["solution difference REAL"] = boost::lexical_cast<std::string>(csetup_->getProblemDefinition()->getSolutionDifference());
00516 run["solution length REAL"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().length());
00517 run["solution clearance REAL"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().asGeometric().clearance());
00518 run["solution segments INTEGER"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().getControlCount());
00519 run["correct solution BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().check());
00520 }
00521 }
00522
00523 base::PlannerData pd (gsetup_ ? gsetup_->getSpaceInformation() : csetup_->getSpaceInformation());
00524 planners_[i]->getPlannerData(pd);
00525 run["graph states INTEGER"] = boost::lexical_cast<std::string>(pd.numVertices());
00526 run["graph motions INTEGER"] = boost::lexical_cast<std::string>(pd.numEdges());
00527
00528 for (std::map<std::string, std::string>::const_iterator it = pd.properties.begin() ; it != pd.properties.end() ; ++it)
00529 run[it->first] = it->second;
00530
00531
00532 try
00533 {
00534 if (postRun_)
00535 {
00536 logInform("Executing post-run event for run %d of planner %s ...", status_.activeRun, status_.activePlanner.c_str());
00537 postRun_(planners_[i], run);
00538 logInform("Completed execution of post-run event");
00539 }
00540 }
00541 catch(std::runtime_error &e)
00542 {
00543 std::stringstream es;
00544 es << "There was an error in the execution of the post-run event for run " << status_.activeRun << " of planner " << status_.activePlanner << std::endl;
00545 es << "*** " << e.what() << std::endl;
00546 std::cerr << es.str();
00547 logError(es.str().c_str());
00548 }
00549
00550 exp_.planners[i].runs.push_back(run);
00551 }
00552 catch(std::runtime_error &e)
00553 {
00554 std::stringstream es;
00555 es << "There was an error in the extraction of planner results: planner = " << status_.activePlanner << ", run = " << status_.activePlanner << std::endl;
00556 es << "*** " << e.what() << std::endl;
00557 std::cerr << es.str();
00558 logError(es.str().c_str());
00559 }
00560 }
00561 }
00562
00563 status_.running = false;
00564 status_.progressPercentage = 100.0;
00565 if (req.displayProgress)
00566 {
00567 while (status_.progressPercentage > progress->count())
00568 ++(*progress);
00569 std::cout << std::endl;
00570 }
00571
00572 exp_.totalDuration = time::seconds(time::now() - exp_.startTime);
00573
00574 logInform("Benchmark complete");
00575 msg::useOutputHandler(oh);
00576 logInform("Benchmark complete");
00577 }