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