Beispiel #1
0
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));
}
Beispiel #2
0
Datei: SBL.cpp Projekt: ompl/ompl
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));
}
Beispiel #3
0
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));
}
Beispiel #4
0
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));
}