View source

= 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!

<pre>
#include <iostream>

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

    return 0;
}
</pre>

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:

# It runs forever, never 'gracefully' shutting down (only by user interruption)
# It runs as fast as possible!
# 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:

<pre>
#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;
}
</pre>

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:

# The other statements in the loop also take time (in this case the printing)
# 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:

<pre>
#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;
}
</pre>

= 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:

# The laser data from the laser range finder
# The odometry data from the wheels

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

# 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:

<pre>
#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;
}
</pre>

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. Currently, it can only read the laser data, but odometry data will be added soon. It looks like this:

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

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:

<pre>
...
std::cout << scan.min_angle << std::endl;
...
</pre>

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.:

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

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:

<pre>
...
for(unsigned int i = 0; i < scan.ranges.size(); ++i)
{
    // do something
}
...
</pre>