MRC/Tutorials/Towards an autonomous robot

Introduction

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 monitoring 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;
}

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!