Ejemplos de Roadmap::indexOfNode en C++ (Cpp)

Ejemplo n.º 1

Archivo: PathPlannerSVC_impl.cpp Proyecto: thomas-moulard/openhrp-deb

/*
 * Methods corresponding to IDL attributes and operations
 */
void OpenHRP_PathPlannerSVC_impl::getRoadmap(OpenHRP::PathPlanner::Roadmap_out graph)
{
    std::cout << "getRoadmap()" << std::endl;

    PathEngine::Roadmap *roadmap = path_->getRoadmap();

    graph = new OpenHRP::PathPlanner::Roadmap;
    std::cout << "the number of nodes = " << roadmap->nNodes() << std::endl;
    graph->length(roadmap->nNodes());
  
    for (unsigned int i=0; i<roadmap->nNodes(); i++) {
        PathEngine::RoadmapNode *node = roadmap->node(i);
        const PathEngine::Configuration& pos = node->position(); 
        graph[i].cfg[0] = pos.value(0);
        graph[i].cfg[1] = pos.value(1);
        graph[i].cfg[2] = pos.value(2);

        graph[i].neighbors.length(node->nChildren());
        for (unsigned int j=0; j<node->nChildren(); j++) {
            graph[i].neighbors[j] = roadmap->indexOfNode(node->child(j));
        }
    }
}