Challenge 5: Path Planning

Goal

The goal for this exercise is to build off our previous mapping work to allow our robot to use its map to plan a path to a target position. For this, we will implement the core functionality of an A* search algorithm.

This challenge is markedly different from the others in that it involves a lot more "regular C++" stuff instead of being more similar to Arduino code. Indeed, you would struggle to implement a path planner like this in the constrained environment of an Arduino.

This is yet another place where our use of a simulator is incredibly handy. This is because under the hood, Mosscap is just C++ code running on your computer, and so we have access to many powerful features such as the Standard Template Library (STL) and many useful functions provided by newer versions of the C++ standard such as C++20 and C++23.

⚠️ This challenge closely follows this blog post from Red Blob Games in its implementation of A*. It is strongly recommended that you take some time to read through this document before completing this exercise. It is well written and features a bunch of great interactive demonstrations.

Setup

As with previous challenges, create a new folder and a new ardMain.cpp to contain our new sketch. You can populate ardMain.cpp with the following starter code:

#include "mosscap/challenge5.h"

void setup() {
    // Mapping button
    pinMode(1, INPUT);

    // Planning button
    pinMode(2, INPUT);

    // Set initial position
    pose.setX(0.5);
    pose.setY(0.5);
}

AStarPlanner planner(map);

bool AStarPlanner::takeSolutionStep() {
    return false;
}

vector<Location> AStarPlanner::reconstructPath() {
    return {};
}

void loop() {
    // Wait for user to press the button
    while (!digitalRead(1)) {}
    while (digitalRead(1)) {}

    performMappingSweep();

    while (!digitalRead(2)) {}
    while (digitalRead(2)) {}

    Location current{5, 5};
    Location goal {18, 26};

    planner.reset(current, goal);
    auto _ = planner.solve();
}

This exercise introduces yet another new class for us to use: the AStarPlanner. This class is similar to our previous ones (TelemetryPosition and TelemetryMap) in that it can draw itself to the screen. This will help us visualize the frontier of our search and our final path if / when we find one.

However, this class, and the ways that we use it here, is very different from ones we have seen previously. Path finding algorithms like A* require some fairly complex data structures to implement (like Priority Queues for example). As such, they are not very well suited for implementation in Arduino code, or for the target complexity level of these challenges.

So, this class handles these complex data structures, and all the drawing logic itself, and leaves us to implement two core functions: takeSolutionStep() and reconstructPath().

When AStarPlanner::solve() is called, this method will repeatedly call takeSolutionStep() until it returns true (indicating that the search is complete). This method is where we will implement our logic for checking and adding new nodes to the frontier.

After takeSolutionStep() returns true, AStarPlanner::solve() will call reconstructPath(). The job of this function is to trace back through the cameFrom dictionary that we updated in takeSolutionStep() and construct the actual path (series of steps) that will take us from our current position to our target position.

Implementing takeSolutionStep()

As stated above, this function will be called repeatedly, and its job is to process nodes in the frontier. Again, it is strongly recommended that you read through the blog post linked above to get acquainted with how graph search algorithms fundamentally work, and how the A* algorithm functions.

1. Getting the next location & early exit

Our AStarPlanner object maintains a fronteir priority queue that we can access and update. To get the next element off of the queue, we can call AStarPlanner::popFromFrontier(). Then, we can compare this returned location against our goal location. If they are the same, we can exit from the function early to ensure that we're not doing more work than we need to (why would we continue to process locations if we had already reached our goal?).

Location current = popFromFrontier();

if (current == goal) {
    return true;
}

2. Checking neighbors

Our goal here is to check each of the eight neighbors of our current location. For each neighbor, we calculate the cost to get to that node. If that cost is less than a cost we have previously seen to get to that node, then we can update our costSoFar dictionary, put this neighbor into the frontier, and update our cameFrom dictionary to allow us to reconstruct our path later.

For now, let's just consider checking the neighbor to the top left of the current node. The first thing we need to do is ensure that this neighbor actually exists (if the current node was on the left side of the map there would be no more nodes to the left, and so there would be nothing to check).

// Move one space to the left and one space up for the top left neighbor
Location neighbor(current.x - 1, current.y + 1);

// If the neighbor is within the bounds of our map
if (neighbor.x >= 0 && neighbor.x < map.getNumElements() && next.y >= 0 && neighbor.y < map.getNumElements()) {
    // We can continue our checks
}

Next, we need to check to see if there is an obstacle in that map cell. If there is, then we don't need to worry about checking the cost to get to it because it's not somewhere we would ever want to be in the first place!

// If the neighbor is within the bounds of our map
if (/* ... */) {
    // If there is *not* an obstacle at this location in the map
    if (!map.get(neighbor.x, neighbor.y)) {
        // We can continue our checks
    }
}

At this point, we're sure that this Location exists, and that it's somewhere we could potentially want to move. Now, we must figure out if we actually do want to move there (or at least if this would be a good candidate location to get us closer to the goal position). This involves computing the cost from our starting position to this new position. Thankfully, our AStarPlanner has a helper function that can make this a little easier.

if (!map.get(neighbor.x, neighbor.y)) {
    // Calculate the total cost to this location (coming from our current location)
    double newCost = costSoFar.at(current) + cost(current, neighbor);
}

Now, if this cost is less than the cost we current have stored for this neighbor location, we should update our costSoFar to reflect the fact that we've found a more efficient path to the node.

double newCost = costSoFar.at(current) + cost(current, neighbor);

// If this new cost is less than what we currently have stored
// Note: It could be the case that our `costSoFar` dictionary doesn't even have an entry
// for `neighbor` yet. In this case, we want to add it (and its cost) to the structure.
if (!costSoFar.contains(neighbor) || newCost < costSoFar.at(neighbor)) {
    // Update the cost in our dictionary
    costSoFar[next] = newCost;
}

This neighbor should also be added to the frontier of the search. For this, we will need to calculate its heuristic so that our priority queue knows how important it is that this location be searched next. Again, our AStarPlanner has a function to calculate this heuristic for two given locations on the map:

// Update the cost in our dictionary
costSoFar[next] = newCost;

double priority = newCost + heuristic(neighbor, goal);

frontier.emplace(priority, neighbor);

Finally, we need to update our cameFrom dictionary so that we know how to trace our path back through this node when the time comes.

frontier.emplace(priority, neighbor);

cameFrom[neighbor] = current;

We should repeat this process for each of the current location's eight neighbors. We could just copy and paste this code eight times, but that would make our function very long, very hard to read, and prone to errors. Instead, let's use a couple of clever for loops to make this easier.

3. Checking all neighbors with a for loop

Usually when we write for loops, we start our index variable (usually i) at zero and run the loop until we reach some end condition (i < someNumber) incrementing i once each time with i++.

But, there's nothing stopping us from starting at a number other than zero, and we can use this to our advantage here:

for (int i = -1; i <= 1; i++) {
    /* Do something */
}

This allows us to loop over "offsets" to our current position and, if we add another loop inside this one, we can cover all the locations we need.

for (int dx = -1; dx <= 1; dx++) {
    for (int dy = -1; dy <= 1; dy++) {
        /* Check neighbor */
    }
}

The one thing we have to be careful of here is that we don't want to check the current cell again, so we can guard against this with an if statement.

if (dx != 0 || dy != 0) {
    // Run all of our checks
}

Then, we can update our creation of neighbor to use these new dx and dy variables:

Location neighbor(current.x + dx, current.y + dy);
// Continue with other checking code

4. Finishing up

Finally, we want to make sure to return false after all of this checking code to signal to the AStarPlanner that we're not done yet, and that it should continue running the search.

Click this dropdown to see the full function implementation

bool AStarPlanner::takeSolutionStep() {
    // Get the next node to check from the frontier
    Location current = popFromFrontier();

    // If we're at the goal, return true
    if (current == goal) {
        return true;
    }

    // For each of the node's neighbors, including diagonals
    for (int dx = -1; dx <= 1; dx++) {
        for (int dy = -1; dy <= 1; dy++) {
            if (dx != 0 || dy != 0) {
                // Compute the location of the neighbor
                Location neighbor(current.x + dx, current.y + dy);

                // If that location is within the bounds of our map
                if (neighbor.x >= 0 && neighbor.x < map.getNumElements() && neighbor.y >= 0 && neighbor.y < map.getNumElements()) {
                    // And, if there is no obstacle at that location
                    if (!map.get(neighbor.x, neighbor.y)) {
                        // Calculate the cost to this location
                        double newCost = costSoFar.at(current) + cost(current, neighbor);

                        // If we've never visited this cell before, or if this cost is lower than the previous cost at this location
                        if (!costSoFar.contains(neighbor) || newCost < costSoFar.at(neighbor)) {
                            // Update the cost in our dictionary
                            costSoFar[neighbor] = newCost;

                            // Calculate the priority for this node
                            double priority = newCost + heuristic(neighbor, goal);

                            // Place the node into the frontier
                            frontier.emplace(priority, neighbor);

                            // Update our `cameFrom` dictionary so that we know which node is this node's parent
                            cameFrom[neighbor] = current;
                        }
                    }
                }
            }
        }
    }

    return false;
}

Implementing reconstructPath()

Now that we have run and (hopefully) finished our search, we need to trace back through our cameFrom dictionary to actually construct a path from our start location to our goal location.

We will implement the reconstructPath() function for this. Note that this function returns a vector of Locations. If you're unfamiliar, a vector is just a list in C++ that we can add things to as we go using push_back().

1. Setup

First things first, we need to make sure that a path actually exists. It may be possible that there was no way to get from start to goal in which case trying to construct a path wouldn't do us much good. To guard against this, we can just return an empty vector if our cameFrom dictionary doesn't contain our goal location.

// If we didn't end up finding a way to `goal`, we can just return an empty vector
if (!cameFrom.contains(goal)) {
    return {}; // These `{}` brackets just represent a vector with no elements
}

Then, we need to set up a place to store our path and a variable to track where we currently are.

// We trace our path backwards from the goal, so we start at the goal position
Location current = goal;

// Stores our path
vector<Location> path;

2. Constructing our path using a while loop

Because we don't know how long the path will be, we can't use a for loop. Instead, we will use a while loop that will run until our current location is our start location.

while (current != start) {
    // Add nodes to the path
}

Then, we can construct the path by appending the current location, and updating the current location to its "parent" using the cameFrom dictionary.

while (current != start) {
    // Put the current node in our path
    path.push_back(current);

    // Update the current node to this node's parent
    current = cameFrom.at(current);
}

We still have a problem, though. Our path is backwards!

3. Reversing the path

We have to do a little bit more work to reverse our path so that the start node is at the beginning of the vector, and the goal node is at the end. The simplest way to do this is just to create another vector, and use a for loop to add stuff in the reverse order.

// Now we have to reverse the path so we can follow it from start to finish
vector<Location> pathReversed;

for (int i = path.size() - 1; i >= 0; i--) {
    pathReversed.push_back(path[i]);
}

Our last step is to return pathReversed so that the AStarPlanner can draw it to the screen.

Testing it Out

Now that we have our functions written, we can see how they work. When you run the simulator, you will see the robot in the lower left corner, some obstacles throughout the environment, and the goal square towards the upper middle marked in green.

Before planning, though, we still have to map, or else we won't know what obstacles we need to avoid! This mapping functionality is the same thing we implemented in the last challenge (although it runs a bit faster) and is provided by the challenge5.h file. So, you can hit the Sense button and let the robot do its thing.

Now, we can hit Plan and watch how the frontier (in magenta) expands outwards and towards the goal square from the robot's current position. Then, once the search reaches the goal, we can see the path (in blue) that our code reconstructed from start to goal.

The last thing to do is to wire all of our code together so that our to make our robot actually drive there! But, we'll save that for the next (and final) challenge.

Solution

#include <mosscap/challenge5.h>

void setup() {
    // Mapping button
    pinMode(1, INPUT);

    // Planning button
    pinMode(2, INPUT);

    // Set initial position
    pose.setX(0.5);
    pose.setY(0.5);
}

AStarPlanner planner(map);

bool AStarPlanner::takeSolutionStep() {
    // Get the next node to check from the frontier
    Location current = popFromFrontier();

    // If we're at the goal, return true
    if (current == goal) {
        return true;
    }

    // For each of the node's neighbors, including diagonals
    for (int dx = -1; dx <= 1; dx++) {
        for (int dy = -1; dy <= 1; dy++) {
            if (dx != 0 || dy != 0) {
                // Compute the location of the neighbor
                Location next(current.x + dx, current.y + dy);

                // If that location is within the bounds of our map
                if (next.x >= 0 && next.x < map.getNumElements() && next.y >= 0 && next.y < map.getNumElements()) {
                    // And, if there is no obstacle at that location
                    if (!map.get(next.x, next.y)) {
                        // Calculate the cost to this location
                        double newCost = costSoFar.at(current) + cost(current, next);

                        // If we've never visited this cell before, or if this cost is lower than the previous cost at this location
                        if (!costSoFar.contains(next) || newCost < costSoFar.at(next)) {
                            // Update the cost in our dictionary
                            costSoFar[next] = newCost;

                            // Calculate the priority for this node
                            double priority = newCost + heuristic(next, goal);

                            // Place the node into the frontier
                            frontier.emplace(priority, next);

                            // Update our `cameFrom` dictionary so that we know which node is this node's parent
                            cameFrom[next] = current;
                        }
                    }
                }
            }
        }
    }
    
    return false;
}

vector<Location> AStarPlanner::reconstructPath() {
    // If we didn't end up finding a valid path, we can just return an empty vector
    if (!cameFrom.contains(goal)) {
        return {};
    }

    // We trace our path backwards from the goal, so we start at the goal
    Location current = goal;

    // Stores our path
    vector<Location> path;

    // While we're not at the start
    while (current != start) {
        // Put the current node in our path
        path.push_back(cameFrom.at(current));

        // Update the current node to this node's parent
        current = cameFrom.at(current);
    }

    // Now we have to reverse the path so we can follow it from start to finis
    vector<Location>  pathReversed;

    for (int i = path.size() - 1; i >= 0; i--) {
        pathReversed.push_back(path[i]);
    }

    return pathReversed;
}

void loop() {
    // Wait for user to press the button
    while (!digitalRead(1)) {}
    while (digitalRead(1)) {}

    performMappingSweep();

    while (!digitalRead(2)) {}
    while (digitalRead(2)) {}

    Location current{5, 5};
    Location goal {18, 26};

    planner.reset(current, goal);
    auto _ = planner.solve();

}