All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 <boost/scoped_ptr.hpp>
41 #include <boost/lexical_cast.hpp>
42 #include <boost/progress.hpp>
43 #include <boost/thread.hpp>
44 #include <fstream>
45 #include <sstream>
46 
48 namespace ompl
49 {
50  namespace tools
51  {
53  static std::string getResultsFilename(const Benchmark::CompleteExperiment &exp)
54  {
55  return "ompl_" + exp.host + "_" + boost::posix_time::to_iso_extended_string(exp.startTime) + ".log";
56  }
57 
59  static std::string getConsoleFilename(const Benchmark::CompleteExperiment &exp)
60  {
61  return "ompl_" + exp.host + "_" + boost::posix_time::to_iso_extended_string(exp.startTime) + ".console";
62  }
63 
64  static bool terminationCondition(const machine::MemUsage_t maxMem, const time::point &endTime)
65  {
66  if (time::now() < endTime && machine::getProcessMemoryUsage() < maxMem)
67  return false;
68  return true;
69  }
70 
71  class RunPlanner
72  {
73  public:
74 
75  RunPlanner(const Benchmark *benchmark, bool useThreads)
76  : benchmark_(benchmark), timeUsed_(0.0), memUsed_(0), useThreads_(useThreads)
77  {
78  }
79 
80  void run(const base::PlannerPtr &planner, const machine::MemUsage_t memStart, const machine::MemUsage_t maxMem, const double maxTime, const double timeBetweenUpdates)
81  {
82  if (!useThreads_)
83  {
84  runThread(planner, memStart + maxMem, time::seconds(maxTime), time::seconds(timeBetweenUpdates));
85  return;
86  }
87 
88  boost::thread t(boost::bind(&RunPlanner::runThread, this, planner, memStart + maxMem, time::seconds(maxTime), time::seconds(timeBetweenUpdates)));
89 
90  // allow 25% more time than originally specified, in order to detect planner termination
91 #if BOOST_VERSION < 105000
92  // For older versions of boost, we have to use this
93  // deprecated form of the timed join
94  if (!t.timed_join(time::seconds(maxTime * 1.25)))
95 #else
96  if (!t.try_join_for(boost::chrono::duration<double>(maxTime * 1.25)))
97 #endif
98  {
100 
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();
105  OMPL_ERROR(es.str().c_str());
106 
107  t.interrupt();
108  t.join();
109 
110  std::string m = "Planning thread cancelled";
111  std::cerr << m << std::endl;
112  OMPL_ERROR(m.c_str());
113  }
114 
115  if (memStart < memUsed_)
116  memUsed_ -= memStart;
117  else
118  memUsed_ = 0;
119  }
120 
121  double getTimeUsed(void) const
122  {
123  return timeUsed_;
124  }
125 
126  machine::MemUsage_t getMemUsed(void) const
127  {
128  return memUsed_;
129  }
130 
131  base::PlannerStatus getStatus(void) const
132  {
133  return status_;
134  }
135 
136  const Benchmark::RunProgressData& getRunProgressData(void) const
137  {
138  return runProgressData_;
139  }
140 
141  private:
142 
143  void runThread(const base::PlannerPtr &planner, const machine::MemUsage_t maxMem, const time::duration &maxDuration, const time::duration &timeBetweenUpdates)
144  {
145  time::point timeStart = time::now();
146 
147  try
148  {
149  base::PlannerTerminationConditionFn ptc = boost::bind(&terminationCondition, maxMem, time::now() + maxDuration);
150  solved_ = false;
151  // Only launch the planner progress property
152  // collector if there is any data for it to report
153  //
154  // \TODO issue here is that at least one sample
155  // always gets taken before planner even starts;
156  // might be worth adding a short wait time before
157  // collector begins sampling
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);
164  solvedFlag_.lock();
165  solved_ = true;
166  solvedCondition_.notify_all();
167  solvedFlag_.unlock();
168  if (t)
169  t->join(); // maybe look into interrupting even if planner throws an exception
170  }
171  catch(std::runtime_error &e)
172  {
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();
177  OMPL_ERROR(es.str().c_str());
178  }
179 
180  timeUsed_ = time::seconds(time::now() - timeStart);
181  memUsed_ = machine::getProcessMemoryUsage();
182  }
183 
184  void collectProgressProperties(const base::Planner::PlannerProgressProperties& properties,
185  const time::duration &timePerUpdate)
186  {
187  time::point timeStart = time::now();
188 
189  boost::unique_lock<boost::mutex> ulock(solvedFlag_);
190  while (!solved_)
191  {
192  if (solvedCondition_.timed_wait(ulock, time::now() + timePerUpdate))
193  return;
194  else
195  {
196  double timeInSeconds = time::seconds(time::now() - timeStart);
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();
203  ++item)
204  {
205  data[item->first] = item->second();
206  }
207  runProgressData_.push_back(data);
208  }
209  }
210  }
211 
212  const Benchmark *benchmark_;
213  double timeUsed_;
214  machine::MemUsage_t memUsed_;
215  base::PlannerStatus status_;
216  bool useThreads_;
217  Benchmark::RunProgressData runProgressData_;
218 
219  // variables needed for progress property collection
220  bool solved_;
221  boost::mutex solvedFlag_;
222  boost::condition_variable solvedCondition_;
223  };
224 
225  }
226 }
228 
229 bool ompl::tools::Benchmark::saveResultsToFile(const char *filename) const
230 {
231  bool result = false;
232 
233  std::ofstream fout(filename);
234  if (fout.good())
235  {
236  result = saveResultsToStream(fout);
237  OMPL_INFORM("Results saved to '%s'", filename);
238  }
239  else
240  {
241  // try to save to a different file, if we can
242  if (getResultsFilename(exp_) != std::string(filename))
243  result = saveResultsToFile();
244 
245  OMPL_ERROR("Unable to write results to '%s'", filename);
246  }
247  return result;
248 }
249 
251 {
252  std::string filename = getResultsFilename(exp_);
253  return saveResultsToFile(filename.c_str());
254 }
255 
256 bool ompl::tools::Benchmark::saveResultsToStream(std::ostream &out) const
257 {
258  if (exp_.planners.empty())
259  {
260  OMPL_WARN("There is no experimental data to save");
261  return false;
262  }
263 
264  if (!out.good())
265  {
266  OMPL_ERROR("Unable to write to stream");
267  return false;
268  }
269 
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;
274 
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;
280 
281  // change this if more enum types are added
282  out << "1 enum type" << std::endl;
283  out << "status";
284  for (unsigned int i = 0 ; i < base::PlannerStatus::TYPE_COUNT ; ++i)
285  out << '|' << base::PlannerStatus(static_cast<base::PlannerStatus::StatusType>(i)).asString();
286  out << std::endl;
287 
288  out << exp_.planners.size() << " planners" << std::endl;
289 
290  for (unsigned int i = 0 ; i < exp_.planners.size() ; ++i)
291  {
292  out << exp_.planners[i].name << std::endl;
293 
294  // get names of common properties
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());
300 
301  // print names & values of common properties
302  out << properties.size() << " common properties" << std::endl;
303  for (unsigned int k = 0 ; k < properties.size() ; ++k)
304  {
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;
307  }
308 
309  // construct the list of all possible properties for all runs
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;
315 
316  properties.clear();
317 
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());
321 
322  // print the property names
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;
326 
327  // print the data for each run
328  out << exp_.planners[i].runs.size() << " runs" << std::endl;
329  for (unsigned int j = 0 ; j < exp_.planners[i].runs.size() ; ++j)
330  {
331  for (unsigned int k = 0 ; k < properties.size() ; ++k)
332  {
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())
335  out << it->second;
336  out << "; ";
337  }
338  out << std::endl;
339  }
340 
341  // print the run progress data if it was reported
342  if (exp_.planners[i].runsProgressData.size() > 0)
343  {
344  // Print number of progress properties
345  out << exp_.planners[i].progressPropertyNames.size() << " progress properties for each run" << std::endl;
346  // Print progress property names
347  for (std::vector<std::string>::const_iterator iter =
348  exp_.planners[i].progressPropertyNames.begin();
349  iter != exp_.planners[i].progressPropertyNames.end();
350  ++iter)
351  {
352  out << *iter << std::endl;
353  }
354  // Print progress properties for each run
355  out << exp_.planners[i].runsProgressData.size() << " runs" << std::endl;
356  for (std::size_t r = 0; r < exp_.planners[i].runsProgressData.size(); ++r)
357  {
358  // For each time point
359  for (std::size_t t = 0; t < exp_.planners[i].runsProgressData[r].size(); ++t)
360  {
361  // Print each of the properties at that time point
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();
365  ++iter)
366  {
367  out << iter->second << ",";
368  }
369 
370  // Separate time points by semicolons
371  out << ";";
372  }
373 
374  // Separate runs by newlines
375  out << std::endl;
376  }
377  }
378 
379  out << '.' << std::endl;
380  }
381  return true;
382 }
383 
385 {
386  // sanity checks
387  if (gsetup_)
388  {
389  if (!gsetup_->getSpaceInformation()->isSetup())
390  gsetup_->getSpaceInformation()->setup();
391  }
392  else
393  {
394  if (!csetup_->getSpaceInformation()->isSetup())
395  csetup_->getSpaceInformation()->setup();
396  }
397 
398  if (!(gsetup_ ? gsetup_->getGoal() : csetup_->getGoal()))
399  {
400  OMPL_ERROR("No goal defined");
401  return;
402  }
403 
404  if (planners_.empty())
405  {
406  OMPL_ERROR("There are no planners to benchmark");
407  return;
408  }
409 
410  status_.running = true;
411  exp_.totalDuration = 0.0;
412  exp_.maxTime = req.maxTime;
413  exp_.maxMem = req.maxMem;
414  exp_.runCount = req.runCount;
415  exp_.host = machine::getHostname();
416  exp_.seed = RNG::getSeed();
417 
418  exp_.startTime = time::now();
419 
420  OMPL_INFORM("Configuring planners ...");
421 
422  // clear previous experimental data
423  exp_.planners.clear();
424  exp_.planners.resize(planners_.size());
425 
426  const base::ProblemDefinitionPtr &pdef = gsetup_ ? gsetup_->getProblemDefinition() : csetup_->getProblemDefinition();
427  // set up all the planners
428  for (unsigned int i = 0 ; i < planners_.size() ; ++i)
429  {
430  // configure the planner
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());
436  }
437 
438  OMPL_INFORM("Done configuring planners.");
439  OMPL_INFORM("Saving planner setup information ...");
440 
441  std::stringstream setupInfo;
442  if (gsetup_)
443  gsetup_->print(setupInfo);
444  else
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);
449 
450  exp_.setupInfo = setupInfo.str();
451 
452  OMPL_INFORM("Done saving information");
453 
454  OMPL_INFORM("Beginning benchmark");
456  boost::scoped_ptr<msg::OutputHandlerFile> ohf;
457  if (req.saveConsoleOutput)
458  {
459  ohf.reset(new msg::OutputHandlerFile(getConsoleFilename(exp_).c_str()));
460  msg::useOutputHandler(ohf.get());
461  }
462  else
464  OMPL_INFORM("Beginning benchmark");
465 
466  boost::scoped_ptr<boost::progress_display> progress;
467  if (req.displayProgress)
468  {
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));
473  }
474 
476  machine::MemUsage_t maxMemBytes = (machine::MemUsage_t)(req.maxMem * 1024 * 1024);
477 
478  for (unsigned int i = 0 ; i < planners_.size() ; ++i)
479  {
480  status_.activePlanner = exp_.planners[i].name;
481  // execute planner switch event, if set
482  try
483  {
484  if (plannerSwitch_)
485  {
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");
489  }
490  }
491  catch(std::runtime_error &e)
492  {
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();
497  OMPL_ERROR(es.str().c_str());
498  }
499  if (gsetup_)
500  gsetup_->setup();
501  else
502  csetup_->setup();
503  planners_[i]->params().getParams(exp_.planners[i].common);
504  planners_[i]->getSpaceInformation()->params().getParams(exp_.planners[i].common);
505 
506  // Add planner progress property names to struct
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();
511  ++iter)
512  {
513  exp_.planners[i].progressPropertyNames.push_back(iter->first);
514  }
515  std::sort(exp_.planners[i].progressPropertyNames.begin(),
516  exp_.planners[i].progressPropertyNames.end());
517 
518  // run the planner
519  for (unsigned int j = 0 ; j < req.runCount ; ++j)
520  {
521  status_.activeRun = j;
522  status_.progressPercentage = (double)(100 * (req.runCount * i + j)) / (double)(planners_.size() * req.runCount);
523 
524  if (req.displayProgress)
525  while (status_.progressPercentage > progress->count())
526  ++(*progress);
527 
528  OMPL_INFORM("Preparing for run %d of %s", status_.activeRun, status_.activePlanner.c_str());
529 
530  // make sure all planning data structures are cleared
531  try
532  {
533  planners_[i]->clear();
534  if (gsetup_)
535  {
536  gsetup_->getProblemDefinition()->clearSolutionPaths();
537  gsetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
538  }
539  else
540  {
541  csetup_->getProblemDefinition()->clearSolutionPaths();
542  csetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
543  }
544  }
545  catch(std::runtime_error &e)
546  {
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();
551  OMPL_ERROR(es.str().c_str());
552  }
553 
554  // execute pre-run event, if set
555  try
556  {
557  if (preRun_)
558  {
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");
562  }
563  }
564  catch(std::runtime_error &e)
565  {
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();
570  OMPL_ERROR(es.str().c_str());
571  }
572 
573  RunPlanner rp(this, req.useThreads);
574  rp.run(planners_[i], memStart, maxMemBytes, req.maxTime, req.timeBetweenUpdates);
575  bool solved = gsetup_ ? gsetup_->haveSolutionPath() : csetup_->haveSolutionPath();
576 
577  // store results
578  try
579  {
580  RunProperties run;
581 
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()));
585  if (gsetup_)
586  {
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());
589  }
590  else
591  {
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());
594  }
595 
596  if (solved)
597  {
598  if (gsetup_)
599  {
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());
607 
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);
612 
613  // simplify solution
614  time::point timeStart = time::now();
615  gsetup_->simplifySolution();
616  double timeUsed = time::seconds(time::now() - timeStart);
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);
626  }
627  else
628  {
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());
635  }
636  }
637 
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());
642 
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;
645 
646  // execute post-run event, if set
647  try
648  {
649  if (postRun_)
650  {
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");
654  }
655  }
656  catch(std::runtime_error &e)
657  {
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();
662  OMPL_ERROR(es.str().c_str());
663  }
664 
665  exp_.planners[i].runs.push_back(run);
666 
667  // Add planner progress data from the planner progress
668  // collector if there was anything to report
669  if (planners_[i]->getPlannerProgressProperties().size() > 0)
670  {
671  exp_.planners[i].runsProgressData.push_back(rp.getRunProgressData());
672  }
673  }
674  catch(std::runtime_error &e)
675  {
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();
680  OMPL_ERROR(es.str().c_str());
681  }
682  }
683  }
684 
685  status_.running = false;
686  status_.progressPercentage = 100.0;
687  if (req.displayProgress)
688  {
689  while (status_.progressPercentage > progress->count())
690  ++(*progress);
691  std::cout << std::endl;
692  }
693 
694  exp_.totalDuration = time::seconds(time::now() - exp_.startTime);
695 
696  OMPL_INFORM("Benchmark complete");
698  OMPL_INFORM("Benchmark complete");
699 }
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
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:169
double timeBetweenUpdates
When collecting time-varying data from a planner during its execution, the planner's progress will be...
Definition: Benchmark.h:175
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
bool displayProgress
flag indicating whether progress is to be displayed or not; true by default
Definition: Benchmark.h:178
unsigned int runCount
the number of times to run each planner; 100 by default
Definition: Benchmark.h:172
bool saveConsoleOutput
flag indicating whether console output is saved (in an automatically generated filename); true by def...
Definition: Benchmark.h:181
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:184
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
bool saveResultsToFile(void) const
Save the results of the benchmark to a file. The name of the file is the current date and time...
Definition: Benchmark.cpp:250
double maxTime
the maximum amount of time a planner is allowed to run (seconds); 5.0 by default
Definition: Benchmark.h:166
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
std::string getHostname(void)
Get the hostname of the machine in use.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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...
Representation of a benchmark request.
Definition: Benchmark.h:151
virtual bool saveResultsToStream(std::ostream &out=std::cout) const
Save the results of the benchmark to a stream.
Definition: Benchmark.cpp:256
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
void useOutputHandler(OutputHandler *oh)
Specify the instance of the OutputHandler to use. By default, this is OutputHandlerSTD.
Definition: Console.cpp:92
OutputHandler * getOutputHandler(void)
Get the instance of the OutputHandler currently used. This is NULL in case there is no output handler...
Definition: Console.cpp:99
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:351
CompleteExperiment exp_
The collected experimental data (for all planners)
Definition: Benchmark.h:303
void noOutputHandler(void)
This function instructs ompl that no messages should be outputted. Equivalent to useOutputHandler(NUL...
Definition: Console.cpp:79
point now(void)
Get the current time point.
Definition: Time.h:56
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:384
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68