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