37 #include "ompl/geometric/planners/sbl/SBL.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
52 ompl::geometric::SBL::~SBL(
void)
64 tStart_.grid.setDimension(projectionEvaluator_->getDimension());
65 tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
72 for (
unsigned int i = 0 ; i < it->second->data.size() ; ++i)
74 if (it->second->data[i]->state)
75 si_->freeState(it->second->data[i]->state);
76 delete it->second->data[i];
88 OMPL_ERROR(
"Unknown type of goal (or goal undefined)");
95 si_->copyState(motion->
state, st);
98 addMotion(tStart_, motion);
101 if (tStart_.size == 0)
103 OMPL_ERROR(
"Motion planning start tree could not be initialized!");
109 OMPL_ERROR(
"Insufficient states in sampleable goal region");
114 sampler_ = si_->allocValidStateSampler();
116 OMPL_INFORM(
"Starting with %d states", (
int)(tStart_.size + tGoal_.size));
118 std::vector<Motion*> solution;
121 bool startTree =
true;
126 TreeData &tree = startTree ? tStart_ : tGoal_;
127 startTree = !startTree;
128 TreeData &otherTree = startTree ? tStart_ : tGoal_;
131 if (tGoal_.size == 0 || pis_.getSampledGoalsCount() < tGoal_.size / 2)
133 const base::State *st = tGoal_.size == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
137 si_->copyState(motion->
state, st);
139 motion->
valid =
true;
140 addMotion(tGoal_, motion);
142 if (tGoal_.size == 0)
144 OMPL_ERROR(
"Unable to sample any valid states for goal tree");
149 Motion *existing = selectMotion(tree);
151 if (!sampler_->sampleNear(xstate, existing->
state, maxDistance_))
156 si_->copyState(motion->
state, xstate);
157 motion->
parent = existing;
159 existing->
children.push_back(motion);
161 addMotion(tree, motion);
163 if (checkSolution(!startTree, tree, otherTree, motion, solution))
166 for (
unsigned int i = 0 ; i < solution.size() ; ++i)
167 path->
append(solution[i]->state);
175 si_->freeState(xstate);
177 OMPL_INFORM(
"Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", tStart_.size + tGoal_.size, tStart_.size, tGoal_.size,
178 tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size());
186 projectionEvaluator_->computeCoordinates(motion->
state, coord);
189 if (cell && !cell->
data.empty())
191 Motion *connectOther = cell->
data[rng_.uniformInt(0, cell->
data.size() - 1)];
193 if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->
root : connectOther->
root, start ? connectOther->
root : motion->
root))
197 si_->copyState(connect->
state, connectOther->
state);
200 motion->
children.push_back(connect);
201 addMotion(tree, connect);
203 if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
206 connectionPoint_ = std::make_pair(motion->
state, connectOther->
state);
208 connectionPoint_ = std::make_pair(connectOther->
state, motion->
state);
212 std::vector<Motion*> mpath1;
213 while (motion != NULL)
215 mpath1.push_back(motion);
219 std::vector<Motion*> mpath2;
220 while (connectOther != NULL)
222 mpath2.push_back(connectOther);
223 connectOther = connectOther->
parent;
229 for (
int i = mpath1.size() - 1 ; i >= 0 ; --i)
230 solution.push_back(mpath1[i]);
231 solution.insert(solution.end(), mpath2.begin(), mpath2.end());
242 std::vector<Motion*> mpath;
245 while (motion != NULL)
247 mpath.push_back(motion);
252 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
253 if (!mpath[i]->valid)
255 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
256 mpath[i]->valid =
true;
259 removeMotion(tree, mpath[i]);
269 return cell && !cell->
data.empty() ? cell->
data[rng_.uniformInt(0, cell->
data.size() - 1)] : NULL;
277 projectionEvaluator_->computeCoordinates(motion->
state, coord);
281 for (
unsigned int i = 0 ; i < cell->
data.size(); ++i)
283 if (cell->
data[i] == motion)
285 cell->
data.erase(cell->
data.begin() + i);
290 if (cell->
data.empty())
293 tree.
grid.remove(cell);
294 tree.
grid.destroyCell(cell);
306 for (
unsigned int i = 0 ; i < motion->
parent->
children.size() ; ++i)
317 for (
unsigned int i = 0 ; i < motion->
children.size() ; ++i)
320 removeMotion(tree, motion->
children[i]);
324 si_->freeState(motion->
state);
331 projectionEvaluator_->computeCoordinates(motion->
state, coord);
335 cell->
data.push_back(motion);
340 cell = tree.
grid.createCell(coord);
341 cell->
data.push_back(motion);
356 tStart_.grid.clear();
363 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
368 Planner::getPlannerData(data);
370 std::vector<MotionInfo> motions;
371 tStart_.grid.getContent(motions);
373 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
374 for (
unsigned int j = 0 ; j < motions[i].size() ; ++j)
375 if (motions[i][j]->parent == NULL)
382 tGoal_.grid.getContent(motions);
383 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
384 for (
unsigned int j = 0 ; j < motions[i].size() ; ++j)
385 if (motions[i][j]->parent == NULL)