37 #include "ompl/geometric/planners/rrt/RRTConnect.h"
38 #include "ompl/datastructures/NearestNeighborsGNAT.h"
39 #include "ompl/base/goals/GoalSampleableRegion.h"
40 #include "ompl/tools/config/SelfConfig.h"
53 ompl::geometric::RRTConnect::~RRTConnect(
void)
74 std::vector<Motion*> motions;
78 tStart_->list(motions);
79 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
81 if (motions[i]->state)
82 si_->freeState(motions[i]->state);
89 tGoal_->list(motions);
90 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
92 if (motions[i]->state)
93 si_->freeState(motions[i]->state);
108 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
114 Motion *nmotion = tree->nearest(rmotion);
121 double d = si_->distance(nmotion->state, rmotion->state);
122 if (d > maxDistance_)
124 si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
131 bool validMotion = tgi.start ? si_->checkMotion(nmotion->state, dstate) : si_->getStateValidityChecker()->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
137 si_->copyState(motion->state, dstate);
138 motion->parent = nmotion;
139 motion->root = nmotion->root;
140 tgi.xmotion = motion;
159 logError(
"Unknown type of goal (or goal undefined)");
166 si_->copyState(motion->state, st);
167 motion->root = motion->state;
168 tStart_->add(motion);
171 if (tStart_->size() == 0)
173 logError(
"Motion planning start tree could not be initialized!");
179 logError(
"Insufficient states in sampleable goal region");
184 sampler_ = si_->allocStateSampler();
186 logInform(
"Starting with %d states", (
int)(tStart_->size() + tGoal_->size()));
189 tgi.xstate = si_->allocState();
193 bool startTree =
true;
196 while (ptc() ==
false)
198 TreeData &tree = startTree ? tStart_ : tGoal_;
199 tgi.start = startTree;
200 startTree = !startTree;
201 TreeData &otherTree = startTree ? tStart_ : tGoal_;
203 if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
205 const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
209 si_->copyState(motion->state, st);
210 motion->root = motion->state;
214 if (tGoal_->size() == 0)
216 logError(
"Unable to sample any valid states for goal tree");
222 sampler_->sampleUniform(rstate);
224 GrowState gs = growTree(tree, tgi, rmotion);
229 Motion *addedMotion = tgi.xmotion;
235 si_->copyState(rstate, tgi.xstate);
238 tgi.start = startTree;
239 while (gsc == ADVANCED)
240 gsc = growTree(otherTree, tgi, rmotion);
242 Motion *startMotion = startTree ? tgi.xmotion : addedMotion;
243 Motion *goalMotion = startTree ? addedMotion : tgi.xmotion;
251 if (startMotion->parent)
252 startMotion = startMotion->parent;
254 goalMotion = goalMotion->parent;
256 connectionPoint_ = std::make_pair<base::State*, base::State*>(startMotion->state, goalMotion->state);
259 Motion *solution = startMotion;
260 std::vector<Motion*> mpath1;
261 while (solution != NULL)
263 mpath1.push_back(solution);
264 solution = solution->parent;
267 solution = goalMotion;
268 std::vector<Motion*> mpath2;
269 while (solution != NULL)
271 mpath2.push_back(solution);
272 solution = solution->parent;
276 path->
getStates().reserve(mpath1.size() + mpath2.size());
277 for (
int i = mpath1.size() - 1 ; i >= 0 ; --i)
278 path->
append(mpath1[i]->state);
279 for (
unsigned int i = 0 ; i < mpath2.size() ; ++i)
280 path->
append(mpath2[i]->state);
289 si_->freeState(tgi.xstate);
290 si_->freeState(rstate);
293 logInform(
"Created %u states (%u start + %u goal)", tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());
300 Planner::getPlannerData(data);
302 std::vector<Motion*> motions;
304 tStart_->list(motions);
306 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
308 if (motions[i]->parent == NULL)
319 tGoal_->list(motions);
321 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
323 if (motions[i]->parent == NULL)