bool ompl::geometric::SBL::checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion,
std::vector<Motion *> &solution)
{
Grid<MotionInfo>::Coord coord(projectionEvaluator_->getDimension());
projectionEvaluator_->computeCoordinates(motion->state, coord);
Grid<MotionInfo>::Cell *cell = otherTree.grid.getCell(coord);
if (cell && !cell->data.empty())
{
Motion *connectOther = cell->data[rng_.uniformInt(0, cell->data.size() - 1)];
if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root,
start ? connectOther->root : motion->root))
{
auto *connect = new Motion(si_);
si_->copyState(connect->state, connectOther->state);
connect->parent = motion;
connect->root = motion->root;
motion->children.push_back(connect);
addMotion(tree, connect);
if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
{
if (start)
connectionPoint_ = std::make_pair(motion->state, connectOther->state);
else
connectionPoint_ = std::make_pair(connectOther->state, motion->state);
/* extract the motions and put them in solution vector */
std::vector<Motion *> mpath1;
while (motion != nullptr)
{
mpath1.push_back(motion);
motion = motion->parent;
}
std::vector<Motion *> mpath2;
while (connectOther != nullptr)
{
mpath2.push_back(connectOther);
connectOther = connectOther->parent;
}
if (!start)
mpath1.swap(mpath2);
for (int i = mpath1.size() - 1; i >= 0; --i)
solution.push_back(mpath1[i]);
solution.insert(solution.end(), mpath2.begin(), mpath2.end());
return true;
}
}
}
return false;
}
LRESULT FolderTree::OnChecked(HTREEITEM hItem, BOOL &bHandled)
{
FolderTreeItemInfo* pItem = (FolderTreeItemInfo*) GetItemData(hItem);
if(!isPathValid(pItem->m_sFQPath))
{
// no checking myComp or network
bHandled = TRUE;
return 1;
}
HTREEITEM hSharedParent = HasSharedParent(hItem);
// if a parent is checked then this folder should be removed from the ex list
if(hSharedParent != NULL)
{
ShareParentButNotSiblings(hItem);
}
else
{
// if no parent folder is checked then this is a new root dir
tstring path = pItem->m_sFQPath;
if (!sp->addDirectory(path)) {
bHandled = TRUE;
return 1;
}
UpdateParentItems(hItem);
}
UpdateChildItems(hItem, true);
return 0;
}
ompl::base::PlannerStatus ompl::geometric::LBKPIECE1::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
if (!goal)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
Discretization<Motion>::Coord xcoord;
while (const base::State *st = pis_.nextStart())
{
Motion* motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = st;
motion->valid = true;
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
dStart_.addMotion(motion, xcoord);
}
if (dStart_.getMotionCount() == 0)
{
OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!goal->couldSample())
{
OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
OMPL_INFORM("%s: Starting with %d states", getName().c_str(), (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
base::State *xstate = si_->allocState();
bool startTree = true;
bool solved = false;
while (ptc == false)
{
Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
startTree = !startTree;
Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
disc.countIteration();
// if we have not sampled too many goals already
if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
{
const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
{
Motion* motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
motion->valid = true;
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
dGoal_.addMotion(motion, xcoord);
}
if (dGoal_.getMotionCount() == 0)
{
OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
break;
}
}
Discretization<Motion>::Cell *ecell = NULL;
Motion *existing = NULL;
disc.selectMotion(existing, ecell);
assert(existing);
sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
/* create a motion */
Motion* motion = new Motion(si_);
si_->copyState(motion->state, xstate);
motion->parent = existing;
motion->root = existing->root;
existing->children.push_back(motion);
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
disc.addMotion(motion, xcoord);
/* attempt to connect trees */
Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord);
if (ocell && !ocell->data->motions.empty())
{
Motion* connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];
if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root))
{
Motion* connect = new Motion(si_);
si_->copyState(connect->state, connectOther->state);
connect->parent = motion;
connect->root = motion->root;
motion->children.push_back(connect);
projectionEvaluator_->computeCoordinates(connect->state, xcoord);
disc.addMotion(connect, xcoord);
if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
{
if (startTree)
connectionPoint_ = std::make_pair(connectOther->state, motion->state);
else
connectionPoint_ = std::make_pair(motion->state, connectOther->state);
/* extract the motions and put them in solution vector */
std::vector<Motion*> mpath1;
while (motion != NULL)
{
mpath1.push_back(motion);
motion = motion->parent;
}
std::vector<Motion*> mpath2;
while (connectOther != NULL)
{
mpath2.push_back(connectOther);
connectOther = connectOther->parent;
}
if (startTree)
mpath1.swap(mpath2);
PathGeometric *path = new PathGeometric(si_);
path->getStates().reserve(mpath1.size() + mpath2.size());
for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
path->append(mpath1[i]->state);
for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
path->append(mpath2[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
solved = true;
break;
}
}
}
}
si_->freeState(xstate);
OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
getName().c_str(),
dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
