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(
"Unknown type of goal (or goal undefined)");
165 si_->copyState(motion->state, st);
166 motion->root = motion->state;
167 tStart_->add(motion);
170 if (tStart_->size() == 0)
172 OMPL_ERROR(
"Motion planning start tree could not be initialized!");
178 OMPL_ERROR(
"Insufficient states in sampleable goal region");
183 sampler_ = si_->allocStateSampler();
185 OMPL_INFORM(
"Starting with %d states", (
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(
"Unable to sample any valid states for goal tree");
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(
"Created %u states (%u start + %u goal)", 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)