本文整理汇总了C++中base::PlannerData::vertexIndex方法的典型用法代码示例。如果您正苦于以下问题：C++ PlannerData::vertexIndex方法的具体用法？C++ PlannerData::vertexIndex怎么用？C++ PlannerData::vertexIndex使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类base::PlannerData的用法示例。
在下文中一共展示了PlannerData::vertexIndex方法的4个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1:
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));
}示例2:
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));
}示例3:
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));
}示例4:
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));
}