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 */
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 <fstream>
44 #include <sstream>
45 
47 namespace ompl
48 {
49  namespace tools
50  {
52  static std::string getResultsFilename(const Benchmark::CompleteExperiment &exp)
53  {
54  return "ompl_" + exp.host + "_" + boost::posix_time::to_iso_extended_string(exp.startTime) + ".log";
55  }
56 
58  static std::string getConsoleFilename(const Benchmark::CompleteExperiment &exp)
59  {
60  return "ompl_" + exp.host + "_" + boost::posix_time::to_iso_extended_string(exp.startTime) + ".console";
61  }
62 
63  static bool terminationCondition(const machine::MemUsage_t maxMem, const time::point &endTime)
64  {
65  if (time::now() < endTime && machine::getProcessMemoryUsage() < maxMem)
66  return false;
67  return true;
68  }
69 
70  class RunPlanner
71  {
72  public:
73 
74  RunPlanner(const Benchmark *benchmark, bool useThreads)
75  : benchmark_(benchmark), timeUsed_(0.0), memUsed_(0), useThreads_(useThreads)
76  {
77  }
78 
79  void run(const base::PlannerPtr &planner, const machine::MemUsage_t memStart, const machine::MemUsage_t maxMem, const double maxTime)
80  {
81  if (!useThreads_)
82  {
83  runThread(planner, memStart + maxMem, time::seconds(maxTime));
84  return;
85  }
86 
87  boost::thread t(boost::bind(&RunPlanner::runThread, this, planner, memStart + maxMem, time::seconds(maxTime)));
88 
89  // allow 25% more time than originally specified, in order to detect planner termination
90  if (!t.timed_join(time::seconds(maxTime * 1.25)))
91  {
93 
94  std::stringstream es;
95  es << "Planner " << benchmark_->getStatus().activePlanner << " did not complete run " << benchmark_->getStatus().activeRun
96  << " within the specified amount of time (possible crash). Attempting to force termination of planning thread ..." << std::endl;
97  std::cerr << es.str();
98  logError(es.str().c_str());
99 
100  t.interrupt();
101  t.join();
102 
103  std::string m = "Planning thread cancelled";
104  std::cerr << m << std::endl;
105  logError(m.c_str());
106  }
107 
108  if (memStart < memUsed_)
109  memUsed_ -= memStart;
110  else
111  memUsed_ = 0;
112  }
113 
114  double getTimeUsed(void) const
115  {
116  return timeUsed_;
117  }
118 
119  machine::MemUsage_t getMemUsed(void) const
120  {
121  return memUsed_;
122  }
123 
124  base::PlannerStatus getStatus(void) const
125  {
126  return status_;
127  }
128 
129  private:
130 
131  void runThread(const base::PlannerPtr &planner, const machine::MemUsage_t maxMem, const time::duration &maxDuration)
132  {
133  time::point timeStart = time::now();
134 
135  try
136  {
137  base::PlannerTerminationConditionFn ptc = boost::bind(&terminationCondition, maxMem, time::now() + maxDuration);
138  status_ = planner->solve(ptc, 0.1);
139  }
140  catch(std::runtime_error &e)
141  {
142  std::stringstream es;
143  es << "There was an error executing planner " << benchmark_->getStatus().activePlanner << ", run = " << benchmark_->getStatus().activeRun << std::endl;
144  es << "*** " << e.what() << std::endl;
145  std::cerr << es.str();
146  logError(es.str().c_str());
147  }
148 
149  timeUsed_ = time::seconds(time::now() - timeStart);
150  memUsed_ = machine::getProcessMemoryUsage();
151  }
152 
153  const Benchmark *benchmark_;
154  double timeUsed_;
155  machine::MemUsage_t memUsed_;
156  base::PlannerStatus status_;
157  bool useThreads_;
158  };
159 
160  }
161 }
163 
164 bool ompl::tools::Benchmark::saveResultsToFile(const char *filename) const
165 {
166  bool result = false;
167 
168  std::ofstream fout(filename);
169  if (fout.good())
170  {
171  result = saveResultsToStream(fout);
172  logInform("Results saved to '%s'", filename);
173  }
174  else
175  {
176  // try to save to a different file, if we can
177  if (getResultsFilename(exp_) != std::string(filename))
178  result = saveResultsToFile();
179 
180  logError("Unable to write results to '%s'", filename);
181  }
182  return result;
183 }
184 
186 {
187  std::string filename = getResultsFilename(exp_);
188  return saveResultsToFile(filename.c_str());
189 }
190 
191 bool ompl::tools::Benchmark::saveResultsToStream(std::ostream &out) const
192 {
193  if (exp_.planners.empty())
194  {
195  logWarn("There is no experimental data to save");
196  return false;
197  }
198 
199  if (!out.good())
200  {
201  logError("Unable to write to stream");
202  return false;
203  }
204 
205  out << "Experiment " << (exp_.name.empty() ? "NO_NAME" : exp_.name) << std::endl;
206  out << "Running on " << (exp_.host.empty() ? "UNKNOWN" : exp_.host) << std::endl;
207  out << "Starting at " << boost::posix_time::to_iso_extended_string(exp_.startTime) << std::endl;
208  out << "<<<|" << std::endl << exp_.setupInfo << "|>>>" << std::endl;
209 
210  out << exp_.seed << " is the random seed" << std::endl;
211  out << exp_.maxTime << " seconds per run" << std::endl;
212  out << exp_.maxMem << " MB per run" << std::endl;
213  out << exp_.runCount << " runs per planner" << std::endl;
214  out << exp_.totalDuration << " seconds spent to collect the data" << std::endl;
215 
216  // change this if more enum types are added
217  out << "1 enum type" << std::endl;
218  out << "status";
219  for (unsigned int i = 0 ; i < base::PlannerStatus::TYPE_COUNT ; ++i)
220  out << '|' << base::PlannerStatus(static_cast<base::PlannerStatus::StatusType>(i)).asString();
221  out << std::endl;
222 
223  out << exp_.planners.size() << " planners" << std::endl;
224 
225  for (unsigned int i = 0 ; i < exp_.planners.size() ; ++i)
226  {
227  out << exp_.planners[i].name << std::endl;
228 
229  // get names of common properties
230  std::vector<std::string> properties;
231  for (std::map<std::string, std::string>::const_iterator mit = exp_.planners[i].common.begin() ;
232  mit != exp_.planners[i].common.end() ; ++mit)
233  properties.push_back(mit->first);
234  std::sort(properties.begin(), properties.end());
235 
236  // print names & values of common properties
237  out << properties.size() << " common properties" << std::endl;
238  for (unsigned int k = 0 ; k < properties.size() ; ++k)
239  {
240  std::map<std::string, std::string>::const_iterator it = exp_.planners[i].common.find(properties[k]);
241  out << it->first << " = " << it->second << std::endl;
242  }
243 
244  // construct the list of all possible properties for all runs
245  std::map<std::string, bool> propSeen;
246  for (unsigned int j = 0 ; j < exp_.planners[i].runs.size() ; ++j)
247  for (std::map<std::string, std::string>::const_iterator mit = exp_.planners[i].runs[j].begin() ;
248  mit != exp_.planners[i].runs[j].end() ; ++mit)
249  propSeen[mit->first] = true;
250 
251  properties.clear();
252 
253  for (std::map<std::string, bool>::iterator it = propSeen.begin() ; it != propSeen.end() ; ++it)
254  properties.push_back(it->first);
255  std::sort(properties.begin(), properties.end());
256 
257  // print the property names
258  out << properties.size() << " properties for each run" << std::endl;
259  for (unsigned int j = 0 ; j < properties.size() ; ++j)
260  out << properties[j] << std::endl;
261 
262  // print the data for each run
263  out << exp_.planners[i].runs.size() << " runs" << std::endl;
264  for (unsigned int j = 0 ; j < exp_.planners[i].runs.size() ; ++j)
265  {
266  for (unsigned int k = 0 ; k < properties.size() ; ++k)
267  {
268  std::map<std::string, std::string>::const_iterator it = exp_.planners[i].runs[j].find(properties[k]);
269  if (it != exp_.planners[i].runs[j].end())
270  out << it->second;
271  out << "; ";
272  }
273  out << std::endl;
274  }
275 
276  out << '.' << std::endl;
277  }
278  return true;
279 }
280 
282 {
283  // sanity checks
284  if (gsetup_)
285  {
286  if (!gsetup_->getSpaceInformation()->isSetup())
287  gsetup_->getSpaceInformation()->setup();
288  }
289  else
290  {
291  if (!csetup_->getSpaceInformation()->isSetup())
292  csetup_->getSpaceInformation()->setup();
293  }
294 
295  if (!(gsetup_ ? gsetup_->getGoal() : csetup_->getGoal()))
296  {
297  logError("No goal defined");
298  return;
299  }
300 
301  if (planners_.empty())
302  {
303  logError("There are no planners to benchmark");
304  return;
305  }
306 
307  status_.running = true;
308  exp_.totalDuration = 0.0;
309  exp_.maxTime = req.maxTime;
310  exp_.maxMem = req.maxMem;
311  exp_.runCount = req.runCount;
312  exp_.host = machine::getHostname();
313  exp_.seed = RNG::getSeed();
314 
315  exp_.startTime = time::now();
316 
317  logInform("Configuring planners ...");
318 
319  // clear previous experimental data
320  exp_.planners.clear();
321  exp_.planners.resize(planners_.size());
322 
323  const base::ProblemDefinitionPtr &pdef = gsetup_ ? gsetup_->getProblemDefinition() : csetup_->getProblemDefinition();
324  // set up all the planners
325  for (unsigned int i = 0 ; i < planners_.size() ; ++i)
326  {
327  // configure the planner
328  planners_[i]->setProblemDefinition(pdef);
329  if (!planners_[i]->isSetup())
330  planners_[i]->setup();
331  exp_.planners[i].name = (gsetup_ ? "geometric_" : "control_") + planners_[i]->getName();
332  logInform("Configured %s", exp_.planners[i].name.c_str());
333  }
334 
335  logInform("Done configuring planners.");
336  logInform("Saving planner setup information ...");
337 
338  std::stringstream setupInfo;
339  if (gsetup_)
340  gsetup_->print(setupInfo);
341  else
342  csetup_->print(setupInfo);
343  setupInfo << std::endl << "Properties of benchmarked planners:" << std::endl;
344  for (unsigned int i = 0 ; i < planners_.size() ; ++i)
345  planners_[i]->printProperties(setupInfo);
346 
347  exp_.setupInfo = setupInfo.str();
348 
349  logInform("Done saving information");
350 
351  logInform("Beginning benchmark");
353  boost::scoped_ptr<msg::OutputHandlerFile> ohf;
354  if (req.saveConsoleOutput)
355  {
356  ohf.reset(new msg::OutputHandlerFile(getConsoleFilename(exp_).c_str()));
357  msg::useOutputHandler(ohf.get());
358  }
359  else
361  logInform("Beginning benchmark");
362 
363  boost::scoped_ptr<boost::progress_display> progress;
364  if (req.displayProgress)
365  {
366  std::cout << "Running experiment " << exp_.name << "." << std::endl;
367  std::cout << "Each planner will be executed " << req.runCount << " times for at most " << req.maxTime << " seconds. Memory is limited at "
368  << req.maxMem << "MB." << std::endl;
369  progress.reset(new boost::progress_display(100, std::cout));
370  }
371 
373  machine::MemUsage_t maxMemBytes = (machine::MemUsage_t)(req.maxMem * 1024 * 1024);
374 
375  for (unsigned int i = 0 ; i < planners_.size() ; ++i)
376  {
377  status_.activePlanner = exp_.planners[i].name;
378  // execute planner switch event, if set
379  try
380  {
381  if (plannerSwitch_)
382  {
383  logInform("Executing planner-switch event for planner %s ...", status_.activePlanner.c_str());
384  plannerSwitch_(planners_[i]);
385  logInform("Completed execution of planner-switch event");
386  }
387  }
388  catch(std::runtime_error &e)
389  {
390  std::stringstream es;
391  es << "There was an error executing the planner-switch event for planner " << status_.activePlanner << std::endl;
392  es << "*** " << e.what() << std::endl;
393  std::cerr << es.str();
394  logError(es.str().c_str());
395  }
396  if (gsetup_)
397  gsetup_->setup();
398  else
399  csetup_->setup();
400  planners_[i]->params().getParams(exp_.planners[i].common);
401  planners_[i]->getSpaceInformation()->params().getParams(exp_.planners[i].common);
402 
403  // run the planner
404  for (unsigned int j = 0 ; j < req.runCount ; ++j)
405  {
406  status_.activeRun = j;
407  status_.progressPercentage = (double)(100 * (req.runCount * i + j)) / (double)(planners_.size() * req.runCount);
408 
409  if (req.displayProgress)
410  while (status_.progressPercentage > progress->count())
411  ++(*progress);
412 
413  logInform("Preparing for run %d of %s", status_.activeRun, status_.activePlanner.c_str());
414 
415  // make sure all planning data structures are cleared
416  try
417  {
418  planners_[i]->clear();
419  if (gsetup_)
420  {
421  gsetup_->getProblemDefinition()->clearSolutionPaths();
422  gsetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
423  }
424  else
425  {
426  csetup_->getProblemDefinition()->clearSolutionPaths();
427  csetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
428  }
429  }
430  catch(std::runtime_error &e)
431  {
432  std::stringstream es;
433  es << "There was an error while preparing for run " << status_.activeRun << " of planner " << status_.activePlanner << std::endl;
434  es << "*** " << e.what() << std::endl;
435  std::cerr << es.str();
436  logError(es.str().c_str());
437  }
438 
439  // execute pre-run event, if set
440  try
441  {
442  if (preRun_)
443  {
444  logInform("Executing pre-run event for run %d of planner %s ...", status_.activeRun, status_.activePlanner.c_str());
445  preRun_(planners_[i]);
446  logInform("Completed execution of pre-run event");
447  }
448  }
449  catch(std::runtime_error &e)
450  {
451  std::stringstream es;
452  es << "There was an error executing the pre-run event for run " << status_.activeRun << " of planner " << status_.activePlanner << std::endl;
453  es << "*** " << e.what() << std::endl;
454  std::cerr << es.str();
455  logError(es.str().c_str());
456  }
457 
458  RunPlanner rp(this, req.useThreads);
459  rp.run(planners_[i], memStart, maxMemBytes, req.maxTime);
460  bool solved = gsetup_ ? gsetup_->haveSolutionPath() : csetup_->haveSolutionPath();
461 
462  // store results
463  try
464  {
465  RunProperties run;
466 
467  run["time REAL"] = boost::lexical_cast<std::string>(rp.getTimeUsed());
468  run["memory REAL"] = boost::lexical_cast<std::string>((double)rp.getMemUsed() / (1024.0 * 1024.0));
469  run["status ENUM"] = boost::lexical_cast<std::string>((int)static_cast<base::PlannerStatus::StatusType>(rp.getStatus()));
470  if (gsetup_)
471  {
472  run["solved BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->haveExactSolutionPath());
473  run["valid segment fraction REAL"] = boost::lexical_cast<std::string>(gsetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
474  }
475  else
476  {
477  run["solved BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->haveExactSolutionPath());
478  run["valid segment fraction REAL"] = boost::lexical_cast<std::string>(csetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
479  }
480 
481  if (solved)
482  {
483  if (gsetup_)
484  {
485  run["approximate solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getProblemDefinition()->hasApproximateSolution());
486  run["solution difference REAL"] = boost::lexical_cast<std::string>(gsetup_->getProblemDefinition()->getSolutionDifference());
487  run["solution length REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().length());
488  run["solution smoothness REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().smoothness());
489  run["solution clearance REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().clearance());
490  run["solution segments INTEGER"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().getStateCount() - 1);
491  run["correct solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
492 
493  unsigned int factor = gsetup_->getStateSpace()->getValidSegmentCountFactor();
494  gsetup_->getStateSpace()->setValidSegmentCountFactor(factor * 4);
495  run["correct solution strict BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
496  gsetup_->getStateSpace()->setValidSegmentCountFactor(factor);
497 
498  // simplify solution
499  time::point timeStart = time::now();
500  gsetup_->simplifySolution();
501  double timeUsed = time::seconds(time::now() - timeStart);
502  run["simplification time REAL"] = boost::lexical_cast<std::string>(timeUsed);
503  run["simplified solution length REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().length());
504  run["simplified solution smoothness REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().smoothness());
505  run["simplified solution clearance REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().clearance());
506  run["simplified solution segments INTEGER"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().getStateCount() - 1);
507  run["simplified correct solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
508  gsetup_->getStateSpace()->setValidSegmentCountFactor(factor * 4);
509  run["simplified correct solution strict BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
510  gsetup_->getStateSpace()->setValidSegmentCountFactor(factor);
511  }
512  else
513  {
514  run["approximate solution BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->getProblemDefinition()->hasApproximateSolution());
515  run["solution difference REAL"] = boost::lexical_cast<std::string>(csetup_->getProblemDefinition()->getSolutionDifference());
516  run["solution length REAL"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().length());
517  run["solution clearance REAL"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().asGeometric().clearance());
518  run["solution segments INTEGER"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().getControlCount());
519  run["correct solution BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().check());
520  }
521  }
522 
523  base::PlannerData pd (gsetup_ ? gsetup_->getSpaceInformation() : csetup_->getSpaceInformation());
524  planners_[i]->getPlannerData(pd);
525  run["graph states INTEGER"] = boost::lexical_cast<std::string>(pd.numVertices());
526  run["graph motions INTEGER"] = boost::lexical_cast<std::string>(pd.numEdges());
527 
528  for (std::map<std::string, std::string>::const_iterator it = pd.properties.begin() ; it != pd.properties.end() ; ++it)
529  run[it->first] = it->second;
530 
531  // execute post-run event, if set
532  try
533  {
534  if (postRun_)
535  {
536  logInform("Executing post-run event for run %d of planner %s ...", status_.activeRun, status_.activePlanner.c_str());
537  postRun_(planners_[i], run);
538  logInform("Completed execution of post-run event");
539  }
540  }
541  catch(std::runtime_error &e)
542  {
543  std::stringstream es;
544  es << "There was an error in the execution of the post-run event for run " << status_.activeRun << " of planner " << status_.activePlanner << std::endl;
545  es << "*** " << e.what() << std::endl;
546  std::cerr << es.str();
547  logError(es.str().c_str());
548  }
549 
550  exp_.planners[i].runs.push_back(run);
551  }
552  catch(std::runtime_error &e)
553  {
554  std::stringstream es;
555  es << "There was an error in the extraction of planner results: planner = " << status_.activePlanner << ", run = " << status_.activePlanner << std::endl;
556  es << "*** " << e.what() << std::endl;
557  std::cerr << es.str();
558  logError(es.str().c_str());
559  }
560  }
561  }
562 
563  status_.running = false;
564  status_.progressPercentage = 100.0;
565  if (req.displayProgress)
566  {
567  while (status_.progressPercentage > progress->count())
568  ++(*progress);
569  std::cout << std::endl;
570  }
571 
572  exp_.totalDuration = time::seconds(time::now() - exp_.startTime);
573 
574  logInform("Benchmark complete");
576  logInform("Benchmark complete");
577 }