CForest.cpp
41 ompl::geometric::CForest::CForest(const base::SpaceInformationPtr &si) : base::Planner(si, "CForest")
52 Planner::declareParam<unsigned int>("num_threads", this, &CForest::setNumThreads, &CForest::getNumThreads, "0:64");
74 OMPL_WARN("%s cannot report intermediate solutions, not added as CForest planner.", planner->getName().c_str());
144 OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
152 OMPL_INFORM("%s: Number and type of instances not specified. Defaulting to %d instances of RRTstar.", getName().c_str(), numThreads_);
160 // This call is needed to make sure the ParamSet is up to date after changes induced by the planner setup calls above, via the state space wrappers for CForest.
164 ompl::base::PlannerStatus ompl::geometric::CForest::solve(const base::PlannerTerminationCondition &ptc)
170 const base::ReportIntermediateSolutionFn prevSolutionCallback = getProblemDefinition()->getIntermediateSolutionCallback();
173 OMPL_WARN("Cannot use previously set intermediate solution callback with %s", getName().c_str());
175 pdef_->setIntermediateSolutionCallback(boost::bind(&CForest::newSolutionFound, this, _1, _2, _3));
209 void ompl::geometric::CForest::newSolutionFound(const base::Planner *planner, const std::vector<const base::State *> &states, const base::Cost cost)
222 for (std::vector<const base::State *>::const_iterator st = states.begin(); st != states.end(); ++st)
238 base::CForestStateSampler *sampler = static_cast<base::CForestStateSampler*>(samplers_[i].get());
239 const base::CForestStateSpaceWrapper *space = static_cast<const base::CForestStateSpaceWrapper*>(sampler->getStateSpace());
246 void ompl::geometric::CForest::solve(base::Planner *planner, const base::PlannerTerminationCondition &ptc)
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: CForest.cpp:137
State space wrapper to use together with CForest. It adds some functionalities to the regular state s...
Definition: CForestStateSpaceWrapper.h:55
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
bool markStartState(const State *st)
Mark the given state as a start vertex. If the given state does not exist in a vertex, false is returned.
Definition: PlannerData.cpp:589
unsigned int numGoalVertices() const
Returns the number of goal vertices.
Definition: PlannerData.cpp:341
const PlannerDataVertex & getStartVertex(unsigned int i) const
Retrieve a reference to the ith start vertex object. If i is greater than the number of start vertice...
Definition: PlannerData.cpp:372
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:112
std::string getNumStatesShared() const
Get number of states actually shared by the algorithm.
Definition: CForest.cpp:204
bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const
Returns the weight of the edge between the given vertex indices. If there exists an edge between v1 a...
Definition: PlannerData.cpp:158
unsigned int getIncomingEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of vertices with outgoing edges to the vertex with index v. The number of edges connec...
Definition: PlannerData.cpp:133
void setNumThreads(unsigned int numThreads=0)
Set default number of threads to use when no planner instances are specified by the user...
Definition: CForest.cpp:66
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: CForest.cpp:119
Extended state sampler to use with the CForest planning algorithm. It wraps the user-specified state ...
Definition: CForestStateSampler.h:51
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
void setStatesToSample(const std::vector< const State * > &states)
Fills the vector StatesToSample_ of states to be sampled in the next calls to sampleUniform(), sampleUniformNear() or sampleGaussian().
Definition: CForestStateSampler.cpp:63
A boost shared pointer wrapper for ompl::base::Planner.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: CForest.cpp:84
const PlannerDataVertex & getGoalVertex(unsigned int i) const
Retrieve a reference to the ith goal vertex object. If i is greater than the number of goal vertices...
Definition: PlannerData.cpp:388
std::string getNumPathsShared() const
Get number of paths shared by the algorithm.
Definition: CForest.cpp:199
boost::function< void(const Planner *, const std::vector< const base::State * > &, const Cost)> ReportIntermediateSolutionFn
When a planner has an intermediate solution (e.g., optimizing planners), a function with this signatu...
Definition: ProblemDefinition.h:139
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
Definition: PlannerData.cpp:203
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
Definition: CForestStateSpaceWrapper.cpp:54
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:86
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: CForest.cpp:164
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Definition: PlannerData.cpp:444
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist...
Definition: PlannerData.cpp:213
An optimization objective which corresponds to optimizing path length.
Definition: PathLengthOptimizationObjective.h:47
virtual PlannerStatus solve(const PlannerTerminationCondition &ptc)=0
Function that can solve the motion planning problem. This function can be called multiple times on th...
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2. If this edge does not exist, NO_EDGE is returned.
Definition: PlannerData.cpp:231
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex, false is returned.
Definition: PlannerData.cpp:606
virtual void getPlannerData(PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: Planner.cpp:118
virtual void setTag(int tag)
Set the integer tag associated with this vertex.
Definition: PlannerData.h:72
unsigned int numStartVertices() const
Returns the number of start vertices.
Definition: PlannerData.cpp:336
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:74
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47