Project 2: Autonomous Navigation: Bug Algorithm

In this project, you will program the MBot-Omni to autonomously navigate to a goal while avoiding obstacles using the Bug Algorithm.


Getting the Code

To get the template code for this project, see your course page for the GitHub Classroom assignment link.

Once you have accepted the assignment, which will create a GitHub repository for your project code, you will need to get the code onto the robot. You will be cloning the repository on the robot's Raspberry Pi. See the robot tutorial for instructions on how to open a remote VSCode session connected to the robot. Once you are connected to the robot, in VSCode, open a terminal. This should be a terminal in the robot's Raspberry Pi. Then, clone the repository in the home directory:

git clone <ADDRESS>

Substitute <ADDRESS> with the Git address for your repository found on GitHub. Open the folder of the repository you just cloned in VSCode using the instructions in the tutorial.

You should always sign the license for your code by editing the file LICENSE.txt. Replace <COPYRIGHT HOLDER> with the names of all teammates, separated by commas. Replace <YEAR> with the current year.

  • P2.0: In the file LICENSE.txt, replace <COPYRIGHT HOLDER> with the names of all teammates, separated by commas. Replace <YEAR> with the current year.

Project Description

In this project, we will implement our first autonomous navigation algorithm: the Bug 0 Algorithm.

The Bug Algorithm navigates by driving towards the goal, while checking for obstacles in the path. If the robot encounters an obstacle while driving to the goal, it drives around the obstacle until the path to the goal is clear again. We will drive around around obstacles using a Wall Follower, with the same algorithm as in Project 1.

To program the robot, we will use the MBot Bridge API.

Robot Hits the Spot

The first step is to navigate to a given goal position and angle. In Project 1.1, we wrote a program that drove in a square by sending velocity commands to the robot. The square was not very accurate because we had no way of checking whether the robot had acheived the desired position.

Now, we will use the robot's odometry to ensure that the robot navigates to the desired goal. You should compare the robot's current pose with the goal pose to compute the velocity command for the robot. Your program should stop when the robot has reached the goal.

  • P2.1 Robot Hits the Spot (4 points): Write an algorithm that drives the robot to a desired goal pose. You should implement this algorithm in the function driveToPose() in the file hit_the_spot.cpp. To get full points for this task, you should demo your robot driving to a series of goals given by course staff. Print the robot's pose after each goal has been reached.

Bug Navigation

Once we can drive to a goal location, we will encorporate avoiding obstacles using Wall Following. We will implement the Bug 0 Algorithm as a State Machine which has three states: drive towards the goal, wall follow around obstacles, and reached goal. The algorithm continues until the goal is reached, doing the following at each iteration:

  1. Check whether the robot has reached the goal, or whether it should be in the drive to goal state or the wall following state.
  2. If the robot has reached the goal, terminate the program.
  3. Otherwise, compute the velocity command which drives towards the goal OR follows the obstacle, depending on the state.
  4. Send the command to the robot.

First, you will need to determine which state your robot is in. If the path to the goal is obstructed, the robot should be in a wall following state. In Project 1, we wrote a function to find the minimum ray in the Lidar scan. This time, we want to know if there is an obstacle along the path to the goal. You should complete the function findMinRayInSlice() which will tell us the minimum ray in a specific slice of the Lidar scan which points in the direction of our go.

  • P2.2 (1 point): Complete the function findMinRayInSlice() which finds the index of the minimum ray in a slice of the Lidar scan. The slice should be centered around target_angle, passed as input to the function. The slice should encompass rays within slice_size radians of target_angle.
  • Hint: You can reuse parts of your function findMinRay() from Project 1.
  • Hint: The function wrapAngle() will be helpful.

Use the function findMinRayInSlice() to determine which state the robot is in. Print out the state of the robot at each iteration of the algorithm.

  • P2.3 (1 point): Determine the state of the robot at each iteration and print the current state to the screen. Course staff should be able to read the output to ensure your bug navigation is correctly transitioning between states.
  • Hint: You can keep track of the state of the robot using an integer, like we did in class.

You should now have the tools to complete your bug navigation, using the steps outlined above. Write your algorithm in the file bug_navigation.cpp. The code should ask the user for x, y, and theta values for the goal, and drive to the given pose.

  • P2.4 Bug Navigation Demo (5 points): In the file bug_navigation.cpp, write a program that drives to a goal location and avoids obstacles following the procedure outlined above. For full points, your robot will need to drive to a goal given by course staff. Staff will test 2-3 runs of your bug navigation.

Code Overview

For Project 2, we will compile and run our code the same way as in Project 1. To build the code, in a terminal, type:

cd ~/[my-bug-nav-dir]/build
cmake ..
make

Remember that the code should be cloned and compiled on the Raspberry Pi. This will fail on your computer! You should replace [my-bug-navigation-dir] with the name of your bug navigation directory. For a review of what these commands do, see the Project 1 description.

Repository structure

The repository includes the following dirctories and files:

  • build: Build files and executables should be generated here. All commands to compile code should be executed inside this directory. The contents are not pushed to GitHub.
  • include: Header files are stored in this directory. These allow us to include code written in separate files in our code.
  • src: Source code and executables are stored here. All your changes should be in this folder.
  • CMakeLists.txt: Instructions for CMake to use to find dependencies and compile executables.

Provided functions

To use provided functions, all you need to do is include the correct header file. The needed header files should already be included in the templates. Also refer to the MBot Bridge API to learn how to control the robot and read its sensor data. The following functions are provided in this repo:

  • void sleepFor(float secs): Sleep for a given number of seconds.
  • float wrapAngle(float angle): Wrap an angle in the range [-pi, pi]. This function returns the wrapped angle.

Task Summary

  • P2.0: In the file LICENSE.txt included in the project template, replace <COPYRIGHT HOLDER> with your name. Replace <YEAR> with the current year.
  • P2.1 Robot Hits the Spot Demo: Write an algorithm that drives the robot to a desired goal pose. You should implement this algorithm in the function driveToPose() in the file hit_the_spot.cpp. To get full points for this task, you should demo your robot driving to a series of goals given by course staff. Print the robot's pose after each goal has been reached.
  • P2.2: Complete the function findMinRayInSlice() which finds the index of the minimum ray in a slice of the Lidar scan. The slice should be centered around target_angle, passed as input to the function. The slice should encompass rays within slice_size radians of target_angle.
  • P2.3: Determine the state of the robot at each iteration and print the current state to the screen. Course staff should be able to read the output to determine whether your bug navigation is correctly transitioning between states.
  • P2.4 Bug Navigation Demo: In the file bug_navigation.cpp, write a program that drives to a goal location and avoids obstacles following the procedure outlined above. For full points, your robot will need to drive to a goal given given by course staff. Staff will test 2-3 runs of your bug navigation.
  • P2.5 Project Webpage: Create a web page for your bug navigation project including at least one video demonstration and a discussion. Add a link to the website to the README.md file in your repository.

Advanced Extensions

If you want to go further, try implementing the Bug 1 or Bug 2 algorithm, as discussed in lecture.