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