Table of Contents
BFS & DFS Implementation
Author: Dylan Wallace Email: wallad3@unlv.nevada.edu
Date: Last modified on 07/11/16
Keywords: Navigation, BFS, DFS, Robotino, ROS, Cpp
The photo above depicts an area with obstacles to navigate around, which allows you to from a start point to and end point autonomously. The big picture problem is robotic navigation. Solving this partially or completely is important because it allows for the development of Artificial Intelligence and navigation such as autonomous vehicles. This tutorial shows you how to implement BFS and DFS methods in C-plus-plus, and takes approximately 3 hours to complete.
Motivation and Audience
This tutorial's motivation is to find ways for robots to autonomously navigate their environments. Readers of this tutorial assumes the reader has the following background and interests:
* Know how to code in C-plus-plus, especially with concepts such as pointers and linked lists.
* Know how Breadth-First and Depth-First Search algorithms work. For info on this refer to the BFS & DFS tutorials.
* Perhaps also know how to create and operate ROS packages, and know how to use the terminal in Linux.
* Perhaps additional background needed may include the Robotino API.
* This tutorial may also attract readers who work with Robotino.
The rest of this tutorial is presented as follows:
- Final Words
Source Files
The ROS package for the BFNav and DFNav can be found here.
Breadth-First Navigation
BFNav Demo
Program Structure
BFNavNode is the main program that runs the BF Navigation, incorporating all of the classes into it.
Map
Map is the class that generates a grid of points to use BFS on. This grid has information such as if a point is explored, or if it is an obstacle. This map is much more helpful than just a traditional graph, as it is nicely organized for navigation.
<Code:c++ linenums:1> 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;
}; </Code>
SearchTreeNode
SearchTreeNode is a class that creates the nodes for each grid point to be searched through in the BFS. These nodes contain info like the child and the parent for the search.
<Code:c++ linenums:1> class searchTreeNode{ public:
searchTreeNode(); std::vector<searchTreeNode *> children; std::vector<searchTreeNode *> parent; map::gridPoint *point;
private:
}; </Code>
BreadthFirst
BreadthFirst is the class that takes the map and the searchTreeNode classes and does the BFS through them. This works through a traditional BFS method: its creates a searchTreeNode at the start location; then it creates more nodes in every direction that the robot could move to on the grid (isn't an obstacle/border), and marks them explored as it adds these to the queue; then it repeats this for all of the child nodes until it has found the grid point that is the end point; finally it traces back through all of the parents of this end node until it gets to the start, and this is the path that the BFS results in.
<Code:c++ linenums:1> 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;
}; </Code>
RobotinoNavigator
This is the main class used for Robotino to navigate. It uses the odometry tools within Robotino in order to derive the velocity and direction, and then navigates with this information.
<Code:c++ linenums:1> class robotinoNavigator{ public:
robotinoNavigator(); void spin(){return;}; // designed to be inherited void setRobotinoVelocity(float vx, float vy, float omega); // sends a velocity message to robotino void setRobotinoGlobalVel(float vx,float vy, float omega); // sets the velocity of the robotion in global reference frame float magnitude(std::vector<float> vect); // find the magnitued of a vector std::vector<float> normalize(std::vector<float> vect); // normalize a vector struct pose{ float array[6]; bool gripperClosed; } BHAPose; void setBHAPose(pose myPose); // set the bhapose using a pose object void setBHAPose(float x0,float x1,float x2,float x3,float x4,float x5, bool gripper); // set bha pose using primitives void sendPoseMsg(); // send the pose message. float targetPhi(float angle); // returns a velocity to hold orientation at angle float targetX(float x); float targetY(float y); bool moveToPos(float x,float y,float phi, float tolerance); bool setOdom(float x, float y, float phi);
// adjustable parameters float targetPhiGain; // gain for proportional controll on in targetPhi() float linearGain; float minimumLinearVelocity; // x or y velocity below this setting will be set to 0 float minimumAngularVelocity; // anguluar velocity below this setting will be set to 0 float maximumLinearVelocity; potentialField::point getRobotinoPoint();
protected:
/* * data */ double x_,y_,phi_; // the position of the robotino /* * callback functions */ void odomCallback(const nav_msgs::Odometry::ConstPtr& msg); /* * publishers/subscribers */ ros::NodeHandle n; ros::Publisher vel_pub; ros::Publisher bha_pub; ros::Publisher gripper_pub; ros::Subscriber odom_sub; /* * services */ ros::ServiceClient odom_reset;
}; </Code>
BFNavigator
BFNavigator heavily relies on the robotinoNavigator in order to operate. All this class really does is follows the path of waypoints given to it by breadthFirst, using the navigation methods created in robotinoNavigator.
<Code:c++ linenums:1> class BFNavigator:public robotinoNavigator{ public:
breadthFirst pathfinder; void navigatePath(); float maxLinearVel;
}; </Code>
BFNavigatorNode
BFNavigatorNode is the main program that runs all of the other classes used. This program starts off by setting up the map with all of the obstacle information, then it sets up the search at the start point, then it sets up the odometry for Robotino, and finally it calls Robotino to navigate through the path generated.
<Code:c++ linenums:1> #include <iostream> #include “BFNavigator.h” #include “ros/ros.h”
int main(int argc, char argv)
{
ros::init(argc, argv, “BFNavigatorNode”);
map myMap;
breadthFirst mySearch;
BFNavigator navigator;
myMap.createGrid(12, 12);
myMap.setOriginOnGrid(0, 0);
myMap.addGoal(1, 9);
for(int i=0;i⇐4;i++){
myMap.addObstacle(i, 3);
}
for(int i=0;i⇐4;i++){
myMap.addObstacle(i, 6);
}
for(int i=0;i⇐4;i++){
myMap.addObstacle(8, i);
}
for(int i=7;i⇐9;i++){
myMap.addObstacle(4, i);
}
for(int i=7;i⇐9;i++){
myMap.addObstacle(6, i);
}
myMap.addObstacle(5, 7);
myMap.addObstacle(5, 9);
myMap.addObstacle(8, 8);
myMap.addObstacle(11, 11);
mySearch.theMap=myMap;
mySearch.start(2, 1);
for (int i=0; i<mySearch.path.size(); i++) {
std::cout « “{” « mySearch.path[i][0] « “,” « mySearch.path[i][1] « “},”;
}
navigator.setOdom(0,0,0);
navigator.pathfinder.theMap=myMap;
navigator.navigatePath();
std::cout«std::endl;
return 0;
}
</Code>
==== Running the BFNav ====
Before starting this step, make sure that you have gone through all of the tutorials for setting up Robotino. Once you have powered on and configured DHCP on Robotino after setup is complete, make sure that you download the BFNav ROS package to your catkin workspace src folder, and run catkin_make. Once this has built correctly, open a terminal and type the following:
roslaunch robotino_node robotino_node.launch hostname:=<robotinoIP>
Now open a second terminal and type the following:
rosrun robotino_control_bfdfnav BFNavigatorNode
Now the BFNav program should run successfully. Please note that the configuration of the obstacles and the grid is detailed in the BFNavigatorNode file, and can be edited for your use there. The current configuration given is using the map given at the very beginning of the tutorial. Now you are ready to move onto Depth-First Navigation.
===== Depth-First Navigation =====
==== DFNav Demo ====
==== Program Structure ====
Since Depth-First Search is very similar to Breadth-First Search, only some of the classes and program files will need to be changed. The map, searchTreeNode, and robotinoNavigator classes/programs will all stay the same. New classes/programs will be made for depthFirst, DFNavigator, and DFNavigatorNode.
==== DepthFirst ====
This is the program that will contain many of the differences between BFnav and DfNav. The main difference here is that BFNav uses the queue data structure to store and extract information, while DFNav uses the stack data structure to store and access information.
<Code:c++ linenums:1>
class depthFirst{
public:
depthFirst();
~depthFirst();
std::vector<std::vector<int> > findPath(searchTreeNode *startNode); run repeatedly to find path
void runStack(); runs the next point in the stack
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 mydepthFirst.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 *> stack;
bool checkPoint(int x,int y);
searchTreeNode *goalNode;
bool goalFound;
};
</Code>
==== DFNavigator ====
There aren't many differences in structure here, just a name change from breadthFirst to depthFirst.
<Code:c++ linenums:1>
class DFNavigator:public robotinoNavigator{
public:
depthFirst pathfinder;
void navigatePath();
float maxLinearVel;
};
</Code>
==== DFNavigatorNode ====
This program also doesn't contain much editing, only changing of names, and possibly obstacles in the future for a more DFS optimized grid.
<Code:c++ linenums:1>
#include <iostream>
#include “DFNavigator.h”
#include “ros/ros.h”
int main(int argc, char argv)
{
ros::init(argc, argv, "DFNavigatorNode"); map myMap; depthFirst mySearch; DFNavigator navigator; myMap.createGrid(12, 12); myMap.setOriginOnGrid(0, 0);
myMap.addGoal(2, 9); for(int i=0;i<=4;i++){ myMap.addObstacle(i, 3); } for(int i=0;i<=4;i++){ myMap.addObstacle(i, 6); } for(int i=0;i<=4;i++){ myMap.addObstacle(8, i); } for(int i=7;i<=9;i++){ myMap.addObstacle(4, i); } for(int i=7;i<=9;i++){ myMap.addObstacle(6, i); } myMap.addObstacle(5, 7); myMap.addObstacle(5, 9); myMap.addObstacle(8, 8); myMap.addObstacle(11, 11); mySearch.theMap=myMap; mySearch.start(2, 1);
for (int i=0; i<mySearch.path.size(); i++) { std::cout << "{" << mySearch.path[i][0] << "," << mySearch.path[i][1] << "},"; } navigator.setOdom(0,0,0); navigator.pathfinder.theMap=myMap; navigator.navigatePath();
std::cout<<std::endl; return 0;
} </Code>
Running the DFNav
Please follow the instructions for implementing BFNav before running the DFNav. Once you have done this, open a terminal and type the following:
roslaunch robotino_node robotino_node.launch hostname:=<robotinoIP>
Now open a second terminal and type the following:
rosrun robotino_control_bfnav DFNavigatorNode
Now the DFNav program should run successfully. Please note, as is the case with BFNav, that the configuration of the obstacles and the grid is detailed in the DFNavigatorNode file, and can be edited for your use there. The current configuration given is using the map given at the very beginning of the tutorial.
Final Words
This tutorial's objective was to implement BFS and DFS methods for navigation on a robot such as Robotino. Complete source code for BFS and DFS can be found at the beginning of the tutorial. Once the concepts were conveyed the reader could successfully write and implement their own BFS and DFS programs on Robotino.
Speculating future work derived from this tutorial, includes Voroni navigation and SLAM on Robotino, as well as BFS and DFS on humanoids such as Darwin OP. In the big picture, the problem of robotic navigation can be solved with this tutorial.
For questions, clarifications, etc, Email: wallad3@unlv.nevada.edu