Thunder.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, JSK, The University of Tokyo.
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 JSK, The University of Tokyo 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: Dave Coleman */
36 
37 #include <ompl/tools/thunder/Thunder.h>
38 #include <ompl/geometric/planners/rrt/RRTConnect.h>
39 #include <ompl/base/PlannerStatus.h>
40 #include <ompl/util/Console.h>
41 
42 namespace og = ompl::geometric;
43 namespace ob = ompl::base;
44 namespace ot = ompl::tools;
45 
46 ompl::tools::Thunder::Thunder(const base::SpaceInformationPtr &si) : ompl::tools::ExperienceSetup(si)
47 {
48  initialize();
49 }
50 
51 ompl::tools::Thunder::Thunder(const base::StateSpacePtr &space) : ompl::tools::ExperienceSetup(space)
52 {
53  initialize();
54 }
55 
56 void ompl::tools::Thunder::initialize()
57 {
58  OMPL_INFORM("Initializing Thunder Framework");
59 
60  filePath_ = "unloaded";
61 
62  // Load the experience database
63  experienceDB_ = std::make_shared<ompl::tools::ThunderDB>(si_->getStateSpace());
64 
65  // Load the Retrieve repair database. We do it here so that setRepairPlanner() works
66  rrPlanner_ = std::make_shared<og::ThunderRetrieveRepair>(si_, experienceDB_);
67 
68  OMPL_INFORM("Thunder Framework initialized.");
69 }
70 
72 {
73  if (filePath_ == "unloaded" || filePath_.empty())
74  {
75  OMPL_WARN("Database filepath has not been set. Unable to setup!");
76  return;
77  }
78 
79  if (!configured_ || !si_->isSetup() || !planner_->isSetup() || !rrPlanner_->isSetup())
80  {
81  SimpleSetup::setup();
82 
83  // Decide if we should setup the second planning from scratch planner for benchmarking w/o recall
84  if (dualThreadScratchEnabled_ && !recallEnabled_)
85  {
86  // Setup planning from scratch planner 2
87  if (!planner2_)
88  {
89  if (pa_)
90  planner2_ = pa_(si_);
91  if (!planner2_)
92  {
93  OMPL_INFORM("Getting default planner: ");
94  planner2_ = std::make_shared<ompl::geometric::RRTConnect>(si_);
95  // This was disabled because i like to use Thunder / SPARSdb without setting a goal definition
96  // planner2_ = ompl::geometric::getDefaultPlanner(pdef_->getGoal()); // we could use the
97  // repairProblemDef_ here but that isn't setup yet
98 
99  OMPL_INFORM("No planner 2 specified. Using default: %s", planner2_->getName().c_str());
100  }
101  }
102  planner2_->setProblemDefinition(pdef_);
103  if (!planner2_->isSetup())
104  planner2_->setup();
105  }
106 
107  // Setup planning from experience planner
108  rrPlanner_->setProblemDefinition(pdef_);
109 
110  if (!rrPlanner_->isSetup())
111  rrPlanner_->setup();
112 
113  // Create the parallel component for splitting into two threads
114  pp_ = std::make_shared<ot::ParallelPlan>(pdef_);
115  if (!scratchEnabled_ && !recallEnabled_)
116  {
117  throw Exception("Both planning from scratch and experience have been disabled, unable to plan");
118  }
119  if (recallEnabled_)
120  pp_->addPlanner(rrPlanner_); // Add the planning from experience planner if desired
121  if (scratchEnabled_)
122  pp_->addPlanner(planner_); // Add the planning from scratch planner if desired
123  if (dualThreadScratchEnabled_ && !recallEnabled_)
124  {
125  OMPL_INFORM("Adding second planning from scratch planner");
126  pp_->addPlanner(planner2_); // Add a SECOND planning from scratch planner if desired
127  }
128 
129  // Setup SPARS
130  if (!experienceDB_->getSPARSdb())
131  {
132  OMPL_INFORM("Calling setup() for SPARSdb");
133 
134  // Load SPARSdb
135  experienceDB_->getSPARSdb() = std::make_shared<ompl::geometric::SPARSdb>(si_);
136  experienceDB_->getSPARSdb()->setProblemDefinition(pdef_);
137  experienceDB_->getSPARSdb()->setup();
138 
139  experienceDB_->getSPARSdb()->setStretchFactor(1.2);
140  experienceDB_->getSPARSdb()->setSparseDeltaFraction(0.05); // vertex visibility range = maximum_extent *
141  // this_fraction
142  // experienceDB_->getSPARSdb()->setDenseDeltaFraction(0.001);
143 
144  experienceDB_->getSPARSdb()->printDebug();
145 
146  experienceDB_->load(filePath_); // load from file
147  }
148  }
149 }
150 
152 {
153  if (planner_)
154  planner_->clear();
155  if (rrPlanner_)
156  rrPlanner_->clear();
157  if (planner2_)
158  planner2_->clear();
159  if (pdef_)
160  pdef_->clearSolutionPaths();
161  if (pp_)
162  {
163  pp_->clearHybridizationPaths();
164  }
165 }
166 
168 {
169  pa_ = pa;
170  planner_.reset();
171  // note: the rrPlanner_ never uses the allocator so does not need to be reset
172  configured_ = false;
173 }
174 
176 {
177  // we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner
178  // termination condition
179 
180  OMPL_INFORM("Thunder Framework: Starting solve()");
181 
182  // Setup again in case it has not been done yet
183  setup();
184 
185  lastStatus_ = base::PlannerStatus::UNKNOWN;
186  time::point start = time::now();
187 
188  // Warn if there are queued paths that have not been added to the experience database
189  if (!queuedSolutionPaths_.empty())
190  {
191  OMPL_WARN("Previous solved paths are currently uninserted into the experience database and are in the "
192  "post-proccessing queue");
193  }
194 
195  // There are two modes for running parallel plan - one in which both threads are run until they both return a result
196  // and/or fail
197  // The second mode stops with the first solution found - we want this one
198  bool stopWhenFirstSolutionFound = true;
199 
200  if (stopWhenFirstSolutionFound)
201  {
202  // If \e hybridize is false, when the first solution is found, the rest of the planners are stopped as well.
203  // OMPL_DEBUG("Thunder: stopping when first solution is found from either thread");
204  // Start both threads
205  bool hybridize = false;
206  lastStatus_ = pp_->solve(ptc, hybridize);
207  }
208  else
209  {
210  OMPL_WARN("Thunder: not stopping until a solution or a failure is found from both threads. THIS MODE IS JUST "
211  "FOR TESTING");
212  // This mode is more for benchmarking, since I don't care about optimality
213  // If \e hybridize is false, when \e minSolCount new solutions are found (added to the set of solutions
214  // maintained by ompl::base::Goal), the rest of the planners are stopped as well.
215 
216  // Start both threads
217  std::size_t minSolCount = 2;
218  std::size_t maxSolCount = 2;
219  bool hybridize = false;
220  lastStatus_ = pp_->solve(ptc, minSolCount, maxSolCount, hybridize);
221  }
222 
223  // Planning time
224  planTime_ = time::seconds(time::now() - start);
225 
226  // Create log
228  log.planning_time = planTime_;
229 
230  // Record stats
231  stats_.totalPlanningTime_ += planTime_; // used for averaging
232  stats_.numProblems_++; // used for averaging
233 
234  if (lastStatus_ == ompl::base::PlannerStatus::TIMEOUT)
235  {
236  // Skip further processing if absolutely no path is available
237  OMPL_ERROR("Thunder Solve: No solution found after %f seconds", planTime_);
238 
239  stats_.numSolutionsTimedout_++;
240 
241  // Logging
242  log.planner = "neither_planner";
243  log.result = "timedout";
244  log.is_saved = "not_saved";
245  }
246  else if (!lastStatus_)
247  {
248  // Skip further processing if absolutely no path is available
249  OMPL_ERROR("Thunder Solve: Unknown failure");
250  stats_.numSolutionsFailed_++;
251 
252  // Logging
253  log.planner = "neither_planner";
254  log.result = "failed";
255  log.is_saved = "not_saved";
256  }
257  else
258  {
259  OMPL_INFORM("Thunder Solve: Possible solution found in %f seconds", planTime_);
260 
261  // Smooth the result
262  simplifySolution(ptc);
263 
264  og::PathGeometric solutionPath = getSolutionPath(); // copied so that it is non-const
265  OMPL_INFORM("Solution path has %d states and was generated from planner %s", solutionPath.getStateCount(),
266  getSolutionPlannerName().c_str());
267 
268  // Logging
269  log.planner = getSolutionPlannerName();
270 
271  // Do not save if approximate
272  if (!haveExactSolutionPath())
273  {
274  OMPL_INFORM("THUNDER RESULTS: Approximate");
275 
276  // Logging
277  log.result = "not_exact_solution";
278  log.is_saved = "not_saved";
279  log.approximate = true;
280 
281  // Stats
282  stats_.numSolutionsApproximate_++;
283 
284  // TODO not sure what to do here, use case not tested
285  OMPL_WARN("NOT saving to database because the solution is APPROXIMATE");
286  }
287  else if (getSolutionPlannerName() == rrPlanner_->getName())
288  {
289  OMPL_INFORM("THUNDER RESULTS: From Recall");
290 
291  // Stats
292  stats_.numSolutionsFromRecall_++;
293 
294  // Logging
295  log.result = "from_recall";
296 
297  // Make sure solution has at least 2 states
298  if (solutionPath.getStateCount() < 2)
299  {
300  OMPL_INFORM("NOT saving to database because solution is less than 2 states long");
301  stats_.numSolutionsTooShort_++;
302 
303  // Logging
304  log.is_saved = "less_2_states";
305  log.too_short = true;
306  }
307  else if (false) // always add when from recall
308  {
309  OMPL_INFORM("Adding path to database because SPARS will decide for us if we should keep the nodes");
310 
311  // Stats
312  stats_.numSolutionsFromRecallSaved_++;
313 
314  // Queue the solution path for future insertion into experience database (post-processing)
315  queuedSolutionPaths_.push_back(solutionPath);
316 
317  // Logging
318  log.insertion_failed = false; // TODO this is wrong logging data
319  log.is_saved = "always_attempt";
320  }
321  else // never add when from recall
322  {
323  OMPL_INFORM("NOT adding path to database because SPARS already has it");
324 
325  // Logging
326  log.is_saved = "skipped";
327  }
328  }
329  else
330  {
331  OMPL_INFORM("THUNDER RESULTS: From Scratch");
332 
333  // Logging
334  log.result = "from_scratch";
335 
336  // Stats
337  stats_.numSolutionsFromScratch_++;
338 
339  // Make sure solution has at least 2 states
340  if (solutionPath.getStateCount() < 2)
341  {
342  OMPL_INFORM("NOT saving to database because solution is less than 2 states long");
343 
344  // Logging
345  log.is_saved = "less_2_states";
346  log.too_short = true;
347 
348  // Stats
349  stats_.numSolutionsTooShort_++;
350  }
351  else
352  {
353  OMPL_INFORM("Adding path to database because best solution was not from database");
354 
355  // Logging
356  log.result = "from_scratch";
357  log.is_saved = "saving";
358 
359  // Queue the solution path for future insertion into experience database (post-processing)
360  queuedSolutionPaths_.push_back(solutionPath);
361 
362  log.insertion_failed = false; // TODO fix this wrong logging info
363  }
364  }
365  }
366 
367  // Final log data
368  // log.insertion_time = insertionTime; TODO fix this
369  log.num_vertices = experienceDB_->getSPARSdb()->getNumVertices();
370  log.num_edges = experienceDB_->getSPARSdb()->getNumEdges();
371  log.num_connected_components = experienceDB_->getSPARSdb()->getNumConnectedComponents();
372 
373  // Flush the log to buffer
374  convertLogToString(log);
375 
376  return lastStatus_;
377 }
378 
380 {
381  ob::PlannerTerminationCondition ptc = ob::timedPlannerTerminationCondition(time);
382  return solve(ptc);
383 }
384 
386 {
387  setup(); // ensure the PRM db has been loaded to the Experience DB
388  return experienceDB_->save(filePath_);
389 }
390 
392 {
393  setup(); // ensure the PRM db has been loaded to the Experience DB
394  return experienceDB_->saveIfChanged(filePath_);
395 }
396 
397 void ompl::tools::Thunder::printResultsInfo(std::ostream &out) const
398 {
399  for (std::size_t i = 0; i < pdef_->getSolutionCount(); ++i)
400  {
401  out << "Solution " << i << "\t | Length: " << pdef_->getSolutions()[i].length_
402  << "\t | Approximate: " << (pdef_->getSolutions()[i].approximate_ ? "true" : "false")
403  << "\t | Planner: " << pdef_->getSolutions()[i].plannerName_ << std::endl;
404  }
405 }
406 
407 void ompl::tools::Thunder::print(std::ostream &out) const
408 {
409  if (si_)
410  {
411  si_->printProperties(out);
412  si_->printSettings(out);
413  }
414  if (planner_)
415  {
416  planner_->printProperties(out);
417  planner_->printSettings(out);
418  }
419  if (rrPlanner_)
420  {
421  rrPlanner_->printProperties(out);
422  rrPlanner_->printSettings(out);
423  }
424  if (planner2_)
425  {
426  planner2_->printProperties(out);
427  planner2_->printSettings(out);
428  }
429  if (pdef_)
430  pdef_->print(out);
431 }
432 
433 void ompl::tools::Thunder::printLogs(std::ostream &out) const
434 {
435  if (!recallEnabled_)
436  out << "Scratch Planning Logging Results (inside Thunder Framework)" << std::endl;
437  else
438  out << "Thunder Framework Logging Results" << std::endl;
439  out << " Solutions Attempted: " << stats_.numProblems_ << std::endl;
440  out << " Solved from scratch: " << stats_.numSolutionsFromScratch_ << " ("
441  << stats_.numSolutionsFromScratch_ / stats_.numProblems_ * 100 << "%)" << std::endl;
442  out << " Solved from recall: " << stats_.numSolutionsFromRecall_ << " ("
443  << stats_.numSolutionsFromRecall_ / stats_.numProblems_ * 100 << "%)" << std::endl;
444  out << " That were saved: " << stats_.numSolutionsFromRecallSaved_ << std::endl;
445  out << " That were discarded: " << stats_.numSolutionsFromRecall_ - stats_.numSolutionsFromRecallSaved_
446  << std::endl;
447  out << " Less than 2 states: " << stats_.numSolutionsTooShort_ << std::endl;
448  out << " Failed: " << stats_.numSolutionsFailed_ << std::endl;
449  out << " Timedout: " << stats_.numSolutionsTimedout_ << std::endl;
450  out << " Approximate: " << stats_.numSolutionsApproximate_ << std::endl;
451  out << " SPARSdb " << std::endl;
452  out << " Vertices: " << experienceDB_->getSPARSdb()->getNumVertices() << std::endl;
453  out << " Edges: " << experienceDB_->getSPARSdb()->getNumEdges() << std::endl;
454  out << " Connected Components: " << experienceDB_->getSPARSdb()->getNumConnectedComponents() << std::endl;
455  out << " Unsaved paths inserted: " << experienceDB_->getNumPathsInserted() << std::endl;
456  out << " Consecutive state failures: " << experienceDB_->getSPARSdb()->getNumConsecutiveFailures() << std::endl;
457  out << " Connected path failures: " << experienceDB_->getSPARSdb()->getNumPathInsertionFailed() << std::endl;
458  out << " Sparse Delta Fraction: " << experienceDB_->getSPARSdb()->getSparseDeltaFraction() << std::endl;
459  out << " Average planning time: " << stats_.getAveragePlanningTime() << std::endl;
460  out << " Average insertion time: " << stats_.getAverageInsertionTime() << std::endl;
461 }
462 
464 {
465  return experienceDB_->getSPARSdb()->getNumVertices();
466 }
467 
468 void ompl::tools::Thunder::getAllPlannerDatas(std::vector<ob::PlannerDataPtr> &plannerDatas) const
469 {
470  experienceDB_->getAllPlannerDatas(plannerDatas);
471 }
472 
473 void ompl::tools::Thunder::convertPlannerData(const ob::PlannerDataPtr &plannerData, og::PathGeometric &path)
474 {
475  // Convert the planner data vertices into a vector of states
476  for (std::size_t i = 0; i < plannerData->numVertices(); ++i)
477  path.append(plannerData->getVertex(i).getState());
478 }
479 
481 {
482  // Reverse path2 if it matches better
483  const ob::State *s1 = path1.getState(0);
484  const ob::State *s2 = path2.getState(0);
485  const ob::State *g1 = path1.getState(path1.getStateCount() - 1);
486  const ob::State *g2 = path2.getState(path2.getStateCount() - 1);
487 
488  double regularDistance = si_->distance(s1, s2) + si_->distance(g1, g2);
489  double reversedDistance = si_->distance(s1, g2) + si_->distance(s2, g1);
490 
491  // Check if path is reversed from normal [start->goal] direction
492  if (regularDistance > reversedDistance)
493  {
494  // needs to be reversed
495  path2.reverse();
496  return true;
497  }
498 
499  return false;
500 }
501 
502 ompl::tools::ThunderDBPtr ompl::tools::Thunder::getExperienceDB()
503 {
504  return experienceDB_;
505 }
506 
508 {
509  OMPL_INFORM("Performing post-processing");
510 
511  for (auto &queuedSolutionPath : queuedSolutionPaths_)
512  {
513  // Time to add a path to experience database
514  double insertionTime;
515 
516  experienceDB_->addPath(queuedSolutionPath, insertionTime);
517  OMPL_INFORM("Finished inserting experience path in %f seconds", insertionTime);
518  stats_.totalInsertionTime_ += insertionTime; // used for averaging
519  }
520 
521  // Remove all inserted paths from the queue
522  queuedSolutionPaths_.clear();
523 
524  return true;
525 }
void reverse()
Reverse the path.
Includes various tools such as self config, benchmarking, etc.
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:116
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:79
Definition of an abstract state.
Definition: State.h:113
This namespace contains sampling based planning routines shared by both planning under geometric cons...
void print(std::ostream &out=std::cout) const override
Print information about the current setup.
Definition: Thunder.cpp:407
bool reversePathIfNecessary(ompl::geometric::PathGeometric &path1, ompl::geometric::PathGeometric &path2)
If path1 and path2 have a better start/goal match when reverse, then reverse path2.
Definition: Thunder.cpp:480
point now()
Get the current time point.
Definition: Time.h:122
bool doPostProcessing() override
Allow accumlated experiences to be processed.
Definition: Thunder.cpp:507
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
Definition of a geometric path.
Definition: PathGeometric.h:97
void convertPlannerData(const ompl::base::PlannerDataPtr &plannerData, ompl::geometric::PathGeometric &path)
Convert PlannerData to PathGeometric. Assume ordering of vertices is order of path.
Definition: Thunder.cpp:473
bool saveIfChanged() override
Save the experience database to file if there has been a change.
Definition: Thunder.cpp:391
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void clear() override
Clear all planning data. This only includes data generated by motion plan computation....
Definition: Thunder.cpp:151
A class to store the exit status of Planner::solve()
Thunder(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
Definition: Thunder.cpp:46
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:501
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
base::PlannerStatus solve(double time=1.0) override
Run the planner for up to a specified amount of time (default is 1 second)
Definition: Thunder.cpp:379
void setPlannerAllocator(const base::PlannerAllocator &pa)
Set the planner allocator to use. This is only used if no planner has been set. This is optional – a ...
Definition: Thunder.cpp:167
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: Thunder.cpp:71
void printResultsInfo(std::ostream &out=std::cout) const override
Display debug data about potential available solutions.
Definition: Thunder.cpp:397
Single entry for the csv data logging file.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
Create the set of classes typically needed to solve a geometric problem.
@ UNKNOWN
Uninitialized status.
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const override
Get a vector of all the planning data in the database.
Definition: Thunder.cpp:468
bool save() override
Save the experience database to file.
Definition: Thunder.cpp:385
ompl::tools::ThunderDBPtr getExperienceDB()
Hook for getting access to debug data.
Definition: Thunder.cpp:502
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void printLogs(std::ostream &out=std::cout) const override
Display debug data about overall results from Thunder since being loaded.
Definition: Thunder.cpp:433
std::size_t getExperiencesCount() const override
Get the total number of paths stored in the database.
Definition: Thunder.cpp:463
The exception type for ompl.
Definition: Exception.h:78
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:128
base::State * getState(unsigned int index)
Get the state located at index along the path.
Main namespace. Contains everything in this library.