37 #include "ompl/tools/benchmark/Benchmark.h"
38 #include "ompl/tools/benchmark/MachineSpecs.h"
39 #include "ompl/util/Time.h"
40 #include <boost/scoped_ptr.hpp>
41 #include <boost/lexical_cast.hpp>
42 #include <boost/progress.hpp>
43 #include <boost/thread.hpp>
53 static std::string getResultsFilename(
const Benchmark::CompleteExperiment &exp)
55 return "ompl_" + exp.host +
"_" + boost::posix_time::to_iso_extended_string(exp.startTime) +
".log";
59 static std::string getConsoleFilename(
const Benchmark::CompleteExperiment &exp)
61 return "ompl_" + exp.host +
"_" + boost::posix_time::to_iso_extended_string(exp.startTime) +
".console";
75 RunPlanner(
const Benchmark *benchmark,
bool useThreads)
76 : benchmark_(benchmark), timeUsed_(0.0), memUsed_(0), useThreads_(useThreads)
88 boost::thread t(boost::bind(&RunPlanner::runThread,
this, planner, memStart + maxMem,
time::seconds(maxTime),
time::seconds(timeBetweenUpdates)));
91 #if BOOST_VERSION < 105000
96 if (!t.try_join_for(boost::chrono::duration<double>(maxTime * 1.25)))
101 std::stringstream es;
102 es <<
"Planner " << benchmark_->getStatus().activePlanner <<
" did not complete run " << benchmark_->getStatus().activeRun
103 <<
" within the specified amount of time (possible crash). Attempting to force termination of planning thread ..." << std::endl;
104 std::cerr << es.str();
110 std::string m =
"Planning thread cancelled";
111 std::cerr << m << std::endl;
115 if (memStart < memUsed_)
116 memUsed_ -= memStart;
121 double getTimeUsed(
void)
const
131 base::PlannerStatus getStatus(
void)
const
136 const Benchmark::RunProgressData& getRunProgressData(
void)
const
138 return runProgressData_;
158 boost::scoped_ptr<boost::thread> t;
159 if (planner->getPlannerProgressProperties().size() > 0)
160 t.reset(
new boost::thread(boost::bind(&RunPlanner::collectProgressProperties,
this,
161 planner->getPlannerProgressProperties(),
162 timeBetweenUpdates)));
163 status_ = planner->solve(ptc, 0.1);
166 solvedCondition_.notify_all();
167 solvedFlag_.unlock();
171 catch(std::runtime_error &e)
173 std::stringstream es;
174 es <<
"There was an error executing planner " << benchmark_->getStatus().activePlanner <<
", run = " << benchmark_->getStatus().activeRun << std::endl;
175 es <<
"*** " << e.what() << std::endl;
176 std::cerr << es.str();
189 boost::unique_lock<boost::mutex> ulock(solvedFlag_);
192 if (solvedCondition_.timed_wait(ulock,
time::now() + timePerUpdate))
197 std::string timeStamp =
198 boost::lexical_cast<std::string>(timeInSeconds);
199 std::map<std::string, std::string> data;
200 data[
"time REAL"] = timeStamp;
201 for (base::Planner::PlannerProgressProperties::const_iterator item = properties.begin();
202 item != properties.end();
205 data[item->first] = item->second();
207 runProgressData_.push_back(data);
212 const Benchmark *benchmark_;
215 base::PlannerStatus status_;
217 Benchmark::RunProgressData runProgressData_;
221 boost::mutex solvedFlag_;
222 boost::condition_variable solvedCondition_;
233 std::ofstream fout(filename);
242 if (getResultsFilename(
exp_) != std::string(filename))
245 OMPL_ERROR(
"Unable to write results to '%s'", filename);
252 std::string filename = getResultsFilename(exp_);
253 return saveResultsToFile(filename.c_str());
258 if (exp_.planners.empty())
260 OMPL_WARN(
"There is no experimental data to save");
270 out <<
"Experiment " << (exp_.name.empty() ?
"NO_NAME" : exp_.name) << std::endl;
271 out <<
"Running on " << (exp_.host.empty() ?
"UNKNOWN" : exp_.host) << std::endl;
272 out <<
"Starting at " << boost::posix_time::to_iso_extended_string(exp_.startTime) << std::endl;
273 out <<
"<<<|" << std::endl << exp_.setupInfo <<
"|>>>" << std::endl;
275 out << exp_.seed <<
" is the random seed" << std::endl;
276 out << exp_.maxTime <<
" seconds per run" << std::endl;
277 out << exp_.maxMem <<
" MB per run" << std::endl;
278 out << exp_.runCount <<
" runs per planner" << std::endl;
279 out << exp_.totalDuration <<
" seconds spent to collect the data" << std::endl;
282 out <<
"1 enum type" << std::endl;
288 out << exp_.planners.size() <<
" planners" << std::endl;
290 for (
unsigned int i = 0 ; i < exp_.planners.size() ; ++i)
292 out << exp_.planners[i].name << std::endl;
295 std::vector<std::string> properties;
296 for (std::map<std::string, std::string>::const_iterator mit = exp_.planners[i].common.begin() ;
297 mit != exp_.planners[i].common.end() ; ++mit)
298 properties.push_back(mit->first);
299 std::sort(properties.begin(), properties.end());
302 out << properties.size() <<
" common properties" << std::endl;
303 for (
unsigned int k = 0 ; k < properties.size() ; ++k)
305 std::map<std::string, std::string>::const_iterator it = exp_.planners[i].common.find(properties[k]);
306 out << it->first <<
" = " << it->second << std::endl;
310 std::map<std::string, bool> propSeen;
311 for (
unsigned int j = 0 ; j < exp_.planners[i].runs.size() ; ++j)
312 for (std::map<std::string, std::string>::const_iterator mit = exp_.planners[i].runs[j].begin() ;
313 mit != exp_.planners[i].runs[j].end() ; ++mit)
314 propSeen[mit->first] =
true;
318 for (std::map<std::string, bool>::iterator it = propSeen.begin() ; it != propSeen.end() ; ++it)
319 properties.push_back(it->first);
320 std::sort(properties.begin(), properties.end());
323 out << properties.size() <<
" properties for each run" << std::endl;
324 for (
unsigned int j = 0 ; j < properties.size() ; ++j)
325 out << properties[j] << std::endl;
328 out << exp_.planners[i].runs.size() <<
" runs" << std::endl;
329 for (
unsigned int j = 0 ; j < exp_.planners[i].runs.size() ; ++j)
331 for (
unsigned int k = 0 ; k < properties.size() ; ++k)
333 std::map<std::string, std::string>::const_iterator it = exp_.planners[i].runs[j].find(properties[k]);
334 if (it != exp_.planners[i].runs[j].end())
342 if (exp_.planners[i].runsProgressData.size() > 0)
345 out << exp_.planners[i].progressPropertyNames.size() <<
" progress properties for each run" << std::endl;
347 for (std::vector<std::string>::const_iterator iter =
348 exp_.planners[i].progressPropertyNames.begin();
349 iter != exp_.planners[i].progressPropertyNames.end();
352 out << *iter << std::endl;
355 out << exp_.planners[i].runsProgressData.size() <<
" runs" << std::endl;
356 for (std::size_t r = 0; r < exp_.planners[i].runsProgressData.size(); ++r)
359 for (std::size_t t = 0; t < exp_.planners[i].runsProgressData[r].size(); ++t)
362 for (std::map<std::string, std::string>::const_iterator iter =
363 exp_.planners[i].runsProgressData[r][t].begin();
364 iter != exp_.planners[i].runsProgressData[r][t].end();
367 out << iter->second <<
",";
379 out <<
'.' << std::endl;
389 if (!gsetup_->getSpaceInformation()->isSetup())
390 gsetup_->getSpaceInformation()->setup();
394 if (!csetup_->getSpaceInformation()->isSetup())
395 csetup_->getSpaceInformation()->setup();
398 if (!(gsetup_ ? gsetup_->getGoal() : csetup_->getGoal()))
404 if (planners_.empty())
406 OMPL_ERROR(
"There are no planners to benchmark");
410 status_.running =
true;
411 exp_.totalDuration = 0.0;
423 exp_.planners.clear();
424 exp_.planners.resize(planners_.size());
428 for (
unsigned int i = 0 ; i < planners_.size() ; ++i)
431 planners_[i]->setProblemDefinition(pdef);
432 if (!planners_[i]->isSetup())
433 planners_[i]->setup();
434 exp_.planners[i].name = (gsetup_ ?
"geometric_" :
"control_") + planners_[i]->getName();
435 OMPL_INFORM(
"Configured %s", exp_.planners[i].name.c_str());
439 OMPL_INFORM(
"Saving planner setup information ...");
441 std::stringstream setupInfo;
443 gsetup_->print(setupInfo);
445 csetup_->print(setupInfo);
446 setupInfo << std::endl <<
"Properties of benchmarked planners:" << std::endl;
447 for (
unsigned int i = 0 ; i < planners_.size() ; ++i)
448 planners_[i]->printProperties(setupInfo);
450 exp_.setupInfo = setupInfo.str();
456 boost::scoped_ptr<msg::OutputHandlerFile> ohf;
466 boost::scoped_ptr<boost::progress_display> progress;
469 std::cout <<
"Running experiment " << exp_.name <<
"." << std::endl;
470 std::cout <<
"Each planner will be executed " << req.
runCount <<
" times for at most " << req.
maxTime <<
" seconds. Memory is limited at "
471 << req.
maxMem <<
"MB." << std::endl;
472 progress.reset(
new boost::progress_display(100, std::cout));
478 for (
unsigned int i = 0 ; i < planners_.size() ; ++i)
480 status_.activePlanner = exp_.planners[i].name;
486 OMPL_INFORM(
"Executing planner-switch event for planner %s ...", status_.activePlanner.c_str());
487 plannerSwitch_(planners_[i]);
488 OMPL_INFORM(
"Completed execution of planner-switch event");
491 catch(std::runtime_error &e)
493 std::stringstream es;
494 es <<
"There was an error executing the planner-switch event for planner " << status_.activePlanner << std::endl;
495 es <<
"*** " << e.what() << std::endl;
496 std::cerr << es.str();
503 planners_[i]->params().getParams(exp_.planners[i].common);
504 planners_[i]->getSpaceInformation()->params().getParams(exp_.planners[i].common);
507 exp_.planners[i].progressPropertyNames.push_back(
"time REAL");
508 base::Planner::PlannerProgressProperties::const_iterator iter;
509 for (iter = planners_[i]->getPlannerProgressProperties().begin();
510 iter != planners_[i]->getPlannerProgressProperties().end();
513 exp_.planners[i].progressPropertyNames.push_back(iter->first);
515 std::sort(exp_.planners[i].progressPropertyNames.begin(),
516 exp_.planners[i].progressPropertyNames.end());
519 for (
unsigned int j = 0 ; j < req.
runCount ; ++j)
521 status_.activeRun = j;
522 status_.progressPercentage = (double)(100 * (req.
runCount * i + j)) / (
double)(planners_.size() * req.
runCount);
525 while (status_.progressPercentage > progress->count())
528 OMPL_INFORM(
"Preparing for run %d of %s", status_.activeRun, status_.activePlanner.c_str());
533 planners_[i]->clear();
536 gsetup_->getProblemDefinition()->clearSolutionPaths();
537 gsetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
541 csetup_->getProblemDefinition()->clearSolutionPaths();
542 csetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
545 catch(std::runtime_error &e)
547 std::stringstream es;
548 es <<
"There was an error while preparing for run " << status_.activeRun <<
" of planner " << status_.activePlanner << std::endl;
549 es <<
"*** " << e.what() << std::endl;
550 std::cerr << es.str();
559 OMPL_INFORM(
"Executing pre-run event for run %d of planner %s ...", status_.activeRun, status_.activePlanner.c_str());
560 preRun_(planners_[i]);
561 OMPL_INFORM(
"Completed execution of pre-run event");
564 catch(std::runtime_error &e)
566 std::stringstream es;
567 es <<
"There was an error executing the pre-run event for run " << status_.activeRun <<
" of planner " << status_.activePlanner << std::endl;
568 es <<
"*** " << e.what() << std::endl;
569 std::cerr << es.str();
575 bool solved = gsetup_ ? gsetup_->haveSolutionPath() : csetup_->haveSolutionPath();
582 run[
"time REAL"] = boost::lexical_cast<std::string>(rp.getTimeUsed());
583 run[
"memory REAL"] = boost::lexical_cast<std::string>((double)rp.getMemUsed() / (1024.0 * 1024.0));
584 run[
"status ENUM"] = boost::lexical_cast<std::string>((int)static_cast<base::PlannerStatus::StatusType>(rp.getStatus()));
587 run[
"solved BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->haveExactSolutionPath());
588 run[
"valid segment fraction REAL"] = boost::lexical_cast<std::string>(gsetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
592 run[
"solved BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->haveExactSolutionPath());
593 run[
"valid segment fraction REAL"] = boost::lexical_cast<std::string>(csetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
600 run[
"approximate solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getProblemDefinition()->hasApproximateSolution());
601 run[
"solution difference REAL"] = boost::lexical_cast<std::string>(gsetup_->getProblemDefinition()->getSolutionDifference());
602 run[
"solution length REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().length());
603 run[
"solution smoothness REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().smoothness());
604 run[
"solution clearance REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().clearance());
605 run[
"solution segments INTEGER"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().getStateCount() - 1);
606 run[
"correct solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
608 unsigned int factor = gsetup_->getStateSpace()->getValidSegmentCountFactor();
609 gsetup_->getStateSpace()->setValidSegmentCountFactor(factor * 4);
610 run[
"correct solution strict BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
611 gsetup_->getStateSpace()->setValidSegmentCountFactor(factor);
615 gsetup_->simplifySolution();
617 run[
"simplification time REAL"] = boost::lexical_cast<std::string>(timeUsed);
618 run[
"simplified solution length REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().length());
619 run[
"simplified solution smoothness REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().smoothness());
620 run[
"simplified solution clearance REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().clearance());
621 run[
"simplified solution segments INTEGER"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().getStateCount() - 1);
622 run[
"simplified correct solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
623 gsetup_->getStateSpace()->setValidSegmentCountFactor(factor * 4);
624 run[
"simplified correct solution strict BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
625 gsetup_->getStateSpace()->setValidSegmentCountFactor(factor);
629 run[
"approximate solution BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->getProblemDefinition()->hasApproximateSolution());
630 run[
"solution difference REAL"] = boost::lexical_cast<std::string>(csetup_->getProblemDefinition()->getSolutionDifference());
631 run[
"solution length REAL"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().length());
632 run[
"solution clearance REAL"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().asGeometric().clearance());
633 run[
"solution segments INTEGER"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().getControlCount());
634 run[
"correct solution BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().check());
638 base::PlannerData pd (gsetup_ ? gsetup_->getSpaceInformation() : csetup_->getSpaceInformation());
639 planners_[i]->getPlannerData(pd);
640 run[
"graph states INTEGER"] = boost::lexical_cast<std::string>(pd.numVertices());
641 run[
"graph motions INTEGER"] = boost::lexical_cast<std::string>(pd.numEdges());
643 for (std::map<std::string, std::string>::const_iterator it = pd.properties.begin() ; it != pd.properties.end() ; ++it)
644 run[it->first] = it->second;
651 OMPL_INFORM(
"Executing post-run event for run %d of planner %s ...", status_.activeRun, status_.activePlanner.c_str());
652 postRun_(planners_[i], run);
653 OMPL_INFORM(
"Completed execution of post-run event");
656 catch(std::runtime_error &e)
658 std::stringstream es;
659 es <<
"There was an error in the execution of the post-run event for run " << status_.activeRun <<
" of planner " << status_.activePlanner << std::endl;
660 es <<
"*** " << e.what() << std::endl;
661 std::cerr << es.str();
665 exp_.planners[i].runs.push_back(run);
669 if (planners_[i]->getPlannerProgressProperties().size() > 0)
671 exp_.planners[i].runsProgressData.push_back(rp.getRunProgressData());
674 catch(std::runtime_error &e)
676 std::stringstream es;
677 es <<
"There was an error in the extraction of planner results: planner = " << status_.activePlanner <<
", run = " << status_.activePlanner << std::endl;
678 es <<
"*** " << e.what() << std::endl;
679 std::cerr << es.str();
685 status_.running =
false;
686 status_.progressPercentage = 100.0;
687 if (req.displayProgress)
689 while (status_.progressPercentage > progress->count())
691 std::cout << std::endl;
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
boost::posix_time::time_duration duration
Representation of a time duration.
duration seconds(double sec)
Return the time duration representing a given number of seconds.
std::string asString() const
Return a string representation.
The number of possible status values.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
boost::posix_time::ptime point
Representation of a point in time.
A class to store the exit status of Planner::solve()
Generic class to handle output from a piece of code.
std::string getHostname(void)
Get the hostname of the machine in use.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
MemUsage_t getProcessMemoryUsage(void)
Get the amount of memory the current process is using. This should work on major platforms (Windows...
static boost::uint32_t getSeed(void)
Get the seed used for random number generation. Passing the returned value to setSeed() at a subseque...
Implementation of OutputHandler that saves messages in a file.
unsigned long long MemUsage_t
Amount of memory used, in bytes.
void useOutputHandler(OutputHandler *oh)
Specify the instance of the OutputHandler to use. By default, this is OutputHandlerSTD.
OutputHandler * getOutputHandler(void)
Get the instance of the OutputHandler currently used. This is NULL in case there is no output handler...
std::map< std::string, PlannerProgressProperty > PlannerProgressProperties
A dictionary which maps the name of a progress property to the function to be used for querying that ...
void noOutputHandler(void)
This function instructs ompl that no messages should be outputted. Equivalent to useOutputHandler(NUL...
point now(void)
Get the current time point.
boost::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.