AnytimePathShortening.cpp
75 ompl::geometric::AnytimePathShortening::AnytimePathShortening(const ompl::base::SpaceInformationPtr &si)
76 : ompl::base::Planner(si, "APS"), defaultNumPlanners_(std::max(1u, std::thread::hardware_concurrency()))
86 Planner::declareParam<unsigned int>("max_hybrid_paths", this, &AnytimePathShortening::setMaxHybridizationPath,
88 Planner::declareParam<unsigned int>("num_planners", this, &AnytimePathShortening::setDefaultNumPlanners,
102 OMPL_ERROR("NOT adding planner %s: SpaceInformation instances are different", planner->getName().c_str());
111 OMPL_ERROR("NOT adding planner %s: Planner instances MUST be unique", planner->getName().c_str());
119 void ompl::geometric::AnytimePathShortening::addPath(const geometric::PathGeometricPtr &path, base::Planner *planner)
134 ompl::geometric::AnytimePathShortening::solve(const ompl::base::PlannerTerminationCondition &ptc)
142 OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
178 geometric::PathGeometric *lastPath = static_cast<PathGeometric *>(paths[numPaths - 1].path_.get());
213 return pdef_->getSolutionCount() > 0 ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::UNKNOWN;
228 geometric::PathGeometric *sln = static_cast<geometric::PathGeometric *>(pdef->getSolutionPath().get());
248 void ompl::geometric::AnytimePathShortening::getPlannerData(ompl::base::PlannerData &data) const
257 void ompl::geometric::AnytimePathShortening::getPlannerData(ompl::base::PlannerData &data, unsigned int idx) const
276 OMPL_INFORM("%s: No planners specified; using %u instances of %s", getName().c_str(), planners_.size(),
295 ompl::base::PlannerPtr ompl::geometric::AnytimePathShortening::getPlanner(unsigned int idx) const
unsigned int maxHybridizationPaths() const
Return the maximum number of paths that will be hybridized.
Definition: AnytimePathShortening.cpp:321
std::string getPlanners() const
Get a string representation of the planners and their parameters in the format of setPlanners.
Definition: AnytimePathShortening.cpp:428
A shared pointer wrapper for ompl::base::SpaceInformation.
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:256
unsigned int recordPath(const geometric::PathGeometricPtr &pp, bool matchAcrossGaps)
Add a path to the hybridization. If matchAcrossGaps is true, more possible edge connections are evalu...
Definition: PathHybridization.cpp:137
void getPlannerData(base::PlannerData &data) const override
Get information about the most recent run of the motion planner.
Definition: AnytimePathShortening.cpp:248
bool isShortcutting() const
Return whether the anytime planner will perform shortcutting on paths.
Definition: AnytimePathShortening.cpp:301
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition: Planner.h:467
std::size_t pathCount() const
Get the number of paths that are currently considered as part of the hybridization.
Definition: PathHybridization.cpp:279
This class contains routines that attempt to simplify geometric paths.
Definition: PathSimplifier.h:129
const geometric::PathGeometricPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before.
Definition: PathHybridization.cpp:132
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
bool simplify(PathGeometric &path, double maxTime, bool atLeastOnce=true)
Run simplification algorithms on the path for at most maxTime seconds, and at least once if atLeastOn...
Definition: PathSimplifier.cpp:672
base::PlannerPtr getPlanner(unsigned int idx) const
Retrieve a pointer to the ith planner instance.
Definition: AnytimePathShortening.cpp:295
unsigned int getNumPlanners() const
Retrieve the number of planners added.
Definition: AnytimePathShortening.cpp:290
A shared pointer wrapper for ompl::base::Planner.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
void setup() override
Perform any necessary configuration steps. This method also invokes ompl::base::SpaceInformation::set...
Definition: AnytimePathShortening.cpp:264
Given multiple geometric paths, attempt to combine them in order to obtain a shorter solution.
Definition: PathHybridization.h:134
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:127
void computeHybridPath()
Run Dijkstra's algorithm to find out the lowest-cost path among the mixed ones.
Definition: PathHybridization.cpp:108
void setLogLevel(LogLevel level)
Set the minimum level of logging data to output. Messages with lower logging levels will not be recor...
Definition: Console.cpp:136
AnytimePathShortening(const base::SpaceInformationPtr &si)
Constructor requires the space information to plan in.
Definition: AnytimePathShortening.cpp:75
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:263
bool isHybridizing() const
Return whether the anytime planner will extract a hybrid path from the set of solution paths.
Definition: AnytimePathShortening.cpp:311
virtual void threadSolve(base::Planner *planner, const base::PlannerTerminationCondition &ptc)
The function that the planning threads execute when solving a motion planning problem.
Definition: AnytimePathShortening.cpp:216
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:112
void setDefaultNumPlanners(unsigned int numPlanners)
Set default number of planners to use if none are specified.
Definition: AnytimePathShortening.cpp:331
void addPath(const geometric::PathGeometricPtr &path, base::Planner *planner)
add a path to set of solutions
Definition: AnytimePathShortening.cpp:119
void printSettings(std::ostream &out) const override
Print settings of this planner as well as those of the planner instances it contains.
Definition: AnytimePathShortening.cpp:454
void setMaxHybridizationPath(unsigned int maxPathCount)
Set the maximum number of paths that will be hybridized.
Definition: AnytimePathShortening.cpp:326
void setHybridize(bool hybridize)
Enable/disable path hybridization on the set of solution paths.
Definition: AnytimePathShortening.cpp:316
unsigned int getDefaultNumPlanners() const
Get default number of planners used if none are specified.
Definition: AnytimePathShortening.cpp:336
bool shortcutPath(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33, double snapToVertex=0.005)
Given a path, attempt to shorten it while maintaining its validity. This is an iterative process that...
Definition: PathSimplifier.cpp:187
std::string getBestCost() const
Return best cost found so far by algorithm.
Definition: AnytimePathShortening.cpp:341
void clear() override
Clear all internal planning datastructures. Planner settings are not affected. Subsequent calls to so...
Definition: AnytimePathShortening.cpp:240
LogLevel getLogLevel()
Retrieve the current level of logging data. Messages with lower logging levels will not be recorded.
Definition: Console.cpp:142
void setPlanners(const std::string &plannerList)
Set the list of planners to use.
Definition: AnytimePathShortening.cpp:346
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns....
Definition: PlannerTerminationCondition.cpp:172
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
~AnytimePathShortening() override
Destructor.
static base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SelfConfig.cpp:243
void addPlanner(base::PlannerPtr &planner)
Adds the given planner to the set of planners used to compute candidate paths.
Definition: AnytimePathShortening.cpp:98
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Method that solves the motion planning problem. This method terminates under just two conditions,...
Definition: AnytimePathShortening.cpp:134
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82
void setShortcut(bool shortcut)
Enable/disable shortcutting on paths.
Definition: AnytimePathShortening.cpp:306
void checkValidity() override
Check to see if the planners are in a working state (setup has been called, a goal was set,...
Definition: AnytimePathShortening.cpp:284