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