====== BFS & DFS Implementation (2023) ====== **Author:** Francis Palustre Email: \\ **Date:** Last modified on <12/11/23> \\ **Keywords:** Navigation, BFS, DFS, Robotino, %%C++%%, \\ \\ The videos below depicts the Robotino XT traversing in a grid-based environment with Breadth-First and Depth-First search respectively, showcasing the different movement each algorithms possess. The big picture problem is robotic navigation and understanding what is needed for the process. Solving this partially or completely is important because this gives robotics the option of autonomous navigation , limiting human interference and involvement. This tutorial shows you how to apply BFS and DFS to Robotino to move in the X/Y direction and be able to navigate through an obstacle course based on the graph creation. With the following in mind, to gain better understanding of BFS/DFS and to achieve movement, it would take no more than 3 hours to complete. \\ {{ youtube>GH0YCbQobio?large }} \\ {{ youtube>mmOaT1ST9q4?large }} \\ Source Code: \\ [[https://github.com/FrancisPalustre/Robotino2_Path_Traversal |Robotino Code]] \\ ===== Motivation and Audience ===== This tutorial's motivation is to apply graph traversal algorithms to a mobile robot to navigate through an environment that has the potential to have obstacles. This tutorial assumes the reader has the following background and interests: * Know how to code in %%C++%% and are familiar with function handling \\ * Know how Breadth-First and Depth-First Search algorithms work \\ * Know how to navigate within the terminal in Linux \\ * This tutorial may also attract readers who are interested in implementing algorithms in their own projects \\ \\ The rest of this tutorial is presented as follows: * [[bfs_and_dfs_implementation_v2#setup_and_understanding|Setup and Understanding]] * [[bfs_and_dfs_implementation_v2#general_functions|General Functions]] * [[bfs_and_dfs_implementation_v2#robotino_functions|Robotino Functions]] * [[bfs_and_dfs_implementation_v2#breadth_first_search|Breadth First Search]] * [[bfs_and_dfs_implementation_v2#depth_first_search|Depth First Search]] * [[bfs_and_dfs_implementation_v2#final_words|Final Words]] ===== Setup and Understanding ===== Before following this tutorial, make sure each of the following wikies have been read thoroughly and you were able to achieve the goal for each of them. \\ \\ To setup Robotino, please take a look at the following wikis: \\ [[https://www.daslhub.org/unlv/wiki/doku.php?id=how_to_setup_robotino_for_programming_and_navigation|How to Setup Robotino for Programming and Navigation]] \\ [[https://www.daslhub.org/unlv/wiki/doku.php?id=robotino_api2_setup|Robotino API2 Setup]] \\ [[https://www.daslhub.org/unlv/wiki/doku.php?id=robotino_hello_world|Hello World on the Robotino]] \\ \\ To understand how Breadth-First Search and Depth-First Search work, please take a look at the following wikis: \\ [[https://www.daslhub.org/unlv/wiki/doku.php?id=breadth-first_search_and_depth-first_search|Breadth-First Search and Depth-First Search]] \\ [[https://www.daslhub.org/unlv/wiki/doku.php?id=breadth-first_search_and_depth-first_search_2023|Breadth-First Search and Depth-First Search (2023)]] ===== General Functions ===== General functions are functions that are found in both files of BFS and DFS * getCurrentTime() gets the current time in UTC in seconds. It is used to provide the time needed for how long movement for Robotino will last per node travel * Because %%C++%%98 doesn't have the [[https://www.geeksforgeeks.org/chrono-in-c/|chrono library]] embedded, a custom time function is used as an alternative double getCurrentTime() { struct timeval tv; //timeval struct to store time values gettimeofday(&tv, NULL); //get current time and store in timeval //convert time to seconds and add microseconds for precision return (tv.tv_sec) + tv.tv_usec / 1000000.0; } * graphFunction() creates the graph for the algorithms to traverse through. * This can only work if it is a grid map> graphFunction() { //set the bounds of the grid int rows = 6; int cols = 6; map> graph; //map used a graph to hold nodes and adjNodes //iterate through each node/square in the grid. for (int i = 1; i <= rows; i++) { for (int j = 1; j <= cols; j++) { //storing adjNodes of current node vector adjNodes; //currNode setup as [row,column] string currNode = to_string(r) + "," + to_string(c); //Following if statements check where to add the nodes if (j > 1) {//left adjNodes.push_back(to_string(i) + "," + to_string(j - 1)); } if (j < cols) {//right adjNodes.push_back(to_string(i) + "," + to_string(j + 1)); } if (i > 1) {//top adjNodes.push_back(to_string(i - 1) + "," + to_string(j)); } if (i < rows) {//botton adjNodes.push_back(to_string(i + 1) + "," + to_string(j)); } //assign adjNodes to the current node in the graph. graph[currNode] = adjNodes; } } return graph; } * removeEdges() removes the connected edges to the node you want to remove, so you can't traverse through it map > removeEdges(map > graph, vector removeVec) { for (vector::iterator it = removeVec.begin(); it != removeVec.end(); it++) { //for every connected edge of the current node... vector adjNodes = graph[*it]; for (vector::iterator edges = adjNodes.begin(); edges != adjNodes.end(); edges++) { //for every node of those edges... //disconnects the connected nodes from current edge vector::iterator removeNode = remove(graph[*edges].begin(), graph[*edges].end(), *it); graph[*edges].erase(removeNode, graph[*edges].end()); //getting rid of edges } graph[*it].clear(); //removing current node from graph } return graph; } ===== Robotino Functions ===== Robotino functions are functions that used for Robotino's movement * moveX() is the movement in the horizontal direction at a set velocity for a certain amount of time void moveX(int curr_X, int desired_X) { int checker = desired_X - curr_X; //determines if movement is positive or negative int distance = abs(checker); //how much travel is needed double start_time = getCurrentTime(); double end_time = (distance * 1.772)+ start_time; //offset of 1.772 is used (can change to whatever is yours) double elapsed_time = 0; for (int i =0; i < distance;i++) { if (checker > 0) { //if positive... elapsed_time = 0; //resetting time to avoid continuous movement do { elapsed_time = getCurrentTime(); //updating elasped time omni.setVelocity(0.0015,-0.2,0); //velocity of Robotino (adjust accordingly) usleep(10000); //slight delay needed to avoid missing data } while (elapsed_time < end_time); } else { //if negative.. elapsed_time = 0; do { elapsed_time = getCurrentTime(); omni.setVelocity(-0.00151,0.2,0); usleep(10000); } while (elapsed_time < end_time); } omni.setVelocity(0,0,0); //setting velocity to 0 for precautionary sake } * moveY() is the movement in the vertical direction at a set velocity for a certain amount of time void moveY(int curr_Y, int desired_Y) { //same comments applied to this function int checker = desired_Y - curr_Y; int distance = abs(checker); double start_time = getCurrentTime(); double end_time = (distance * 1.772)+ start_time; double elapsed_time = 0; for (int i =0; i < distance;i++) { if (checker > 0) { elapsed_time = 0; do { elapsed_time = getCurrentTime(); omni.setVelocity(0.2,-0.00131,0); usleep(10000); } while (elapsed_time < end_time); } else { elapsed_time = 0; do { elapsed_time = getCurrentTime(); omni.setVelocity(-0.2,0.01,0); usleep(10000); } while (elapsed_time < end_time); } } omni.setVelocity(0,0,0); } * roboMove() uses the final path generated by BFS/DFS to dictate the movement of Robotino based on current index and next index void roboMove(map > graph, string startNode, string desiredNode) { vector roboPath = BFS(graph, startNode, desiredNode); //can be DFS as well //Movement of Robotino based on index for (int i = 0; i < int(carPath.size()) - 1; i++) { //Position is based on current and next value in path int currPos = atoi(carPath.at(i).c_str()); int nextPos = atoi(carPath.at(i + 1).c_str()); //values being +/- are based on your grid setup if ((currPos + 6) == nextPos) { moveY(0,1); //up } if ((currPos - 6) == nextPos) { moveY(1,0); //down } if ((currPos - 1) == nextPos) { moveX(1,0); //left } if ((currPos + 1) == nextPos) { moveX(0,1); //right } } omni.setVelocity(0,0,0); //precautionary sake } ===== Breadth First Search===== Algorithm for BFS vector BFS(map > graph, string startingNode, string desiredNode) { //Creating containers of visited and queued values vector visited; queue > queue; queue.push(vector(1, startingNode)); //starting the queue with our starting node while (!queue.empty()) { vector path = queue.front(); //first item in queue queue.pop(); //removes first item string currNode = path.back(); //tailed node becomes current node //iterates through a set to find the current value in the path vector::iterator findCurr = find(visited.begin(), visited.end(), currNode); if (findCurr == visited.end()) { visited.push_back(currNode); //marks as visited vector adjNodes = graph[currNode]; //nearby nodes of current for (vector::iterator it = adjNodes.begin(); it != adjNodes.end(); it++) { //iterates the adjNodes vector newPath = path; newPath.push_back(*it); //add neighbor to newPath vector queue.push(newPath); //adds resultant path to queue if (*it == desiredNode) { //if desired found, return current path return newPath; } } } } //if no path is found, return an empty vector return vector(); } ===== Depth First Search ===== \\ Algorithm for DFS. \\ There are two main ways to traverse with DFS, an iterative approach and recursive approach. The following is a recursive approach because it is that method is more widely known in DFS. In the source code, there will be iterative as well. vector DFS(map> &graph, const string &startingNode, const string &desiredNode, map &visited) { //check if only one node is the entire traversal if (startingNode == desiredNode) { return vector{startingNode}; //returns just the starting node } //mark the current node as visited visited[startingNode] = true; //containers of current path vector path; //get neighboring nodes for starting node vector neighbors = graph[startingNode]; //iterate the adjacent nodes for (const string &nextNode : neighbors) { //check if next node has been visited if (!visited[nextNode]) { //recursively call DFS on the next node vector subPath = DFS(graph, nextNode, desiredNode, visited); //if subPath not empty, path to desired node is found if (!subPath.empty()) {//subPath means the other path DFS algor takes due to recursive nature //insert current node at beginning of subPath and return final path subPath.insert(subPath.begin(), startingNode); return subPath; } } } //if no path is found, return an empty vector return vector(); } ==== Final Words ==== This tutorial's objective was to implement the BFS and DFS algorithm into Robotino using an alternative method when compared to the first version. Complete source code for the algorithms can be found at the beginning of the tutorial. Once the concepts were conveyed the reader could try to implement BFS and DFS and other path planning algorithms into their own projects. \\ \\ Speculating future work derived from this tutorial, includes applying these and other algorithms such as Dijkstra and A* to Robotino and other robots. In the big picture, the problem of autonomous navigation can be solved with this tutorial. \\ \\ For questions, clarifications, etc, Email: