Esempi in C++ (Cpp) per PlannerData::recordEdge

Esempio n. 1

File: KPIECE1.cpp Progetto: megan-starr9/UAV_Aiolos

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

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

    if (PlannerData *cpd = dynamic_cast<control::PlannerData*>(&data))
    {
        double delta = siC_->getPropagationStepSize();

        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)
                    cpd->recordEdge(m->parent->state, m->state, m->control, m->steps * delta);
                else
                    cpd->recordEdge(NULL, m->state, NULL, 0.);
                cpd->tagState(m->state, cells[i]->border ? 2 : 1);
            }
    }
    else
    {
        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];
                data.recordEdge(m->parent ? m->parent->state : NULL, m->state);
                data.tagState(m->state, cells[i]->border ? 2 : 1);
            }
    }
}

Esempio n. 2

File: SyclopEST.cpp Progetto: megan-starr9/UAV_Aiolos

void ompl::control::SyclopEST::getPlannerData(base::PlannerData& data) const
{
    Planner::getPlannerData(data);
    if (PlannerData *cpd = dynamic_cast<control::PlannerData*>(&data))
    {
        const double delta = siC_->getPropagationStepSize();

        for (std::vector<Motion*>::const_iterator i = motions_.begin(); i != motions_.end(); ++i)
        {
            const Motion* m = *i;
            if (m->parent)
                cpd->recordEdge(m->parent->state, m->state, m->control, m->steps * delta);
            else
                cpd->recordEdge(NULL, m->state, NULL, 0.);
        }
    }
    else
    {
        for (std::vector<Motion*>::const_iterator i = motions_.begin(); i != motions_.end(); ++i)
        {
            const Motion* m = *i;
            data.recordEdge(m->parent ? m->parent->state : NULL, m->state);
        }
    }
}

Esempio n. 3

File: BasicPRMmodif.cpp Progetto: nicocoaste/newBoundingBoxPlanner

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

    for (unsigned int i = 0 ; i < milestones_.size() ; ++i)
        for (unsigned int j = 0 ; j < milestones_[i]->adjacent.size() ; ++j)
            data.recordEdge(milestones_[i]->state, milestones_[i]->adjacent[j]->state);
}

Esempio n. 4

File: BallTreeRRTstar.cpp Progetto: megan-starr9/UAV_Aiolos

void ompl::geometric::BallTreeRRTstar::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.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
}

Esempio n. 5

File: LazyRRT.cpp Progetto: megan-starr9/UAV_Aiolos

void ompl::geometric::LazyRRT::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.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
        if (motions[i]->valid)
            data.tagState(motions[i]->state, 1);
    }
}