void ompl::control::PDST::getPlannerData(ompl::base::PlannerData &data) const
{
base::Planner::getPlannerData(data);
double dt = siC_->getPropagationStepSize();
base::State *scratch = si_->allocState();
std::vector<Motion *> motions;
motions.reserve(priorityQueue_.size());
priorityQueue_.getContent(motions);
// Add goal vertex
if (lastGoalMotion_ != nullptr)
data.addGoalVertex(lastGoalMotion_->endState_);
for (auto &motion : motions)
if (!motion->isSplit_)
{
// We only add one edge for each motion that has been split into smaller segments
Motion *cur = motion, *ancestor;
unsigned int duration = findDurationAndAncestor(cur, cur->endState_, scratch, ancestor);
if (cur->parent_ == nullptr)
data.addStartVertex(base::PlannerDataVertex(cur->endState_));
else if (data.hasControls())
{
data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->endState_),
PlannerDataEdgeControl(cur->control_, duration * dt));
if (ancestor->parent_)
{
// Include an edge between start state of parent ancestor motion and
// the start state of the ancestor motion, which lies somewhere on
// the parent ancestor motion.
cur = ancestor;
duration = findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
data.addEdge(base::PlannerDataVertex(ancestor->startState_),
base::PlannerDataVertex(cur->startState_),
PlannerDataEdgeControl(ancestor->control_, duration * dt));
}
}
else
{
data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->endState_));
if (ancestor->parent_)
{
// Include an edge between start state of parent ancestor motion and
// the start state of the ancestor motion, which lies somewhere on
// the parent ancestor motion.
cur = ancestor;
findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
data.addEdge(base::PlannerDataVertex(ancestor->startState_),
base::PlannerDataVertex(cur->startState_));
}
}
}
si_->freeState(scratch);
}
void ompl::control::EST::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<MotionInfo> motions;
tree_.grid.getContent(motions);
double stepSize = siC_->getPropagationStepSize();
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (unsigned int i = 0 ; i < motions.size() ; ++i)
for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
{
if (motions[i][j]->parent)
{
if (data.hasControls())
data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state),
base::PlannerDataVertex (motions[i][j]->state),
PlannerDataEdgeControl(motions[i][j]->control, motions[i][j]->steps * stepSize));
else
data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state),
base::PlannerDataVertex (motions[i][j]->state));
}
else
data.addStartVertex (base::PlannerDataVertex (motions[i][j]->state));
}
}
void ompl::control::EST::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<MotionInfo> motionInfo;
tree_.grid.getContent(motionInfo);
double stepSize = siC_->getPropagationStepSize();
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (auto &mi : motionInfo)
for (auto &motion : mi.motions_)
{
if (motion->parent)
{
if (data.hasControls())
data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state),
PlannerDataEdgeControl(motion->control, motion->steps * stepSize));
else
data.addEdge(base::PlannerDataVertex(motion->parent->state),
base::PlannerDataVertex(motion->state));
}
else
data.addStartVertex(base::PlannerDataVertex(motion->state));
}
}
