MRC/Tutorials/Towards an autonomous robot

Introduction

Note: before you start, run emc-update. Some needed software pieces might be added in the meantime.

So far, we have seen how to create a simple C++ project, run the simulator, show some visualizations and drive the simulated robot around using the keyboard. That's nice and all, but we don't want to manually drive around a virtual robot. We want an autonomous, real robot!

As was already stated during the lecture, we won't expose you to the (sometimes somewhat frustrating) low-level details of connecting software to hardware. Instead, we provide you with an abstraction layer that can be easily used within your program to read sensor data and send goals to the base. In previous years we used ROS to do this. Now, it is even simpler.

The loop

When we want to control and monitor a piece of hardware, we often want to perform a series of steps, computations, procedures, etc., in a repetitive manner. When we talk about doing something repeatedly in software, the first 'control flow statement' that comes to mind is a loop. Remember the while and for loops from the C++ tutorials? Right, that's the kind of stuff we're talking about. So, lets' create a loop!

#include <iostream>

int main()
{
    while(true)
    {
        std::cout << "Hello World!" << std::endl;
    }

    return 0;
}

Remember that while the condition in the while statement is true, the body of the while loop will be executed. In this case, that is forever! Fortunately you can always interrupt the program using ctrl-C from the command line. By the way, the default behavior is that this directly kills your program, so all statements after the while loop (if there were any) would never be executed. You can verify this by putting a print statement there. You will see it is never called...

So, it's a nice loop, but there's at least three things wrong with it:

1. It runs forever, never 'gracefully' shutting down (only by user interruption)
2. It runs as fast as possible!
3. It doesn't do much useful except for flooding the screen...

For now, let's focus on point 2). Your operating system schedules the execution of programs: if multiple programs are running simultaneously, it gives each program a short period of time to perform their executions and then jumps to the next. What our program does in that time slice is printing 'Hello World!' as fast as it can! It is like a horrible, zappy kid taking up all of your time as soon as you give it some attention. We can be better than that.

Let's wait a little

In fact, you can tell the operating system that you're done for some time. This allows it to schedule other tasks, or just sit idle until you or another program wants to do something again. This is called sleeping. It's like setting an alarm clock: you tell the operating system: wake me up in this-and-this much time.

So, let's add a sleep statement:

#include <iostream>
#include <unistd.h> // needed for usleep

int main()
{
    while(true)
    {
        std::cout << "Hello World!" << std::endl;
        usleep(1000000); // sleep period of time (specified in microseconds)
    }

    return 0;
}

Ahhh, that's much better! Now approximately every second a statement is printed. This will use way less CPU power that the previous implementation. Note that we explicitly stated 'approximately'. The loop runs at approximately 1 Hz because:

1. The other statements in the loop also take time (in this case the printing)
2. The operating system can not guarantee that it will wake you up in exactly the time specified. This has to do with program priorities, schedules, etc. In high-performance mechatronics system, it is often needed that this frequency can be specified as 'hard' or 'strict' as possible. Therefore these machines often run real-time operating systems that will guarantee that, or at least to some extent. Don't worry about it, you wont notice Ubuntu is not hard real-time.

Although you shouldn't worry about the second point, it is important to take the first into account. As you will put more and more code into the body of your application, it will take more and more time to process it. Sleeping for a fixed amount of time causes your system to start lagging behind at some point.

Fortunately, we created something for you: a class that can be used to keep track of the time spent since the last sleep statement, and which will only sleep the remaining loop time. Use it like this:

#include <iostream>
#include <emc/rate.h>

int main()
{
    emc::Rate r(3); // set the frequency here

    while(true)
    {
        std::cout << "Hello World!" << std::endl;
        r.sleep(); // sleep for the remaining time
    }

    return 0;
}

Control the robot!

Now finally, let's connect to the robot (even though it is a simulated one...)! As was already stated, we can use two types of inputs from the robot:

1. The laser data from the laser range finder
2. The odometry data from the wheels

And, in fact, we only have to provide one output:

1. Base velocity command

That's it! All we have to do is create a mapping from these inputs to this output! We provide an easy to use IO object (IO stands for input/output) that can be used to access the robot's laser data and odometry, and send commands to the base. Let's take a look at an example:

#include <emc/io.h>
#include <emc/rate.h>

int main()
{
    // Create IO object, which will initialize the io layer
    emc::IO io;

    // Create Rate object, which will help using keeping the loop at a fixed frequency
    emc::Rate r(10);

    // Loop while we are properly connected
    while(io.ok())
    {
        // Send a reference to the base controller (vx, vy, vtheta)
        io.sendBaseReference(0.1, 0, 0);

        // Sleep remaining time
        r.sleep();
    }

    return 0;
}

First, try to understand what is going on. You should already be able to derive what will happen if this program is executed.

Now, note a few things: The IO connection is created by just constructing an emc::IO object, it's that easy! We will loop as long as the connection is OK. Then, we can send a command to the base by sampling calling a function on the io object. We do this at a fixed frequency of 10 Hz.

Now, fire up the simulator, and run the example. And what do you see? Voila, we move the robot using our application! Try modifying the sendBaseReference arguments, and see how they affect the robot behavior.

Making PICO aware

So, the robot moves, but it's still pretty stupid. The simulator doesn't have collision detection, so once the robot is near a wall it just goes through it. We don't want that to happen to the real robot, because it will crash! Let's see if we can do something smart. Maybe using the laser?

The io object can not only be used to send commands, but also to read data from the sensors (more detailed information about the different sensor data, i.e., the laser data, odometry data, and the control effort data, is given in the next tutorial step). Here, we will treat a reading of the laser data, which can be done as follows:

...
emc::LaserData scan;
if (io.readLaserData(scan))
{
    // We got new data, so do something with it
}
...

First, you have to create an object / variable that will hold the laser data. Then you call readLaserData with this object, and two things may happen: either new laser data was received and the function returns true. We can then directly start processing it. Or, the function returned false, which means there is no new data.

The emc::LaserData type is in fact a struct that holds all kind of data. Inside you will find information about the maximum range the sensor can measure, the minimum and maximum angle, and of course, the measured distances, or ranges themselves. Try putting the snippet above in the loop you created earlier. Add code that will be executed if new laser data is received, and which prints the minimum angle of the received data:

...
std::cout << scan.angle_min << std::endl;
...

See, you can use the period ('.') to access members of the scan object. This is very similar to accessing the functions sendBaseVelocity and readLaserData of the io object. Now if you run the example, and your simulator is running, you will see a value being printed. This represents the minimum scanning angle of the Laser Range Finder. It doesn't change, and that makes sense: the minimum angle doesn't change because it is a fixed value.

Now let's try accessing a more interesting member of the scan object. The ranges member is not simply a single value but an array of values, or rather, an std::vector. These values represent the measured differences at different angles. The scan object specifies a minimum angle and the angle_increment per array index, which means that you can calculate for each range index to which angle it belongs. Accessing a range value can be done using [ and '], e.g.:

...
std::cout << scan.ranges[0] << std::endl;
...

prints the first distance in the ranges vector (i.e., the range belonging to the minimum angle). Note that all distances are in meters. Now, finally, we can make the robot a bit smarter. If you have done the C++ tutorials, you know about the for loop. We can use it to loop over the values in the range vector:

...
for(unsigned int i = 0; i < scan.ranges.size(); ++i)
{
    if (scan.ranges[i] < some_value)
    {

    }
}
...

Alright!! Now you have all the ingredients to create an application that drives PICO forward, but stops if an obstacle is near!! We can create a main loop at a fixed frequency, send base commands, read the laser data and do something sensible with it. Just now that you can stop the base by simply calling sendBaseReference(0, 0, 0). We leave the full creation of this application as an exercise for you!