void ompl::geometric::pSBL::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<MotionInfo> motions;
tStart_.grid.getContent(motions);
for (unsigned int i = 0 ; i < motions.size() ; ++i)
for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
if (motions[i][j]->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state, 1));
else
data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state, 1),
base::PlannerDataVertex(motions[i][j]->state, 1));
motions.clear();
tGoal_.grid.getContent(motions);
for (unsigned int i = 0 ; i < motions.size() ; ++i)
for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
if (motions[i][j]->parent == nullptr)
data.addGoalVertex(base::PlannerDataVertex(motions[i][j]->state, 2));
else
// The edges in the goal tree are reversed so that they are in the same direction as start tree
data.addEdge(base::PlannerDataVertex(motions[i][j]->state, 2),
base::PlannerDataVertex(motions[i][j]->parent->state, 2));
data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
}
void ompl::geometric::SBL::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<MotionInfo> motionInfo;
tStart_.grid.getContent(motionInfo);
for (auto &m : motionInfo)
for (auto &motion : m.motions_)
if (motion->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
else
data.addEdge(base::PlannerDataVertex(motion->parent->state, 1),
base::PlannerDataVertex(motion->state, 1));
motionInfo.clear();
tGoal_.grid.getContent(motionInfo);
for (auto &m : motionInfo)
for (auto &motion : m.motions_)
if (motion->parent == nullptr)
data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
else
// The edges in the goal tree are reversed so that they are in the same direction as start tree
data.addEdge(base::PlannerDataVertex(motion->state, 2),
base::PlannerDataVertex(motion->parent->state, 2));
data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
}
void ompl::geometric::TRRTConnect::getPlannerData(base::PlannerData &data) const {
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (tStart_) tStart_->list(motions);
for (std::size_t i(0); i < motions.size(); ++i) {
if (!motions[i]->parent) {
data.addStartVertex(base::PlannerDataVertex(motions[i]->state,1));
} else {
data.addEdge(base::PlannerDataVertex(motions[i]->parent->state,1),
base::PlannerDataVertex(motions[i]->state,1));
}
}
motions.clear();
if (tGoal_) tGoal_->list(motions);
for (std::size_t i(0); i < motions.size(); ++i) {
if (!motions[i]->parent) {
data.addGoalVertex(base::PlannerDataVertex(motions[i]->state,2));
} else {
//The edges in the goal tree are reversed to be consistent with start tree
data.addEdge(base::PlannerDataVertex(motions[i]->state,2),
base::PlannerDataVertex(motions[i]->parent->state,2));
}
}
//Add the edge connecting the two trees
data.addEdge(data.vertexIndex(connectionPoint_.first),
data.vertexIndex(connectionPoint_.second));
}
void ompl::geometric::LBKPIECE1::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
dStart_.getPlannerData(data, 1, true, NULL);
dGoal_.getPlannerData(data, 2, false, NULL);
// Insert the edge connecting the two trees
data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
}