Project 3: Path Planning

Due November 22nd, 2021 at 11:59 PM.

In this project, we will write a path planning algorithm, which will find the shortest path to the goal every time. When you've completed the project, you'll be able to find paths like this one:

We will use the same code base and web app that we did for Project 2. You can review the instructions for running the app and using the Docker container here.

Getting the Code

This project will be completed in the autonomous_navigation repository, which is the same one used for Project 2.

Submitting the Assignment

Your submission for this assignment should include your code and a project webpage. You should make one submission for your team. Teammates will be graded together.

The LICENSE.txt file should already be updated from Project 2. If you did not complete this task, please do so now.

Submitting the code: Tag the verion of the code you wish to submit. See instructions from Project 0.

  • P3 Code: Push the final version of your code to the main branch of your GitHub repository and make a tag to indicate the version you would like graded.

Submitting the web page: Create a web page for your graph search implementation. Include at least one video demo of breadth first search or A-Star running on the robot, and a number of example of your graph search algorithm(s) on the web app. The website should also include a brief discussion of the algorithm(s). Details about what to include can be found at the end of the project description. Include the link to your project page in the README.md file. See the Project 2 spefication for details on how to create the website.

  • P3 Webpage: Create a web page for your graph search implementation. Include the link to your project page in the README.md file.
  • Hint: You may expand your website from Project 2 if you would like. Just make sure to make a new section or page for Project 3 and make it clear which parts are relavant for grading Project 3.

Grading Breakdown

This project will be graded as follows:

  • Breadth first search photo demo: (3 points) On your website, include images of paths found by your algorithm, as described in Part 2 (you can take screenshots of the web app). For full credit, include multiple examples of maps and start and goal locations.
  • Graph Search Live Demo (in-class): (3 points) Part 3 of the project will be demonstrated on the robot, in class on November 22nd.
  • Graph Search Video Demo: (2 points) On your website, include at least one video of your robot navigating with graph search, as described in Part 3. For full credit, include multiple examples of start and goal locations, and maps.
  • Code: (3 points) Your code should be pushed to GitHub and tagged. It should be organized and complete. The instructors might run your code to test it further.
  • Discussion: (3 points) On your website, include a brief discussion of your implementation (see reflection questions).

Code Overview

The code for Project 3 will be written in the same repository as the Project 2 code. Refer to the Project 2 code overview for instructions on how to run the code.

To test the Breadth First Search or A-Star functions, select the "Breadth First Search" or "A-Star" algorithm options in the web app. Your functions for each algorithm will be called by the navigation web server.

Changes to the Utility Functions

A few changes should be made to the existing utility template functions to make Project 3 work better. The instructors will not make these changes for you, since they involve modifying the source file src/utils/graph_utils.cpp which you made changes to in Project 2. Please make these changes before starting Project 3. They should not break any of your existing code.

  1. In src/utils/graph_utils.cpp, in the function initGraph(GridGraph& graph), remove the following line: 
    graph.obstacle_distances = std::vector(graph.width * graph.height, 0);
  2. In include/autonomous_navigation/utils/graph_utils.h, add the following function definition under the function definition for checkCollision(): 
    bool checkCollisionFast(int idx, const GridGraph& graph);
  3. In src/utils/graph_utils.cpp, add the following function under the function checkCollision(): 
    bool checkCollisionFast(int idx, const GridGraph& graph)
{
return graph.obstacle_distances[idx] * graph.meters_per_cell <= graph.collision_radius;
}

Provided Utility Functions

You have access to all the utility functions and structs from Project 2. This section will describe additional functions relevant to this project, from the header include/autonomous_navigation/utils/graph_utils.h (implemented in src/utils/graph_utils.cpp). Other than those marked, these are implemented for you.

  • bool loadFromFile(const std::string& file_path, GridGraph& graph): Loads graph data from a file. Also initializes the distance transform values to all zeros and calls the initGraph() function.
  • void initGraph(GridGraph& graph): (TODO, see Part 1) Initializes the graph data.
    Note: This function should not modify graph.obstacle_distances.
  • std::vector findNeighbors(int idx, const GridGraph& graph): Returns a list of the indices of the neighbors of the cell at a given index in the graph.
    Warning: This only works if cellToIdx() and idxToCell() are implemented.
  • bool checkCollisionFast(int idx, const GridGraph& graph): Uses the distance transform values to check whether visiting the cell at idx would result in a collision.
    Warning: This only works if the distance transform values are stored in graph.obstacle_distances. When using the web app, the function distanceTransform() will be called when the map is loaded.
  • bool checkCollision(int idx, const GridGraph& graph): Manually checks in a radius around the given cell for collisions.
    Warning: This version does not require the distance transform but will be slower than the above version.
  • int getParent(int idx, const GridGraph& graph): (TODO, see Part 1) Returns the index of the parent of the node at idx.
  • float getScore(int idx, const GridGraph& graph): (TODO, A-Star only, see Advanced Extension: A-Star) Returns the f-score of the node at idx.
  • std::vector<Cell> tracePath(int goal, const GridGraph& graph): Returns the path from the start cell to the given goal cell.
    Warning: This only works if getParent() is implemented.
  • int findLowestScore(const std::vector<int>& node_list, const GridGraph& graph): Returns the index of the node with the lowest f-score.
    Warning: This only works if getScore() is implemented (A-Star only).

Project Description

Before you begin implementing code for Project 3, please make the changes described above.

This assignment has the following parts:

  1. Part 1: Storing Node Data
  2. Part 2: Breadth First Search
  3. Part 3: Path Planning on the Robot

We also provide an advanced extension to implement the A-Star search algorithm, as discussed in lecture.

Part 1: Storing Node Data

We will be using the same GridGraph struct as last time to do our graph search (see Project 2 description for details). In order to perform graph search, we need to keep track of a few things for each cell:

  1. The cell's parent along the current path,
  2. The distance from the start node to the cell along the current path,
  3. Whether the cell has been visited.

Using the concepts we have learned so far, design code to store this data. Then, modify GridGraph to store the node information. You may extend your implementation to store any additional information you need.

Note: The nodes should be indexed the same way as the cells, as a single vector, as we did in Project 2. Refer to Project 2, Part 0 for more information. You can reuse the cellToIdx() and idxToCell() functions to access node data for a specific cell.

  • P3.1.1: In the graph_utils.h file, design a data structure to store the cell information and modify GridGraph to add any new member variables accordingly.
  • P3.1.2: In the graph_utils.cpp file, complete function initGraph() to initialize the cell data. You can assume the graph will be loaded when the function is called.
  • P3.1.3: In the graph_utils.cpp file, complete function getParent() so it returns the index of the parent of the given cell. If a cell does not have a parent, it should return -1.
  • Hint: You might consider implementing a struct to represent your cell node and store a vector of cell node structs in GridGraph. Alternatively, you might consider adding a few vectors to the GridGraph struct to store the relevant information.
  • Hint: The start node and unexplored nodes should not have parents.

Part 2: Breadth First Search

In this section, you will write code to implement a Breadth First Search (BFS) over a graph, given a start and goal cell. You will write your code in the file src/graph_search/graph_search.cpp. Your code should go in the function breadthFirstSearch(), which looks like this:

std::vector breadthFirstSearch(GridGraph& graph, const Cell& start, const Cell& goal, std::function showVisitedCell)
{
std::vector path;  // The final path should be placed here.
initGraph(graph);  // Make sure all the node values are reset.
int start_idx = cellToIdx(start.i, start.j, graph);

/**
* TODO (P3): Implement BFS.
*/

return path;
}

Assume the graph is loaded with the corresponding map data. The function first calls initGraph() to make sure that all the nodes are initialized. The start and goal position are passed in as Cell structs.Cell has two integer member variables: i and j, corresponding to the row and column coordinates of the cell. When the path is found, your code should store the path in the provided vector, path. You can use the provided function tracePath(), as follows:

path = tracePath(goal_idx, graph);

where goal_idx is an integer value which stores the index of the goal. If you have correctly maintained the parent structure of the cells and the idxToCell() and getParent() functions are implemented, this will return the path to the start node. The start node should not have a parent. If no path was found, your code should return an empty vector.

Use either checkCollision() or checkCollisionFast() to check whether a cell is in collision. It is highly recommended to use checkCollisionFast(). This requires that you call your distance transform function first. The results will be slightly different than checkCollision() if you use the Manhattan distance transform, but both are valid.

The function also accepts a showVisitedCell argument. This is to interface with helper functions which will help you visualize your algorithm in the web app. To mark a cell with coordinate (i, j) as visited (shown in grey) in the web app, do:

showVisitedCell(i, j);
  • P3.2.1: In the graph_search.cpp file, implement Breadth First Search in function breadthFirstSearch().
  • Hint: BFS maintains a list of nodes to visit using a First-In-First-Out data structure (or a queue). C++ has a queue implementation called std::queue. Assuming you have a queue q, q.push(my_node_idx) will push a value onto the back of the queue, q.front() will return the value at the front of the queue, and q.pop() will remove the first element of the queue.
  • Hint: Your visit list should store integers corresponding to the indices of the nodes of interest. You can use these indices to retrieve and modify cell data directly in the graph. Do not push any structs your graph stores onto the visit list! This makes a copy of the data, so any changes you make would not be reflected in the graph.
  • Hint: You should not add any cells which are in collision to your visit list.

Part 3: Path Planning on the Robot

You will write code to search for and drive along a path on the robot in the file src/robot_plan_path.cpp. We have provided a function drivePath(std::vector& path, GridGraph& graph) which will send the robot a command to follow the path defined in vector of cells path. This function sends a path to the motion controller. Modify the provided code template to call your path planning algorithm and then send the path to the robot.

Note: The utility function loadFromFile() initializes the radius for collision checking to the robot radius, stored in variable ROBOT_RADIUS, plus the width of one cell. You might want to make the collision radius larger on the robot, so that your planner is more conservative on the robot. To do this modify the value of graph.collision_radius after the map is loaded.

  • P3.3.1: In the robot_plan_path.cpp file, call your path planning function and use the drivePath() function provided to send the path to the robot.
  • Hint: If you are using the checkCollision() function in your graph search, do not forget to call your distance transform function after the map is loaded.
  • Hint: The provided function drivePath() returns immediately after sending the path command to motion controller. That means your program will quit before the robot has reached the goal. If you wish to send another path to the robot or perform some processing when the robot has reached the goal, you can write code to wait until the robot has reached the goal before finishing.

Before you run the code, you should make a map of the environment you want to run in. Once you have made a map, launch localization (with motion controller) as follows:

~/botlab-bin/launch_botlab.sh -m [PATH/TO/MAP]

To compile and run path planning on the robot, connect to the robot using VSCode (as described in the robot tutorial). Then, from inside the repository you cloned onto the robot, compile as follows:

cd build
cmake -DOMNIBOT=On ..
make

Do not forget the argument -DOMNIBOT=On when running CMake. This is how the compiler knows whether to compile the robot code (which is not compiled by default in the Docker). To run the potential field controller, in the build folder where you compiled, do:

./robot_plan_path [PATH/TO/MAP] [goal_x] [goal_y]

The map is stored in ~/botlab-bin/maps/current.map. Use that path if you did not move the map since creating it. You can also pass in goal_x and goal_y, the global position of the goal, relative to the starting position of your map, in meters. If you do not pass these in, they will default to zero.

When you pass in arguments to the code, do not include the square brackets.

Advanced Extension: A-Star Search

As an advanced extension, you may write code to implement A-Star Search over a graph, given a start and goal cell. You will write your code in the file src/graph_search/graph_search.cpp. Your code should go in the function aStarSearch(). This function looks a lot like the one for BFS. However, you will need to maintain the f-score of the nodes you explore.

  • Advanced Extension P3.i: In the graph_utils.h file, extend your cell node data to include the f-score of the node.
  • Advanced Extension P3.ii: In the graph_utils.cpp file, implement the function getScore() to return a double corresponding to the f-score of the given cell index.

In A-Star, the next node to explore is the one with the lowest f-score. The function findLowestScore() takes a std::vector of integers as an argument, along with the graph, and returns the index of the lowest score node in the list. You can use this function as follows to get the lowest scoring node, and then remove it from the list:

int min_score_idx = findLowestScore(visit_list, graph);
int current = visit_list[min_score_idx];
visit_list.erase(visit_list.begin() + min_score_idx);
  • Advanced Extension P3.iii: In the graph_search.cpp file, implement A-Star Search in function aStarSearch().

Project Webpage: Results & Reflection Questions

On your project webpage, include multiple images showing paths found by your breadth first search algorithm (and by A-Star, if applicable). Include examples of different maps, with different start and goal locations. Include at least one video demonstration of your robot following a path and avoiding obstacles (for example, in a maze-like environment) using either BFS or A-Star.

Include a discussion section on the web page. Consider the following questions:

  1. What are the strengths and limitations of breadth first search? Graph search in general?
  2. Does breadth first search always find the shortest path? Does A-Star always find the shortest path?
  3. How might you improve the algorithm(s) you implemented? (Include at least one idea)
  4. (If you implemented A-Star) In what types of environments is A-Star much faster than BFS? Are there any environments where A-Star is not much faster than BFS?

Consider including images or videos whenever possible to illustrate your points. Keep the discussion brief: it should be no longer than one or two paragraphs. If you did not implement A-Star, you may ignore the A-Star questions, or answer based on what we discussed in lecture.

Task Summary

  • P3 Code: Push the final version of your code to the main branch of your GitHub repository and make a tag to indicate the version you would like graded.
  • P3 Webpage: Create a web page for your graph search implementation. Include the link to your project page in the README.md file.
  • P3.1.1: In the graph_utils.h file, design a data structure to store the cell information and modify GridGraph to add any new member variables accordingly.
  • P3.1.2: In the graph_utils.cpp file, complete function initGraph() to initialize the cell data. You can assume the graph will be loaded when the function is called.
  • P3.1.3: In the graph_utils.cpp file, complete function getParent() so it returns the index of the parent of the given cell. If a cell does not have a parent, it should return -1.
  • P3.2.1: In the graph_search.cpp file, implement Breadth First Search in function breadthFirstSearch().
  • P3.3.1: In the robot_plan_path.cpp file, call your path planning function and use the drivePath() function provided to send the path to the robot.

Advanced Extensions

  • Advanced Extension P3.i: In the graph_utils.h file, extend your cell node data to include the f-score of the node.
  • Advanced Extension P3.ii: In the graph_utils.cpp file, implement the function getScore() to return a double corresponding to the f-score of the given cell index.
  • Advanced Extension P3.iii: In the graph_search.cpp file, implement A-Star Search in function aStarSearch().