C++ (Cpp) PlannerData::addEdge示例

示例#1

文件： EST.cpp 项目： jdmartin86/ompl

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));
        }
}

示例#2

文件： TRRTConnect.cpp 项目： iocroblab/kautham

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));
}

示例#3

文件： SyclopRRT.cpp 项目： davetcoleman/ompl

void ompl::control::SyclopRRT::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);
    std::vector<Motion *> motions;
    if (nn_)
        nn_->list(motions);
    double delta = siC_->getPropagationStepSize();

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));

    for (auto &motion : motions)
    {
        if (motion->parent)
        {
            if (data.hasControls())
                data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state),
                             control::PlannerDataEdgeControl(motion->control, motion->steps * delta));
            else
                data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
        }
        else
            data.addStartVertex(base::PlannerDataVertex(motion->state));
    }
}

示例#4

文件： SyclopEST.cpp 项目： HRZaheri/batch-informed-trees

void ompl::control::SyclopEST::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    double delta = siC_->getPropagationStepSize();

    if (lastGoalMotion_)
        data.addGoalVertex(lastGoalMotion_->state);

    for (size_t i = 0; i < motions_.size(); ++i)
    {
        if (motions_[i]->parent)
        {
            if (data.hasControls())
                data.addEdge (base::PlannerDataVertex(motions_[i]->parent->state),
                              base::PlannerDataVertex(motions_[i]->state),
                              control::PlannerDataEdgeControl (motions_[i]->control, motions_[i]->steps * delta));
            else
                data.addEdge (base::PlannerDataVertex(motions_[i]->parent->state),
                              base::PlannerDataVertex(motions_[i]->state));
        }
        else
            data.addStartVertex (base::PlannerDataVertex(motions_[i]->state));
    }
}

示例#5

文件： pSBL.cpp 项目： RickOne16/ompl

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));
}

示例#6

文件： SBL.cpp 项目： 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));
}

示例#7

文件： KPIECE1.cpp 项目： mpomarlan/ompl_slprm

void ompl::control::KPIECE1::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    Grid::CellArray cells;
    tree_.grid.getCells(cells);

    double delta = siC_->getPropagationStepSize();

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));

    for (unsigned int i = 0 ; i < cells.size() ; ++i)
    {
        for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
        {
            const Motion* m = cells[i]->data->motions[j];
            if (m->parent)
            {
                if (data.hasControls())
                    data.addEdge(base::PlannerDataVertex (m->parent->state),
                                 base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1),
                                 control::PlannerDataEdgeControl (m->control, m->steps * delta));
                else
                    data.addEdge(base::PlannerDataVertex (m->parent->state),
                                 base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1));
            }
            else
                data.addStartVertex(base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1));

            // A state created as a parent first may have an improper tag variable
            data.tagState(m->state, cells[i]->border ? 2 : 1);
        }
    }
}

示例#8

文件： EST.cpp 项目： ompl/ompl

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));
        }
}

示例#9

文件： LazyPRM.cpp 项目： HRZaheri/batch-informed-trees

void ompl::geometric::LazyPRM::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    // Explicitly add start and goal states. Tag all states known to be valid as 1.
    // Unchecked states are tagged as 0.
    for (size_t i = 0; i < startM_.size(); ++i)
        data.addStartVertex(base::PlannerDataVertex(stateProperty_[startM_[i]], 1));

    for (size_t i = 0; i < goalM_.size(); ++i)
        data.addGoalVertex(base::PlannerDataVertex(stateProperty_[goalM_[i]], 1));

    // Adding edges and all other vertices simultaneously
    foreach(const Edge e, boost::edges(g_))
    {
        const Vertex v1 = boost::source(e, g_);
        const Vertex v2 = boost::target(e, g_);
        data.addEdge(base::PlannerDataVertex(stateProperty_[v1]),
                     base::PlannerDataVertex(stateProperty_[v2]));

        // Add the reverse edge, since we're constructing an undirected roadmap
        data.addEdge(base::PlannerDataVertex(stateProperty_[v2]),
                     base::PlannerDataVertex(stateProperty_[v1]));

        // Add tags for the newly added vertices
        data.tagState(stateProperty_[v1], (vertexValidityProperty_[v1] & VALIDITY_TRUE) == 0 ? 0 : 1);
        data.tagState(stateProperty_[v2], (vertexValidityProperty_[v2] & VALIDITY_TRUE) == 0 ? 0 : 1);
    }
}

示例#10

文件： RRT.cpp 项目： giogadi/ompl

void ompl::control::RRT::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    if (nn_)
        nn_->list(motions);

    double delta = siC_->getPropagationStepSize();

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));

    for (unsigned int i = 0 ; i < motions.size() ; ++i)
    {
        const Motion* m = motions[i];
        if (m->parent)
        {
            if (data.hasControls())
                data.addEdge(base::PlannerDataVertex(m->parent->state),
                             base::PlannerDataVertex(m->state),
                             control::PlannerDataEdgeControl(m->control, m->steps * delta));
            else
                data.addEdge(base::PlannerDataVertex(m->parent->state),
                             base::PlannerDataVertex(m->state));
        }
        else
            data.addStartVertex(base::PlannerDataVertex(m->state));
    }
}

示例#11

文件： RRTsharp.cpp 项目： EmmanuelBoidot/RRTsharp

void ompl::geometric::RRTsharp::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    if (nn_)
        nn_->list(motions);

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));

    for (std::size_t i = 0 ; i < motions.size() ; ++i)
    {
        if (motions[i]->parent == NULL)
            data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
        else
            data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
                         base::PlannerDataVertex(motions[i]->state));
        // if (visitedMotions.count(motions[i])!=0){
        //     data.tagState(motions[i]->state,1);
        // }
        // if (toVisitMotions.count(motions[i])!=0){
        //     data.tagState(motions[i]->state,2);
        // }
    }
}

示例#12

文件： CForest.cpp 项目： jvgomez/ompl

void ompl::geometric::CForest::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    for (std::size_t i = 0; i < planners_.size(); ++i)
    {
        base::PlannerData pd(si_);
        planners_[i]->getPlannerData(pd);

        for (unsigned int j = 0; j < pd.numVertices(); ++j)
        {
            base::PlannerDataVertex &v = pd.getVertex(j);

            v.setTag(i);
            std::vector<unsigned int> edgeList;
            unsigned int numEdges = pd.getIncomingEdges(j, edgeList);
            for (unsigned int k = 0; k < numEdges; ++k)
            {
                base::Cost edgeWeight;
                base::PlannerDataVertex &w = pd.getVertex(edgeList[k]);

                w.setTag(i);
                pd.getEdgeWeight(j, k, &edgeWeight);
                data.addEdge(v, w, pd.getEdge(j, k), edgeWeight);
            }
        }

        for (unsigned int j = 0; j < pd.numGoalVertices(); ++j)
            data.markGoalState(pd.getGoalVertex(j).getState());

        for (unsigned int j = 0; j < pd.numStartVertices(); ++j)
            data.markStartState(pd.getStartVertex(j).getState());
    }
}

示例#13

文件： SST.cpp 项目： HRZaheri/batch-informed-trees

void ompl::control::SST::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    std::vector<Motion*> allMotions;
    if (nn_)
        nn_->list(motions);

    for(unsigned i=0;i<motions.size();i++)
    {
        if(motions[i]->numChildren_==0)
        {
            allMotions.push_back(motions[i]);
        }
    }
    for(unsigned i=0;i<allMotions.size();i++)
    {
        if(allMotions[i]->parent_!=NULL)
        {
            allMotions.push_back(allMotions[i]->parent_);
        }
    }

    double delta = siC_->getPropagationStepSize();

    if (prevSolution_.size()!=0)
        data.addGoalVertex(base::PlannerDataVertex(prevSolution_[0]));

    for (unsigned int i = 0 ; i < allMotions.size() ; ++i)
    {
        const Motion *m = allMotions[i];
        if (m->parent_)
        {
            if (data.hasControls())
                data.addEdge(base::PlannerDataVertex(m->parent_->state_),
                             base::PlannerDataVertex(m->state_),
                             control::PlannerDataEdgeControl(m->control_, m->steps_ * delta));
            else
                data.addEdge(base::PlannerDataVertex(m->parent_->state_),
                             base::PlannerDataVertex(m->state_));
        }
        else
            data.addStartVertex(base::PlannerDataVertex(m->state_));
    }
}

示例#14

文件： LBKPIECE1.cpp 项目： giogadi/ompl

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));
}

示例#15

文件： RRTstar.cpp 项目： mpomarlan/ompl_slprm

void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    if (nn_)
        nn_->list(motions);

    for (unsigned int i = 0 ; i < motions.size() ; ++i)
        data.addEdge (base::PlannerDataVertex (motions[i]->parent ? motions[i]->parent->state : NULL),
                      base::PlannerDataVertex (motions[i]->state));
}

示例#16

文件： LightningRetrieveRepair.cpp 项目： GrapeTec/ompl

void ompl::geometric::LightningRetrieveRepair::getPlannerData(base::PlannerData &data) const
{
    OMPL_INFORM("LightningRetrieveRepair: including %d similar paths", nearestPaths_.size());

    // Visualize the n candidate paths that we recalled from the database
    for (std::size_t i = 0 ; i < nearestPaths_.size() ; ++i)
    {
        ompl::base::PlannerDataPtr pd = nearestPaths_[i];
        for (std::size_t j = 1; j < pd->numVertices(); ++j)
        {
            data.addEdge(
                         base::PlannerDataVertex(pd->getVertex(j - 1).getState()),
                         base::PlannerDataVertex(pd->getVertex(j).getState()));
        }
    }
}

示例#17

文件： pid_ctrl.cpp 项目： Copilot-Language/copilot-discussion

void ompl::control::PID::getPlannerData(base::PlannerData &data) const
{
	Planner::getPlannerData(data);
	double delta = siC_->getPropagationStepSize();

	if (motions.back())
	{
		data.addGoalVertex(base::PlannerDataVertex(motions.back()->state));
	}

	for (int i = motions.size()-1 ; i >= 1 ; --i)
	{
		const Motion* m = motions[i];
		data.addEdge(base::PlannerDataVertex(motions[i-1]->state), base::PlannerDataVertex(m->state), control::PlannerDataEdgeControl(m->control, delta));
	}
	data.addStartVertex(base::PlannerDataVertex(motions.front()->state));
}

示例#18

文件： pRRT.cpp 项目： ompl/ompl

void ompl::geometric::pRRT::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion *> motions;
    if (nn_)
        nn_->list(motions);

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));

    for (auto &motion : motions)
    {
        if (motion->parent == nullptr)
            data.addStartVertex(base::PlannerDataVertex(motion->state));
        else
            data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
    }
}

示例#19

文件： FMT.cpp 项目： RickOne16/ompl

void ompl::geometric::FMT::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);
    std::vector<Motion*> motions;
    nn_->list(motions);

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->getState()));

    unsigned int size = motions.size();
    for (unsigned int i = 0; i < size; ++i)
    {
        if (motions[i]->getParent() == nullptr)
            data.addStartVertex(base::PlannerDataVertex(motions[i]->getState()));
        else
            data.addEdge(base::PlannerDataVertex(motions[i]->getParent()->getState()),
                         base::PlannerDataVertex(motions[i]->getState()));
    }
}

示例#20

文件： ProjEST.cpp 项目： davetcoleman/ompl

void ompl::geometric::ProjEST::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<MotionInfo> motionInfo;
    tree_.grid.getContent(motionInfo);

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));

    for (auto &m : motionInfo)
        for (auto &motion : m.motions_)
        {
            if (motion->parent == nullptr)
                data.addStartVertex(base::PlannerDataVertex(motion->state));
            else
                data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
        }
}

示例#21

文件： RRTstarAR.cpp 项目： EmmanuelBoidot/alternative-routing

void ompl::geometric::RRTstarAR::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    if (nn_)
        nn_->list(motions);

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));

    for (std::size_t i = 0 ; i < motions.size() ; ++i)
    {
        if (motions[i]->parent == NULL)
            data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
        else
            data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
                         base::PlannerDataVertex(motions[i]->state));
    }
}

示例#22

文件： LBTRRT.cpp 项目： RickOne16/ompl

void ompl::geometric::LBTRRT::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    if (nn_)
        nn_->list(motions);

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state_));

    for (unsigned int i = 0 ; i < motions.size() ; ++i)
    {
        if (motions[i]->parentApx_ == nullptr)
            data.addStartVertex(base::PlannerDataVertex(motions[i]->state_));
        else
            data.addEdge(base::PlannerDataVertex(motions[i]->parentApx_->state_),
                         base::PlannerDataVertex(motions[i]->state_));
    }
}

示例#23

文件： EST.cpp 项目： giogadi/ompl

void ompl::geometric::EST::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<MotionInfo> motions;
    tree_.grid.getContent(motions);

    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 == NULL)
                data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state));
            else
                data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state),
                             base::PlannerDataVertex(motions[i][j]->state));
        }
}

示例#24

文件： LazyRRT.cpp 项目： OspreyX/ompl-informed

void ompl::geometric::LazyRRT::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    if (nn_)
        nn_->list(motions);

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state, 1));

    for (unsigned int i = 0 ; i < motions.size() ; ++i)
    {
        if (motions[i]->parent == NULL)
            data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
        else
            data.addEdge(base::PlannerDataVertex(motions[i]->parent ? motions[i]->parent->state : NULL),
                         base::PlannerDataVertex(motions[i]->state));

        data.tagState(motions[i]->state, motions[i]->valid ? 1 : 0);
    }
}

示例#25

文件： RRTstar.cpp 项目： arjungm/ompl

void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    if (nn_)
        nn_->list(motions);

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));

    for (std::size_t i = 0 ; i < motions.size() ; ++i)
    {
        if (motions[i]->parent == NULL)
            data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
        else
            data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
                         base::PlannerDataVertex(motions[i]->state));
    }
    data.properties["iterations INTEGER"] = boost::lexical_cast<std::string>(iterations_);
    data.properties["collision_checks INTEGER"] =
        boost::lexical_cast<std::string>(collisionChecks_);
}