37 #include "ompl/control/planners/rrt/RRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/datastructures/NearestNeighborsGNAT.h"
55 ompl::control::RRT::~RRT(
void)
72 controlSampler_.reset();
76 lastGoalMotion_ = NULL;
83 std::vector<Motion*> motions;
85 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
87 if (motions[i]->state)
88 si_->freeState(motions[i]->state);
89 if (motions[i]->control)
90 siC_->freeControl(motions[i]->control);
105 si_->copyState(motion->
state, st);
106 siC_->nullControl(motion->
control);
110 if (nn_->size() == 0)
112 logError(
"There are no valid initial states!");
117 sampler_ = si_->allocStateSampler();
118 if (!controlSampler_)
119 controlSampler_ = siC_->allocDirectedControlSampler();
121 logInform(
"Starting with %u states", nn_->size());
125 double approxdif = std::numeric_limits<double>::infinity();
132 while (ptc() ==
false)
135 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
138 sampler_->sampleUniform(rstate);
141 Motion *nmotion = nn_->nearest(rmotion);
144 unsigned int cd = controlSampler_->sampleTo(rctrl, nmotion->
control, nmotion->
state, rmotion->
state);
146 if (addIntermediateStates_)
149 std::vector<base::State *> pstates;
150 cd = siC_->propagateWhileValid(nmotion->
state, rctrl, cd, pstates,
true);
152 if (cd >= siC_->getMinControlDuration())
154 Motion *lastmotion = nmotion;
157 for ( ; p < pstates.size(); ++p)
161 motion->
state = pstates[p];
163 motion->
control = siC_->allocControl();
164 siC_->copyControl(motion->
control, rctrl);
166 motion->
parent = lastmotion;
177 if (dist < approxdif)
185 while (++p < pstates.size())
186 si_->freeState(pstates[p]);
191 for (
size_t p = 0 ; p < pstates.size(); ++p)
192 si_->freeState(pstates[p]);
196 cd = siC_->propagateWhileValid(nmotion->
state, rctrl, cd, xstate);
198 if (cd >= siC_->getMinControlDuration())
202 si_->copyState(motion->
state, xstate);
203 siC_->copyControl(motion->
control, rctrl);
216 if (dist < approxdif)
226 bool approximate =
false;
227 if (solution == NULL)
229 solution = approxsol;
233 if (solution != NULL)
235 lastGoalMotion_ = solution;
238 std::vector<Motion*> mpath;
239 while (solution != NULL)
241 mpath.push_back(solution);
242 solution = solution->parent;
247 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
248 if (mpath[i]->parent)
249 path->
append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
251 path->
append(mpath[i]->state);
253 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxdif);
257 si_->freeState(rmotion->
state);
259 siC_->freeControl(rmotion->
control);
261 si_->freeState(xstate);
263 logInform(
"Created %u states", nn_->size());
270 Planner::getPlannerData(data);
272 std::vector<Motion*> motions;
276 double delta = siC_->getPropagationStepSize();
281 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
283 const Motion* m = motions[i];
286 if (data.hasControls())