Loading...
Searching...
No Matches
Thunder.cpp
46ompl::tools::Thunder::Thunder(const base::SpaceInformationPtr &si) : ompl::tools::ExperienceSetup(si)
51ompl::tools::Thunder::Thunder(const base::StateSpacePtr &space) : ompl::tools::ExperienceSetup(space)
83 // Decide if we should setup the second planning from scratch planner for benchmarking w/o recall
117 throw Exception("Both planning from scratch and experience have been disabled, unable to plan");
140 experienceDB_->getSPARSdb()->setSparseDeltaFraction(0.05); // vertex visibility range = maximum_extent *
175ompl::base::PlannerStatus ompl::tools::Thunder::solve(const base::PlannerTerminationCondition &ptc)
177 // we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner
191 OMPL_WARN("Previous solved paths are currently uninserted into the experience database and are in the "
195 // There are two modes for running parallel plan - one in which both threads are run until they both return a result
202 // If \e hybridize is false, when the first solution is found, the rest of the planners are stopped as well.
210 OMPL_WARN("Thunder: not stopping until a solution or a failure is found from both threads. THIS MODE IS JUST "
213 // If \e hybridize is false, when \e minSolCount new solutions are found (added to the set of solutions
265 OMPL_INFORM("Solution path has %d states and was generated from planner %s", solutionPath.getStateCount(),
309 OMPL_INFORM("Adding path to database because SPARS will decide for us if we should keep the nodes");
445 out << " That were discarded: " << stats_.numSolutionsFromRecall_ - stats_.numSolutionsFromRecallSaved_
454 out << " Connected Components: " << experienceDB_->getSPARSdb()->getNumConnectedComponents() << 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;
468void ompl::tools::Thunder::getAllPlannerDatas(std::vector<ob::PlannerDataPtr> &plannerDatas) const
473void ompl::tools::Thunder::convertPlannerData(const ob::PlannerDataPtr &plannerData, og::PathGeometric &path)
480bool ompl::tools::Thunder::reversePathIfNecessary(og::PathGeometric &path1, og::PathGeometric &path2)
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition PlannerTerminationCondition.h:64
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
Definition PathGeometric.h:260
base::State * getState(unsigned int index)
Get the state located at index along the path.
Definition PathGeometric.h:248
PathGeometric & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
Definition SimpleSetup.cpp:208
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
Definition SimpleSetup.h:141
const std::string getSolutionPlannerName() const
Get the best solution's planer name. Throw an exception if no solution is available.
Definition SimpleSetup.cpp:194
bool configured_
Flag indicating whether the classes needed for planning are set up.
Definition SimpleSetup.h:297
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
Definition SimpleSetup.cpp:171
void convertLogToString(const ExperienceLog &log)
Move data to string format and put in buffer.
Definition ExperienceSetup.cpp:73
bool recallEnabled_
Flag indicating whether recalled plans should be used to find solutions. Enabled by default.
Definition ExperienceSetup.h:196
bool scratchEnabled_
Flag indicating whether planning from scratch should be used to find solutions. Enabled by default.
Definition ExperienceSetup.h:199
ExperienceSetup(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
Definition ExperienceSetup.cpp:42
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
void printResultsInfo(std::ostream &out=std::cout) const override
Display debug data about potential available solutions.
Definition Thunder.cpp:397
bool doPostProcessing() override
Allow accumlated experiences to be processed.
Definition Thunder.cpp:507
void printLogs(std::ostream &out=std::cout) const override
Display debug data about overall results from Thunder since being loaded.
Definition Thunder.cpp:433
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 print(std::ostream &out=std::cout) const override
Print information about the current setup.
Definition Thunder.cpp:407
Thunder(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
Definition Thunder.cpp:46
void clear() override
Clear all planning data. This only includes data generated by motion plan computation....
Definition Thunder.cpp:151
bool saveIfChanged() override
Save the experience database to file if there has been a change.
Definition Thunder.cpp:391
std::vector< ompl::geometric::PathGeometric > queuedSolutionPaths_
Accumulated experiences to be later added to experience database.
Definition Thunder.h:198
std::size_t getExperiencesCount() const override
Get the total number of paths stored in the database.
Definition Thunder.cpp:463
bool dualThreadScratchEnabled_
Definition Thunder.h:189
ompl::tools::ThunderDBPtr getExperienceDB()
Hook for getting access to debug data.
Definition Thunder.cpp:502
ompl::tools::ThunderDBPtr experienceDB_
A shared object between all the planners for saving and loading previous experience.
Definition Thunder.h:195
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition Thunder.cpp:71
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
ompl::tools::ParallelPlanPtr pp_
Instance of parallel planning to use for computing solutions in parallel.
Definition Thunder.h:192
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
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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition ConstrainedSpaceInformation.h:55
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition Planner.h:437
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
Definition PlannerTerminationCondition.cpp:216
This namespace contains code that is specific to planning under geometric constraints.
Definition GeneticSearch.h:48
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition Time.h:64
Includes various tools such as self config, benchmarking, etc.
Definition LightningRetrieveRepair.h:48
Main namespace. Contains everything in this library.
Definition MultiLevelPlanarManipulatorDemo.cpp:66
A class to store the exit status of Planner::solve()
Definition PlannerStatus.h:49
Single entry for the csv data logging file.
Definition ExperienceSetup.h:102