37 #include "ompl/geometric/planners/rrt/RRTConnect.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
52 ompl::geometric::RRTConnect::~RRTConnect(
void)
64 tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
66 tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
73 std::vector<Motion*> motions;
77 tStart_->list(motions);
78 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
80 if (motions[i]->state)
81 si_->freeState(motions[i]->state);
88 tGoal_->list(motions);
89 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
91 if (motions[i]->state)
92 si_->freeState(motions[i]->state);
107 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
113 Motion *nmotion = tree->nearest(rmotion);
120 double d = si_->distance(nmotion->state, rmotion->state);
121 if (d > maxDistance_)
123 si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
130 bool validMotion = tgi.start ? si_->checkMotion(nmotion->state, dstate) : si_->getStateValidityChecker()->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
136 si_->copyState(motion->state, dstate);
137 motion->parent = nmotion;
138 motion->root = nmotion->root;
139 tgi.xmotion = motion;
158 OMPL_ERROR(
"%s: Unknown type of goal", getName().c_str());
165 si_->copyState(motion->state, st);
166 motion->root = motion->state;
167 tStart_->add(motion);
170 if (tStart_->size() == 0)
172 OMPL_ERROR(
"%s: Motion planning start tree could not be initialized!", getName().c_str());
178 OMPL_ERROR(
"%s: Insufficient states in sampleable goal region", getName().c_str());
183 sampler_ = si_->allocStateSampler();
185 OMPL_INFORM(
"%s: Starting with %d states", getName().c_str(), (
int)(tStart_->size() + tGoal_->size()));
188 tgi.xstate = si_->allocState();
192 bool startTree =
true;
197 TreeData &tree = startTree ? tStart_ : tGoal_;
198 tgi.start = startTree;
199 startTree = !startTree;
200 TreeData &otherTree = startTree ? tStart_ : tGoal_;
202 if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
204 const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
208 si_->copyState(motion->state, st);
209 motion->root = motion->state;
213 if (tGoal_->size() == 0)
215 OMPL_ERROR(
"%s: Unable to sample any valid states for goal tree", getName().c_str());
221 sampler_->sampleUniform(rstate);
223 GrowState gs = growTree(tree, tgi, rmotion);
228 Motion *addedMotion = tgi.xmotion;
234 si_->copyState(rstate, tgi.xstate);
237 tgi.start = startTree;
238 while (gsc == ADVANCED)
239 gsc = growTree(otherTree, tgi, rmotion);
241 Motion *startMotion = startTree ? tgi.xmotion : addedMotion;
242 Motion *goalMotion = startTree ? addedMotion : tgi.xmotion;
250 if (startMotion->parent)
251 startMotion = startMotion->parent;
253 goalMotion = goalMotion->parent;
255 connectionPoint_ = std::make_pair(startMotion->state, goalMotion->state);
258 Motion *solution = startMotion;
259 std::vector<Motion*> mpath1;
260 while (solution != NULL)
262 mpath1.push_back(solution);
263 solution = solution->parent;
266 solution = goalMotion;
267 std::vector<Motion*> mpath2;
268 while (solution != NULL)
270 mpath2.push_back(solution);
271 solution = solution->parent;
275 path->
getStates().reserve(mpath1.size() + mpath2.size());
276 for (
int i = mpath1.size() - 1 ; i >= 0 ; --i)
277 path->
append(mpath1[i]->state);
278 for (
unsigned int i = 0 ; i < mpath2.size() ; ++i)
279 path->
append(mpath2[i]->state);
288 si_->freeState(tgi.xstate);
289 si_->freeState(rstate);
292 OMPL_INFORM(
"%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());
299 Planner::getPlannerData(data);
301 std::vector<Motion*> motions;
303 tStart_->list(motions);
305 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
307 if (motions[i]->parent == NULL)
318 tGoal_->list(motions);
320 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
322 if (motions[i]->parent == NULL)
RRTConnect(const base::SpaceInformationPtr &si)
Constructor.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
The planner failed to find a solution.
void setRange(double distance)
Set the range the planner is supposed to use.
double getRange(void) const
Get the range the planner is using.
GoalType recognizedGoal
The type of goal specification the planner can use.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
boost::shared_ptr< NearestNeighbors< Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
std::vector< base::State * > & getStates(void)
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
double maxDistance_
The maximum length of a motion to be added to a tree.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Invalid start state or no start state specified.
Abstract definition of a goal region that can be sampled.
The goal is of a type that a planner does not recognize.
Representation of a motion.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
The planner found an exact solution.
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
A class to store the exit status of Planner::solve()
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...
virtual bool isStartGoalPairValid(const State *, const State *) const
Since there can be multiple starting states (and multiple goal states) it is possible certain pairs a...
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
GrowState
The state of the tree after an attempt to extend it.
virtual bool couldSample(void) const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
void freeMemory(void)
Free the memory allocated by this planner.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
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 of a geometric path.
Information attached to growing a tree of motions (used internally)
GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
Grow a tree towards a random state.
A boost shared pointer wrapper for ompl::base::Path.
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.