Roadmap.h

Go to the documentation of this file.

// -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; tab-width: 4; -*-
#ifndef __ROADMAP_H__
#define __ROADMAP_H__

#include <vector>
#include <boost/shared_ptr.hpp>
#include "Configuration.h"
#include "RoadmapNode.h"
#include "exportdef.h"

namespace PathEngine{
    class PathPlanner;
    class Roadmap;
    typedef boost::shared_ptr<Roadmap> RoadmapPtr;

    class HRPPLANNER_API Roadmap{
    public:
        Roadmap(PathPlanner *planner) : planner_(planner), m_nEdges(0) {}

        ~Roadmap();

        void addNode(RoadmapNodePtr node) { nodes_.push_back(node); }

        void addEdge(RoadmapNodePtr from, RoadmapNodePtr to);

        bool removeEdge(RoadmapNodePtr from, RoadmapNodePtr to);

        void integrate(RoadmapPtr rdmp);

        unsigned int nNodes() const { return nodes_.size(); }

        unsigned int nEdges() const { return m_nEdges; }

        RoadmapNodePtr node(unsigned int index);

        void findNearestNode(const Configuration& cfg, RoadmapNodePtr &node, double &distance); 

        RoadmapNodePtr lastAddedNode();

        int indexOfNode(RoadmapNodePtr node);

        std::vector<RoadmapNodePtr > DFS(RoadmapNodePtr startNode, RoadmapNodePtr goalNode);

        void tryConnection(RoadmapNodePtr from,  RoadmapNodePtr to, bool tryReversed=true);

        void clear();
    private:
        std::vector<RoadmapNodePtr> nodes_;
        PathPlanner *planner_;
        unsigned int m_nEdges;
    };
};

#endif