Challenge 4: Basics of Mapping

Goal

This challenge introduces us to some new hardware (a time of flight distance sensor) as well as to the concept of robotic mapping.

The goal here is to gain knowledge about the environment in which the robot exists. We do this by using the distance sensor to detect obstacles and then storing the locations of those obstacles in a map.

We will spin our robot in a slow circle, updating our map with data from the distance sensor periodically.

Setup

As with previous exercises, create a new folder to hold your sketch, add an ardMain.cpp and populate it with the following skeleton code:

#include <mosscap/challenge4.h>

#include <VL53L0X.h>

VL53L0X tof;

const double mapSize = /* Your value here */;
const double mapResolution = /*Your value here */;

TelemetryMap map(mapSize, mapResolution);

void updateMap() {}

void setup() {
    pinMode(1, INPUT);
}

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

    performMappingSweep();
}

Ok, this looks different, let's break it down.

Where did the rest of our hardware go?

Where did our Motors and Encoders go? For this challenge, they live inside of the mosscap/challenge4.h header file that we included at the top of our sketch. This demo focuses on new hardware and new concepts. We're not doing anything particularly interesting with our motors and encoders this time around, so we've just gone ahead and written the necessary code for those ourself.

This is also true of our updatePosition() function and a new performMappingSweep() function. You may have noticed that this is called after the button is pressed in loop().

This function is very similar to our P-Controller based turnAngle() function from the last challenge. The only difference is that it calls updateMap() inside the while loop. This allows us to focus solely on the mapping code for this challenge.

If you're curious, the code for this `performMappingSweep()` function can be found in this drop-down.

const int minPWM = 70;
const double TURN_KP = 2.8;

void performMappingSweep() {
    const double angle = 360.0;
    double startAngle = pose.getTheta();

    while (abs(startAngle + angle - pose.getTheta()) >= 2) {
        updatePosition();

        // CALL NEW UPDATE MAP FUNCTION
        updateMap();

        double error = abs(startAngle + angle - pose.getTheta());

        int command = error * TURN_KP;

        if (command < 0) command -= minPWM;
        if (command > 0) command += minPWM;

        // Ensure that our command is within our PWM range
        if (command > 100) command = 100;
        if (command < -100) command = -100;

        left.run(-command);
        right.run(command);
    }

    left.run(0);
    right.run(0);
}

New Hardware: Time of Flight Sensor

Our robot now has a fancy new piece of hardware: a time of flight sensor. This simulated sensor is based off of the popular Vl53L0X IC (hence why its type is VL53L0X in our starter code). See this adafruit page for more information about the hardware.

The way these sensors work is almost magic (almost!). They have a small LED that emits light in the infrared frequency range (so we can't see it with our eyes). They also have a small light detector that is tuned to detect those same frequencies of light.

Every so often, they send out a pulse of light from the LED. That light will travel forward and eventually bounce off of something and back towards the sensor where it will be detected. The sensor will measure the time it takes for this all to happen and, because the speed of light is constant, it can use some math to figure out how far away object the light bounced off of was.

Our interface for this sensor (the set of functions we can use to interact with it) is modeled after the popular vl53l0x-arduino library from Pololu.

The primary function we care about here is readRangeSingleMillimeters(). This function will return the distance that the sensor is currently detecting in millimeters. This is the function we will use to collect data and populate our map. Speaking of maps...

New Tool: TelemetryMap

We also have a new tool available to us, the TelemetryMap. Similar to the TelemetryPosition from Challenge 2 this is something that we could implement ourselves if we wanted.

Our map is just a grid of cells or, in programming speak, a 2-dimensional array of booleans. Each cell can either be marked as filled (true) or empty (false). All the cells in the map start as false and, as we detect objects in our environment, we can change specific locations to true to keep track of where our obstacles are.

If we were to do this ourselves, we may write something like:

int mapRows = 10;
int mapCols = 10;

bool map[mapRows][mapCols];

// Some code to initialize everything to false

In fact, this is quite similar to what the TelemetryMap does internally. Much like TelemetryPosition, though, our map also has some secret sauce.

This map can draw itself to our simulator window. When we mark a cell as occupied in our map, that cell will show up as a translucent green square at the given location in our simulator window.

This allows us to very easily determine if our mapping routines are working as we expect them to, we can just compare the green squares (marked map cells) with the red boxes (obstacles that the sensor can detect) on the screen.

When we create our map, we have to give it two pieces of information: the size of our map (in our case this is 3x3 meters, the size of our environment) and the map's resolution. This resolution is a tune-able constant that we will explore as we write our code. For now, you can set it to 0.1 meters (or 10cm per cell).

So, let's get started already!

Implementing updateMap()

The provided code in mosscap/challenge4.h gives us a great starting point. When we press the button, the robot will slowly move in a circle and stop after one rotation, our job is to implement the updateMap() function that will be run repeatedly during that time.

1. Reading data from the sensor

To be able to do anything useful, we need to get the current distance being sensed by the sensor. As discussed above, we can do this with the sensor's readRangeSingleMillimeters() function. The rest of our code uses meters rather than millimeters, though, so we should also convert this measurement into our standard units.

double dist = tof.readRangeSingleMillimeters() / 1000.0;

2. Translating into the world frame

This measurement was taken from the robot's chassis as that's where the sensor is mounted. To figure out where this measurement corresponds to in our environment, we need to do some math to translate the measurement into the world reference frame. This is the set of coordinates measured from the bottom left corner of the screen (rather than from the robot's chassis).

To do this, we can use our knowledge of trigonometry! The robot is oriented at some angle $\theta$. By combining our distance measurement with this angle, we form a right triangle. We can use $\sin$ and $\cos$ to get the $x$ and $y$ components of this line, and add them to our robot's position to get our world frame coordinates:

double thetaRadians = pose.getTheta() * PI / 180.0;
double globalX = pose.getX() + dist * cos(thetaRadians);
double globalY = pose.getY() + dist * sin(thetaRadians);

3. Converting to a map cell

Now, we have to figure out which map cell our location falls into. This can be accomplished with the handy floor() function and some division.

We know our map's resolution, and we can use this information to do this conversion:

int mapX = floor(globalX / mapResolution);
int mapY = floor(globalY / mapResolution);

4. Marking the map

Finally, we have to update our map to reflect the fact that we now know there's an obstacle at that location. This is easy with our map's set() function:

map.set(mapX, mapY);

The full function

So, our completed mapping function looks like:

void updateMap() {
    double currentAngle = pose.getTheta();

    double dist = tof.readRangeSingleMillimeters() / 1000.0;

    double thetaRadians = currentAngle * PI / 180.0;

    double globalX = pose.getX() + dist * cos(thetaRadians);
    double globalY = pose.getY() + dist * sin(thetaRadians);

    int mapX = floor(globalX / mapResolution);
    int mapY = floor(globalY / mapResolution);

    map.set(mapX, mapY);
}

Give this a run in the simulator to see how it performs!

Extra Credit: Obstacle Inflation

Well, not really, you don't get any credit in the first place, so there isn't a great way to give "extra" credit. But, you should still do this piece, because it's fun! Yeah...

Our next challenge will be a path planning one, where we use our map information to plan a path to a goal point from our current position. Our current mapping implementation only marks where obstacles are and, while it might not seem that way initially, this is not the same thing as marking all the valid locations that the robot could travel to.

The problem is that our robot has size, it's not just a point in space. If you imagine moving the center of the robot over a map cell that is right beside one that has been marked as occupied, you might notice that the robot is intersecting the obstacle! This is no good, and we need to find a way to ensure that the robot can only go to spaces that are truly unobstructed.

There are many different approaches that we could take for this but, as the title of this section implies, we are going to tackle this problem with inflation.

Essentially, instead of just marking the cell where the obstacle is, we are going to also mark all of its neighbors as well, to ensure that our robot leaves a healthy amount of space between itself and whatever could get in its way.

Because this is extra credit, go ahead and give this a shot on your own. When you're done (or when you get stuck) you can check the dropdown below to see one possible solution.

Obstacle Inflation Code

Remember that this is just one valid solution, if your way is different, but it works, then it's equally as valid! Great job!

for (int rOffset = -1; rOffset <= 1; rOffset++) {
    for (int cOffset = -1; cOffset <= 1; cOffset++) {
        map.set(mapX + cOffset, mapY + rOffset);
    }
}

Solution

#include "mosscap/challenge4.h"

#include <VL53L0X.h>

VL53L0X tof;

const double mapSize = 3.0;
const double mapResolution = 0.1;

TelemetryMap map(mapSize, mapResolution);

void updateMap() {
    double currentAngle = pose.getTheta();

    double dist = tof.readRangeSingleMillimeters() / 1000.0;

    double thetaRadians = currentAngle * PI / 180.0;

    double globalX = pose.getX() + dist * cos(thetaRadians);
    double globalY = pose.getY() + dist * sin(thetaRadians);

    int mapX = floor(globalX / mapResolution);
    int mapY = floor(globalY / mapResolution);

    for (int rOffset = -1; rOffset <= 1; rOffset++) {
        for (int cOffset = -1; cOffset <= 1; cOffset++) {
            map.set(mapX + cOffset, mapY + rOffset);
        }
    }

    map.set(mapX, mapY);
}

void setup() {
    // Buttons to move the robot around (forward, left, and right)
    pinMode(1, INPUT);

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

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

    performMappingSweep();
}