This tutorial is developed from the Robotino Operation series. It is recommended the reader familiarize themselves with that series before continuing

The completed project can be downloaded here robotino_control_bfnav.zip

The completed project in action.

This is the main program that generates our ROS node for the project. It is designed to generate a map then call BFNavigator to navigate it.

Map is the class used to represent our map of the surroundings. It generates a 2D vector of gridPoints. the gridPoints hold data about each point describing their location, weather the are explored or if they are an obstacle.

map.h

class map{
 
    public:
        map();
        struct gridPoint{
            bool isObstacle;
            bool isExplored;
            bool isGoal;
            int location[2];
        };
        std::vector<std::vector<gridPoint> > grid;
        void createGrid(int xDim,int yDim);
        void setOriginOnGrid(int x, int y);
        gridPoint *getPoint(int x,int y);
        void addGoal(int x,int y);
        void addObstacle(int x,int y);
        float scale;
        int gridDim[2];
 
    private:
        int originLocation[2];
 
        gridPoint * border;  
};

This class represents a node in a generalized search tree algorithm. It contains pointers to it's parent nodes as well as its child nodes. It also stores a pointer to the gridPoint it represents.

searchTreeNode.h

class searchTreeNode{
 
    public:
        searchTreeNode();
        std::vector<searchTreeNode *> children;
        std::vector<searchTreeNode *> parent;
        map::gridPoint *point;
 
    private:
 
};

This class starts by creating a searchTreeNode at the start location. It then creates a node in every possible direction the robot can move (i.e. not explored and not an obstacle). Those nodes are then added to the queue. This process is repeated until the goal node is found. Now the goal node will trace it's parent nodes all the way back to the start point and produce a vector of waypoints.

breadthFirst.h

class breadthFirst{
 
    public:
        breadthFirst();
        //~breadthFirst();
        std::vector<std::vector<int> > findPath(searchTreeNode *startNode);   // run repeatedly to find path
        void runQueue();	// runs the next point in the queue
        void start(int x,int y);  // used to start the search from point x,y
        map theMap;		// this is where the map data is stored access by mybreadthFirst.map.someMemberFunction();
        std::vector<std::vector<int> > goHome(searchTreeNode *node);  // after the location is found we can follow the parent node tree to the start location and retrieve our path
        std::vector<std::vector<int> > path;  // where we actually store the path
        float getMapScale();	//  gets the scale of the map
        void setMapScale(float s);
        map::gridPoint * debug;
 
    private:
      std::vector<searchTreeNode *> queue;
      bool checkPoint(int x,int y);
      searchTreeNode *goalNode;
      bool goalFound;
};

BFNavigator is a simple class, relying heavily on its parent class robotinoNavigator and breadthFirst to do the heavy lifting. All BFNavigator has to do is follow a vector of waypoints.

BFNavigator.h

class BFNavigator:public robotinoNavigator{
 
    public:
      breadthFirst pathfinder;
      void navigatePath();
      float maxLinearVel;
};

At this point neither map or breadthFirst have deconstructors.

to launch your project

open 4 terminal windows

terminal 1

roslaunch robotino_node robotino_node.launch hostname:=<robotinoIP>

terminal 2

rosrun robotino_control BFNavigatorNode