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
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_);
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
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 
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 
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 }
base::PlannerPtr rrPlanner_
Definition: Thunder.h:183
std::vector< ompl::geometric::PathGeometric > queuedSolutionPaths_
Accumulated experiences to be later added to experience database.
Definition: Thunder.h:198
bool doPostProcessing() override
Allow accumlated experiences to be processed.
Definition: Thunder.cpp:526
const std::string getSolutionPlannerName() const
Get the best solution&#39;s planer name. Throw an exception if no solution is available.
ompl::tools::ThunderDBPtr getExperienceDB()
Hook for getting access to debug data.
Definition: Thunder.cpp:521
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
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
base::PlannerAllocator pa_
The optional planner allocator.
Definition: SimpleSetup.h:283
The planner failed to find a solution.
Definition: PlannerStatus.h:62
PathGeometric & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
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.
ompl::tools::ParallelPlanPtr pp_
Instance of parallel planning to use for computing solutions in parallel.
Definition: Thunder.h:192
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...
base::PlannerPtr planner2_
Definition: Thunder.h:186
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:274
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
ExperienceStats stats_
States data for display to console.
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
std::string filePath_
File location of database.
base::PlannerPtr planner_
The maintained planner instance.
Definition: SimpleSetup.h:280
void reverse()
Reverse the path.
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:277
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...
Definition: Cost.h:44
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
bool scratchEnabled_
Flag indicating whether planning from scratch should be used to find solutions. Enabled by default...
The exception type for ompl.
Definition: Exception.h:46
base::PlannerStatus lastStatus_
The status of the last planning request.
Definition: SimpleSetup.h:298
point now()
Get the current time point.
Definition: Time.h:70
bool configured_
Flag indicating whether the classes needed for planning are set up.
Definition: SimpleSetup.h:289
double planTime_
The amount of time the last planning step took.
Definition: SimpleSetup.h:292
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
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
bool recallEnabled_
Flag indicating whether recalled plans should be used to find solutions. Enabled by default...
void convertLogToString(const ExperienceLog &log)
Move data to string format and put in buffer.
bool dualThreadScratchEnabled_
Definition: Thunder.h:189
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
ompl::tools::ThunderDBPtr experienceDB_
A shared object between all the planners for saving and loading previous experience.
Definition: Thunder.h:195
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68