Benchmark.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan, Luis G. Torres */
36 
37 #include "ompl/tools/benchmark/Benchmark.h"
38 #include "ompl/tools/benchmark/MachineSpecs.h"
39 #include "ompl/util/Time.h"
40 #include "ompl/config.h"
41 #include "ompl/util/String.h"
42 #include <boost/scoped_ptr.hpp>
43 #include <boost/progress.hpp>
44 #include <thread>
45 #include <mutex>
46 #include <condition_variable>
47 #include <fstream>
48 #include <sstream>
49 
51 namespace ompl
52 {
53  namespace tools
54  {
57  static std::string getResultsFilename(const Benchmark::CompleteExperiment &exp)
58  {
59  return "ompl_" + exp.host + "_" + time::as_string(exp.startTime) + ".log";
60  }
61 
64  static std::string getConsoleFilename(const Benchmark::CompleteExperiment &exp)
65  {
66  return "ompl_" + exp.host + "_" + time::as_string(exp.startTime) + ".console";
67  }
68 
69  static bool terminationCondition(const machine::MemUsage_t maxMem, const time::point &endTime)
70  {
71  if (time::now() < endTime && machine::getProcessMemoryUsage() < maxMem)
72  return false;
73  return true;
74  }
75 
76  class RunPlanner
77  {
78  public:
79  RunPlanner(const Benchmark *benchmark, bool useThreads)
80  : benchmark_(benchmark), timeUsed_(0.0), memUsed_(0), useThreads_(useThreads)
81  {
82  }
83 
84  void run(const base::PlannerPtr &planner, const machine::MemUsage_t memStart,
85  const machine::MemUsage_t maxMem, const double maxTime, const double timeBetweenUpdates)
86  {
87  // if (!useThreads_)
88  // {
89  runThread(planner, memStart + maxMem, time::seconds(maxTime), time::seconds(timeBetweenUpdates));
90  // return;
91  // }
92 
93  // std::thread t([planner, memStart, maxMem, maxTime, timeBetweenUpdates]
94  // {
95  // runThread(planner, memStart + maxMem, time::seconds(maxTime),
96  // time::seconds(timeBetweenUpdates)); });
97  // }
98 
99  // allow 25% more time than originally specified, in order to detect planner termination
100  // if (!t.try_join_for(time::seconds(maxTime * 1.25)))
101  // {
102  // status_ = base::PlannerStatus::CRASH;
103  //
104  // std::stringstream es;
105  // es << "Planner " << benchmark_->getStatus().activePlanner << " did not complete run " <<
106  // benchmark_->getStatus().activeRun
107  // << " within the specified amount of time (possible crash). Attempting to force termination of
108  // planning thread ..." << std::endl;
109  // std::cerr << es.str();
110  // OMPL_ERROR(es.str().c_str());
111  //
112  // t.interrupt();
113  // t.join();
114  //
115  // std::string m = "Planning thread cancelled";
116  // std::cerr << m << std::endl;
117  // OMPL_ERROR(m.c_str());
118  // }
119 
120  // if (memStart < memUsed_)
121  // memUsed_ -= memStart;
122  // else
123  // memUsed_ = 0;
124  }
125 
126  double getTimeUsed() const
127  {
128  return timeUsed_;
129  }
130 
131  machine::MemUsage_t getMemUsed() const
132  {
133  return memUsed_;
134  }
135 
136  base::PlannerStatus getStatus() const
137  {
138  return status_;
139  }
140 
141  const Benchmark::RunProgressData &getRunProgressData() const
142  {
143  return runProgressData_;
144  }
145 
146  private:
147  void runThread(const base::PlannerPtr &planner, const machine::MemUsage_t maxMem,
148  const time::duration &maxDuration, const time::duration &timeBetweenUpdates)
149  {
150  time::point timeStart = time::now();
151 
152  try
153  {
154  const time::point endtime = time::now() + maxDuration;
155  base::PlannerTerminationConditionFn ptc([maxMem, endtime]
156  {
157  return terminationCondition(maxMem, endtime);
158  });
159  solved_ = false;
160  // Only launch the planner progress property
161  // collector if there is any data for it to report
162  //
163  // \todo issue here is that at least one sample
164  // always gets taken before planner even starts;
165  // might be worth adding a short wait time before
166  // collector begins sampling
167  boost::scoped_ptr<std::thread> t;
168  if (planner->getPlannerProgressProperties().size() > 0)
169  t.reset(new std::thread([this, &planner, timeBetweenUpdates]
170  {
171  collectProgressProperties(planner->getPlannerProgressProperties(),
172  timeBetweenUpdates);
173  }));
174  status_ = planner->solve(ptc, 0.1);
175  solvedFlag_.lock();
176  solved_ = true;
177  solvedCondition_.notify_all();
178  solvedFlag_.unlock();
179  if (t)
180  t->join(); // maybe look into interrupting even if planner throws an exception
181  }
182  catch (std::runtime_error &e)
183  {
184  std::stringstream es;
185  es << "There was an error executing planner " << benchmark_->getStatus().activePlanner
186  << ", run = " << benchmark_->getStatus().activeRun << std::endl;
187  es << "*** " << e.what() << std::endl;
188  std::cerr << es.str();
189  OMPL_ERROR(es.str().c_str());
190  }
191 
192  timeUsed_ = time::seconds(time::now() - timeStart);
193  memUsed_ = machine::getProcessMemoryUsage();
194  }
195 
196  void collectProgressProperties(const base::Planner::PlannerProgressProperties &properties,
197  const time::duration &timePerUpdate)
198  {
199  time::point timeStart = time::now();
200 
201  std::unique_lock<std::mutex> ulock(solvedFlag_);
202  while (!solved_)
203  {
204  if (solvedCondition_.wait_for(ulock, timePerUpdate) == std::cv_status::no_timeout)
205  return;
206  else
207  {
208  double timeInSeconds = time::seconds(time::now() - timeStart);
209  std::string timeStamp = ompl::toString(timeInSeconds);
210  std::map<std::string, std::string> data;
211  data["time REAL"] = timeStamp;
212  for (const auto &property : properties)
213  {
214  data[property.first] = property.second();
215  }
216  runProgressData_.push_back(data);
217  }
218  }
219  }
220 
221  const Benchmark *benchmark_;
222  double timeUsed_;
223  machine::MemUsage_t memUsed_;
224  base::PlannerStatus status_;
225  bool useThreads_;
226  Benchmark::RunProgressData runProgressData_;
227 
228  // variables needed for progress property collection
229  bool solved_;
230  std::mutex solvedFlag_;
231  std::condition_variable solvedCondition_;
232  };
233  }
234 }
236 
237 bool ompl::tools::Benchmark::saveResultsToFile(const char *filename) const
238 {
239  bool result = false;
240 
241  std::ofstream fout(filename);
242  if (fout.good())
243  {
244  result = saveResultsToStream(fout);
245  OMPL_INFORM("Results saved to '%s'", filename);
246  }
247  else
248  {
249  // try to save to a different file, if we can
250  if (getResultsFilename(exp_) != std::string(filename))
251  result = saveResultsToFile();
252 
253  OMPL_ERROR("Unable to write results to '%s'", filename);
254  }
255  return result;
256 }
257 
259 {
260  std::string filename = getResultsFilename(exp_);
261  return saveResultsToFile(filename.c_str());
262 }
263 
264 bool ompl::tools::Benchmark::saveResultsToStream(std::ostream &out) const
265 {
266  if (exp_.planners.empty())
267  {
268  OMPL_WARN("There is no experimental data to save");
269  return false;
270  }
271 
272  if (!out.good())
273  {
274  OMPL_ERROR("Unable to write to stream");
275  return false;
276  }
277 
278  out << "OMPL version " << OMPL_VERSION << std::endl;
279  out << "Experiment " << (exp_.name.empty() ? "NO_NAME" : exp_.name) << std::endl;
280 
281  out << exp_.parameters.size() << " experiment properties" << std::endl;
282  for (const auto &parameter : exp_.parameters)
283  out << parameter.first << " = " << parameter.second << std::endl;
284 
285  out << "Running on " << (exp_.host.empty() ? "UNKNOWN" : exp_.host) << std::endl;
286  out << "Starting at " << time::as_string(exp_.startTime) << std::endl;
287  out << "<<<|" << std::endl << exp_.setupInfo << "|>>>" << std::endl;
288  out << "<<<|" << std::endl << exp_.cpuInfo << "|>>>" << std::endl;
289 
290  out << exp_.seed << " is the random seed" << std::endl;
291  out << exp_.maxTime << " seconds per run" << std::endl;
292  out << exp_.maxMem << " MB per run" << std::endl;
293  out << exp_.runCount << " runs per planner" << std::endl;
294  out << exp_.totalDuration << " seconds spent to collect the data" << std::endl;
295 
296  // change this if more enum types are added
297  out << "1 enum type" << std::endl;
298  out << "status";
299  for (unsigned int i = 0; i < base::PlannerStatus::TYPE_COUNT; ++i)
300  out << '|' << base::PlannerStatus(static_cast<base::PlannerStatus::StatusType>(i)).asString();
301  out << std::endl;
302 
303  out << exp_.planners.size() << " planners" << std::endl;
304 
305  for (const auto &planner : exp_.planners)
306  {
307  out << planner.name << std::endl;
308 
309  // get names of common properties
310  std::vector<std::string> properties;
311  for (auto &property : planner.common)
312  properties.push_back(property.first);
313  std::sort(properties.begin(), properties.end());
314 
315  // print names & values of common properties
316  out << properties.size() << " common properties" << std::endl;
317  for (auto &property : properties)
318  {
319  auto it = planner.common.find(property);
320  out << it->first << " = " << it->second << std::endl;
321  }
322 
323  // construct the list of all possible properties for all runs
324  std::map<std::string, bool> propSeen;
325  for (auto &run : planner.runs)
326  for (auto &property : run)
327  propSeen[property.first] = true;
328 
329  properties.clear();
330 
331  for (auto &it : propSeen)
332  properties.push_back(it.first);
333  std::sort(properties.begin(), properties.end());
334 
335  // print the property names
336  out << properties.size() << " properties for each run" << std::endl;
337  for (auto &property : properties)
338  out << property << std::endl;
339 
340  // print the data for each run
341  out << planner.runs.size() << " runs" << std::endl;
342  for (auto &run : planner.runs)
343  {
344  for (auto &property : properties)
345  {
346  auto it = run.find(property);
347  if (it != run.end())
348  out << it->second;
349  out << "; ";
350  }
351  out << std::endl;
352  }
353 
354  // print the run progress data if it was reported
355  if (planner.runsProgressData.size() > 0)
356  {
357  // Print number of progress properties
358  out << planner.progressPropertyNames.size() << " progress properties for each run" << std::endl;
359  // Print progress property names
360  for (const auto &progPropName : planner.progressPropertyNames)
361  {
362  out << progPropName << std::endl;
363  }
364  // Print progress properties for each run
365  out << planner.runsProgressData.size() << " runs" << std::endl;
366  for (const auto &r : planner.runsProgressData)
367  {
368  // For each time point
369  for (const auto &t : r)
370  {
371  // Print each of the properties at that time point
372  for (const auto &iter : t)
373  {
374  out << iter.second << ",";
375  }
376 
377  // Separate time points by semicolons
378  out << ";";
379  }
380 
381  // Separate runs by newlines
382  out << std::endl;
383  }
384  }
385 
386  out << '.' << std::endl;
387  }
388  return true;
389 }
390 
392 {
393  // sanity checks
394  if (gsetup_)
395  {
396  if (!gsetup_->getSpaceInformation()->isSetup())
397  gsetup_->getSpaceInformation()->setup();
398  }
399  else
400  {
401  if (!csetup_->getSpaceInformation()->isSetup())
402  csetup_->getSpaceInformation()->setup();
403  }
404 
405  if (!(gsetup_ ? gsetup_->getGoal() : csetup_->getGoal()))
406  {
407  OMPL_ERROR("No goal defined");
408  return;
409  }
410 
411  if (planners_.empty())
412  {
413  OMPL_ERROR("There are no planners to benchmark");
414  return;
415  }
416 
417  status_.running = true;
418  exp_.totalDuration = 0.0;
419  exp_.maxTime = req.maxTime;
420  exp_.maxMem = req.maxMem;
421  exp_.runCount = req.runCount;
422  exp_.host = machine::getHostname();
423  exp_.cpuInfo = machine::getCPUInfo();
424  exp_.seed = RNG::getSeed();
425 
426  exp_.startTime = time::now();
427 
428  OMPL_INFORM("Configuring planners ...");
429 
430  // clear previous experimental data
431  exp_.planners.clear();
432  exp_.planners.resize(planners_.size());
433 
434  const base::ProblemDefinitionPtr &pdef =
435  gsetup_ ? gsetup_->getProblemDefinition() : csetup_->getProblemDefinition();
436  // set up all the planners
437  for (unsigned int i = 0; i < planners_.size(); ++i)
438  {
439  // configure the planner
440  planners_[i]->setProblemDefinition(pdef);
441  if (!planners_[i]->isSetup())
442  planners_[i]->setup();
443  exp_.planners[i].name = (gsetup_ ? "geometric_" : "control_") + planners_[i]->getName();
444  OMPL_INFORM("Configured %s", exp_.planners[i].name.c_str());
445  }
446 
447  OMPL_INFORM("Done configuring planners.");
448  OMPL_INFORM("Saving planner setup information ...");
449 
450  std::stringstream setupInfo;
451  if (gsetup_)
452  gsetup_->print(setupInfo);
453  else
454  csetup_->print(setupInfo);
455  setupInfo << std::endl << "Properties of benchmarked planners:" << std::endl;
456  for (auto &planner : planners_)
457  planner->printProperties(setupInfo);
458 
459  exp_.setupInfo = setupInfo.str();
460 
461  OMPL_INFORM("Done saving information");
462 
463  OMPL_INFORM("Beginning benchmark");
465  boost::scoped_ptr<msg::OutputHandlerFile> ohf;
466  if (req.saveConsoleOutput)
467  {
468  ohf.reset(new msg::OutputHandlerFile(getConsoleFilename(exp_).c_str()));
469  msg::useOutputHandler(ohf.get());
470  }
471  else
473  OMPL_INFORM("Beginning benchmark");
474 
475  boost::scoped_ptr<boost::progress_display> progress;
476  if (req.displayProgress)
477  {
478  std::cout << "Running experiment " << exp_.name << "." << std::endl;
479  std::cout << "Each planner will be executed " << req.runCount << " times for at most " << req.maxTime
480  << " seconds. Memory is limited at " << req.maxMem << "MB." << std::endl;
481  progress.reset(new boost::progress_display(100, std::cout));
482  }
483 
485  auto maxMemBytes = (machine::MemUsage_t)(req.maxMem * 1024 * 1024);
486 
487  for (unsigned int i = 0; i < planners_.size(); ++i)
488  {
489  status_.activePlanner = exp_.planners[i].name;
490  // execute planner switch event, if set
491  try
492  {
493  if (plannerSwitch_)
494  {
495  OMPL_INFORM("Executing planner-switch event for planner %s ...", status_.activePlanner.c_str());
496  plannerSwitch_(planners_[i]);
497  OMPL_INFORM("Completed execution of planner-switch event");
498  }
499  }
500  catch (std::runtime_error &e)
501  {
502  std::stringstream es;
503  es << "There was an error executing the planner-switch event for planner " << status_.activePlanner
504  << std::endl;
505  es << "*** " << e.what() << std::endl;
506  std::cerr << es.str();
507  OMPL_ERROR(es.str().c_str());
508  }
509  if (gsetup_)
510  gsetup_->setup();
511  else
512  csetup_->setup();
513  planners_[i]->params().getParams(exp_.planners[i].common);
514  planners_[i]->getSpaceInformation()->params().getParams(exp_.planners[i].common);
515 
516  // Add planner progress property names to struct
517  exp_.planners[i].progressPropertyNames.emplace_back("time REAL");
518  base::Planner::PlannerProgressProperties::const_iterator iter;
519  for (iter = planners_[i]->getPlannerProgressProperties().begin();
520  iter != planners_[i]->getPlannerProgressProperties().end(); ++iter)
521  {
522  exp_.planners[i].progressPropertyNames.push_back(iter->first);
523  }
524  std::sort(exp_.planners[i].progressPropertyNames.begin(), exp_.planners[i].progressPropertyNames.end());
525 
526  // run the planner
527  for (unsigned int j = 0; j < req.runCount; ++j)
528  {
529  status_.activeRun = j;
530  status_.progressPercentage =
531  (double)(100 * (req.runCount * i + j)) / (double)(planners_.size() * req.runCount);
532 
533  if (req.displayProgress)
534  while (status_.progressPercentage > progress->count())
535  ++(*progress);
536 
537  OMPL_INFORM("Preparing for run %d of %s", status_.activeRun, status_.activePlanner.c_str());
538 
539  // make sure all planning data structures are cleared
540  try
541  {
542  planners_[i]->clear();
543  if (gsetup_)
544  {
545  gsetup_->getProblemDefinition()->clearSolutionPaths();
546  gsetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
547  }
548  else
549  {
550  csetup_->getProblemDefinition()->clearSolutionPaths();
551  csetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
552  }
553  }
554  catch (std::runtime_error &e)
555  {
556  std::stringstream es;
557  es << "There was an error while preparing for run " << status_.activeRun << " of planner "
558  << status_.activePlanner << std::endl;
559  es << "*** " << e.what() << std::endl;
560  std::cerr << es.str();
561  OMPL_ERROR(es.str().c_str());
562  }
563 
564  // execute pre-run event, if set
565  try
566  {
567  if (preRun_)
568  {
569  OMPL_INFORM("Executing pre-run event for run %d of planner %s ...", status_.activeRun,
570  status_.activePlanner.c_str());
571  preRun_(planners_[i]);
572  OMPL_INFORM("Completed execution of pre-run event");
573  }
574  }
575  catch (std::runtime_error &e)
576  {
577  std::stringstream es;
578  es << "There was an error executing the pre-run event for run " << status_.activeRun << " of planner "
579  << status_.activePlanner << std::endl;
580  es << "*** " << e.what() << std::endl;
581  std::cerr << es.str();
582  OMPL_ERROR(es.str().c_str());
583  }
584 
585  RunPlanner rp(this, req.useThreads);
586  rp.run(planners_[i], memStart, maxMemBytes, req.maxTime, req.timeBetweenUpdates);
587  bool solved = gsetup_ ? gsetup_->haveSolutionPath() : csetup_->haveSolutionPath();
588 
589  // store results
590  try
591  {
592  RunProperties run;
593 
594  run["time REAL"] = ompl::toString(rp.getTimeUsed());
595  run["memory REAL"] = ompl::toString((double)rp.getMemUsed() / (1024.0 * 1024.0));
596  run["status ENUM"] = std::to_string((int)static_cast<base::PlannerStatus::StatusType>(rp.getStatus()));
597  if (gsetup_)
598  {
599  run["solved BOOLEAN"] = std::to_string(gsetup_->haveExactSolutionPath());
600  run["valid segment fraction REAL"] =
601  ompl::toString(gsetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
602  }
603  else
604  {
605  run["solved BOOLEAN"] = std::to_string(csetup_->haveExactSolutionPath());
606  run["valid segment fraction REAL"] =
607  ompl::toString(csetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
608  }
609 
610  if (solved)
611  {
612  if (gsetup_)
613  {
614  run["approximate solution BOOLEAN"] =
615  std::to_string(gsetup_->getProblemDefinition()->hasApproximateSolution());
616  run["solution difference REAL"] =
617  ompl::toString(gsetup_->getProblemDefinition()->getSolutionDifference());
618  run["solution length REAL"] = ompl::toString(gsetup_->getSolutionPath().length());
619  run["solution smoothness REAL"] = ompl::toString(gsetup_->getSolutionPath().smoothness());
620  run["solution clearance REAL"] = ompl::toString(gsetup_->getSolutionPath().clearance());
621  run["solution segments INTEGER"] =
622  std::to_string(gsetup_->getSolutionPath().getStateCount() - 1);
623  run["correct solution BOOLEAN"] = std::to_string(gsetup_->getSolutionPath().check());
624 
625  unsigned int factor = gsetup_->getStateSpace()->getValidSegmentCountFactor();
626  gsetup_->getStateSpace()->setValidSegmentCountFactor(factor * 4);
627  run["correct solution strict BOOLEAN"] = std::to_string(gsetup_->getSolutionPath().check());
628  gsetup_->getStateSpace()->setValidSegmentCountFactor(factor);
629 
630  if (req.simplify)
631  {
632  // simplify solution
633  time::point timeStart = time::now();
634  gsetup_->simplifySolution();
635  double timeUsed = time::seconds(time::now() - timeStart);
636  run["simplification time REAL"] = ompl::toString(timeUsed);
637  run["simplified solution length REAL"] =
638  ompl::toString(gsetup_->getSolutionPath().length());
639  run["simplified solution smoothness REAL"] =
640  ompl::toString(gsetup_->getSolutionPath().smoothness());
641  run["simplified solution clearance REAL"] =
642  ompl::toString(gsetup_->getSolutionPath().clearance());
643  run["simplified solution segments INTEGER"] =
644  std::to_string(gsetup_->getSolutionPath().getStateCount() - 1);
645  run["simplified correct solution BOOLEAN"] =
646  std::to_string(gsetup_->getSolutionPath().check());
647  gsetup_->getStateSpace()->setValidSegmentCountFactor(factor * 4);
648  run["simplified correct solution strict BOOLEAN"] =
649  std::to_string(gsetup_->getSolutionPath().check());
650  gsetup_->getStateSpace()->setValidSegmentCountFactor(factor);
651  }
652  }
653  else
654  {
655  run["approximate solution BOOLEAN"] =
656  std::to_string(csetup_->getProblemDefinition()->hasApproximateSolution());
657  run["solution difference REAL"] =
658  ompl::toString(csetup_->getProblemDefinition()->getSolutionDifference());
659  run["solution length REAL"] = ompl::toString(csetup_->getSolutionPath().length());
660  run["solution clearance REAL"] =
661  ompl::toString(csetup_->getSolutionPath().asGeometric().clearance());
662  run["solution segments INTEGER"] = std::to_string(csetup_->getSolutionPath().getControlCount());
663  run["correct solution BOOLEAN"] = std::to_string(csetup_->getSolutionPath().check());
664  }
665  }
666 
667  base::PlannerData pd(gsetup_ ? gsetup_->getSpaceInformation() : csetup_->getSpaceInformation());
668  planners_[i]->getPlannerData(pd);
669  run["graph states INTEGER"] = std::to_string(pd.numVertices());
670  run["graph motions INTEGER"] = std::to_string(pd.numEdges());
671 
672  for (const auto &prop : pd.properties)
673  run[prop.first] = prop.second;
674 
675  // execute post-run event, if set
676  try
677  {
678  if (postRun_)
679  {
680  OMPL_INFORM("Executing post-run event for run %d of planner %s ...", status_.activeRun,
681  status_.activePlanner.c_str());
682  postRun_(planners_[i], run);
683  OMPL_INFORM("Completed execution of post-run event");
684  }
685  }
686  catch (std::runtime_error &e)
687  {
688  std::stringstream es;
689  es << "There was an error in the execution of the post-run event for run " << status_.activeRun
690  << " of planner " << status_.activePlanner << std::endl;
691  es << "*** " << e.what() << std::endl;
692  std::cerr << es.str();
693  OMPL_ERROR(es.str().c_str());
694  }
695 
696  exp_.planners[i].runs.push_back(run);
697 
698  // Add planner progress data from the planner progress
699  // collector if there was anything to report
700  if (planners_[i]->getPlannerProgressProperties().size() > 0)
701  {
702  exp_.planners[i].runsProgressData.push_back(rp.getRunProgressData());
703  }
704  }
705  catch (std::runtime_error &e)
706  {
707  std::stringstream es;
708  es << "There was an error in the extraction of planner results: planner = " << status_.activePlanner
709  << ", run = " << status_.activePlanner << std::endl;
710  es << "*** " << e.what() << std::endl;
711  std::cerr << es.str();
712  OMPL_ERROR(es.str().c_str());
713  }
714  }
715  }
716 
717  status_.running = false;
718  status_.progressPercentage = 100.0;
719  if (req.displayProgress)
720  {
721  while (status_.progressPercentage > progress->count())
722  ++(*progress);
723  std::cout << std::endl;
724  }
725 
726  exp_.totalDuration = time::seconds(time::now() - exp_.startTime);
727 
728  OMPL_INFORM("Benchmark complete");
730  OMPL_INFORM("Benchmark complete");
731 }
std::string getHostname()
Get the hostname of the machine in use.
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 ...
Definition: Planner.h:413
MemUsage_t getProcessMemoryUsage()
Get the amount of memory the current process is using. This should work on major platforms (Windows,...
void noOutputHandler()
This function instructs ompl that no messages should be outputted. Equivalent to useOutputHandler(nul...
Definition: Console.cpp:95
Generic class to handle output from a piece of code.
Definition: Console.h:102
void useOutputHandler(OutputHandler *oh)
Specify the instance of the OutputHandler to use. By default, this is OutputHandlerSTD.
Definition: Console.cpp:108
CompleteExperiment exp_
The collected experimental data (for all planners)
Definition: Benchmark.h:439
std::string asString() const
Return a string representation.
@ TYPE_COUNT
The number of possible status values.
point now()
Get the current time point.
Definition: Time.h:121
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:115
unsigned int numEdges() const
Retrieve the number of edges in this structure.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
bool simplify
flag indicating whether simplification should be applied to path; true by default
Definition: Benchmark.h:295
bool useThreads
flag indicating whether planner runs should be run in a separate thread. It is advisable to set this ...
Definition: Benchmark.h:292
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
unsigned int runCount
the number of times to run each planner; 100 by default
Definition: Benchmark.h:276
bool saveConsoleOutput
flag indicating whether console output is saved (in an automatically generated filename); true by def...
Definition: Benchmark.h:287
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
bool displayProgress
flag indicating whether progress is to be displayed or not; true by default
Definition: Benchmark.h:283
bool saveResultsToFile() const
Save the results of the benchmark to a file. The name of the file is the current date and time.
Definition: Benchmark.cpp:258
A class to store the exit status of Planner::solve()
Representation of a benchmark request.
Definition: Benchmark.h:252
std::map< std::string, std::string > RunProperties
The data collected from a run of a planner is stored as key-value pairs.
Definition: Benchmark.h:175
Implementation of OutputHandler that saves messages in a file.
Definition: Console.h:125
static std::uint_fast32_t getSeed()
Get the seed used to generate the seeds of each RNG instance. Passing the returned value to setSeed()...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:474
virtual void benchmark(const Request &req)
Benchmark the added planners on the defined problem. Repeated calls clear previously gathered data.
Definition: Benchmark.cpp:391
std::string getCPUInfo()
Get information about the CPU of the machine in use.
StatusType
The possible values of the status returned by a planner.
double maxMem
the maximum amount of memory a planner is allowed to use (MB); 4096.0 by default
Definition: Benchmark.h:273
std::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner,...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
unsigned long long MemUsage_t
Amount of memory used, in bytes.
Definition: MachineSpecs.h:112
std::string as_string(const point &p)
Return string representation of point in time.
Definition: Time.h:141
OutputHandler * getOutputHandler()
Get the instance of the OutputHandler currently used. This is nullptr in case there is no output hand...
Definition: Console.cpp:115
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:127
virtual bool saveResultsToStream(std::ostream &out=std::cout) const
Save the results of the benchmark to a stream.
Definition: Benchmark.cpp:264
double timeBetweenUpdates
When collecting time-varying data from a planner during its execution, the planner's progress will be...
Definition: Benchmark.h:280
Main namespace. Contains everything in this library.
double maxTime
the maximum amount of time a planner is allowed to run (seconds); 5.0 by default
Definition: Benchmark.h:270
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition: Time.h:118