37 #include "ompl/control/planners/syclop/SyclopRRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
44 sampler_ =
si_->allocStateSampler();
50 if (!nn_ && !regionalNN_)
52 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(
si_->getStateSpace()));
63 lastGoalMotion_ = NULL;
68 Planner::getPlannerData(data);
69 std::vector<Motion*> motions;
72 double delta = siC_->getPropagationStepSize();
77 for (
size_t i = 0; i < motions.size(); ++i)
79 if (motions[i]->parent)
97 si_->copyState(motion->
state, s);
98 siC_->nullControl(motion->
control);
109 decomp_->sampleFromRegion(region.
index, sampler, rmotion->
state);
116 std::vector<int> searchRegions;
117 decomp_->getNeighbors(region.
index, searchRegions);
118 searchRegions.push_back(region.
index);
120 std::vector<Motion*> motions;
121 for (std::vector<int>::const_iterator i = searchRegions.begin(); i != searchRegions.end(); ++i)
123 const std::vector<Motion*>& regionMotions = getRegionFromIndex(*i).motions;
124 motions.insert(motions.end(), regionMotions.begin(), regionMotions.end());
127 std::vector<Motion*>::const_iterator i = motions.begin();
129 double minDistance = distanceFunction(rmotion, nmotion);
131 while (i != motions.end())
134 const double dist = distanceFunction(rmotion, m);
135 if (dist < minDistance)
146 nmotion = nn_->nearest(rmotion);
151 unsigned int duration = controlSampler_->sampleTo(rmotion->
control, nmotion->
control, nmotion->
state, rmotion->
state);
153 duration = siC_->propagateWhileValid(nmotion->
state, rmotion->
control, duration, newState);
155 if (duration >= siC_->getMinControlDuration())
158 si_->copyState(motion->
state, newState);
160 motion->
steps = duration;
162 newMotions.push_back(motion);
165 lastGoalMotion_ = motion;
168 si_->freeState(rmotion->
state);
169 siC_->freeControl(rmotion->
control);
171 si_->freeState(newState);
178 std::vector<Motion*> motions;
180 for (std::vector<Motion*>::iterator i = motions.begin(); i != motions.end(); ++i)
184 si_->freeState(m->
state);