MRC/Tutorials/The EMC environment

The main loop

When controlling and monitoring hardware, we often need to execute a series of steps, computations, and procedures in a repetitive manner. In software, the most common way to achieve this is by using a loop, a fundamental control flow statement. Let's create one!

#include <iostream>

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

Remember a while loop continues executing its body as long as the while statement is true. In this case, it would be running forever! If your loop lacks a stopping condition, the program will keep running until manually interrupted. You can always stop execution using Ctrl+C in the command line. By default, this forcefully terminates the program, meaning any statements after the loop (if there were any) will never execute. You can verify this by adding a print statement after the loop. You'll notice it never gets printed....

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 the second issue. Your operating system schedules the execution of programs, allowing multiple processes to share CPU time. When our program gets its time slice, it immediately starts printing 'Hello World!' as fast as it can. This is like a hyperactive child demanding all of your attention at once! We can do better.

Sleep

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! With a small delay, statements are now printed approximately every second. This will use way less CPU power that the previous implementation. However, approximately is the key word here. The loop runs at roughly 1 Hz due to the following reasons:

1. Other statements inside the loop take time to execute (In this case as printing)
2. The operating system cannot guarantee exact sleep timing due to scheduling, priority handling, and other system processes.

The second issue is less important for now since it only becomes relevant when working with high-performance mechatronic systems. However, the first point is something you should consider. As your program grows and executes more instructions within the loop, the processing time will increase, and the fixed sleep duration to eventually cause your system to start lagging behind at some point.

Fortunately, we created something for you: we provide a time-tracking class that ensures the loop maintains a consistent execution rate. Instead of sleeping for a fixed duration, it calculates the time already spent in the loop and only sleeps for the remaining required time. This prevents performance drift and keeps execution closer what you want from it. Use it like this:

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

int main()
{
    emc::IO io; // Create IO object, which will initialize the io layer
    emc::Rate r(3); // set the frequency here
    while(true)
    {
        io.speak("Hello World! ");
        r.sleep(); // sleep for the remaining time
    }
    return 0;
}

After compiling this example, to test the executable, first run mrc-sim

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

#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 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 simply calling a function on the io object.

We do this at a fixed frequency of 10 Hz.

Now, fire up the simulator and visualization, 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.

Sensor inputs

This section provides a short description of the laser data, odometry data, and bumper data that can be obtained through the IO object introduced earlier.

Laser Data

To obtain the laser data, do the following:

emc::LaserData scan;

if (io.readLaserData(scan))
{
    // ... We got the laser data, now do something useful with it!
}

The LaserData struct is defined as follows:

struct LaserData
{
    double range_min;
    double range_max;
    double angle_min;
    double angle_max;
    double angle_increment;
    std::vector<float> ranges;
    double timestamp;
};

The range_min and range_max values define the smallest and largest measurable distances. Any distance reading outside this range, either below range_min or above range_max, is considered invalid. The angle_min and angle_max values specify the angles of the first and last beams in the measurement. The angle_increment represents the angular difference between consecutive beams. However, this value is redundant, as it can be derived from angle_min, angle_max, and the total number of beams.

The actual sensor readings are stored in ranges, which is an std::vector of float numbers. Each element in this vector corresponds to a distance measurement in meters at a specific angle. The angle for each reading can be calculated using angle_min, angle_increment, and the index of the vector element.

Lastly, the timestamp indicates when the data was recorded. It is represented in Unix time, i.e., the number of seconds since January 1, 1970. While the absolute value of the timestamp is not necessarily important, it can be useful for tracking the laser data over time or synchronizing it with other data sources, such as odometry.

Odometry Data

The robot has a holonomic wheelbase, consisting of two wheels arranged in a differential drive configuration, and a rotational joint that allows the entire body to rotate. This particular wheel configuration allows the robot to move both forward and sideways, as well as rotate around its axis. Each of the wheels and the rotational joint is equipped with an encoder that tracks the rotations of that wheel. By using all three encoders and knowing the specific wheel configuration, we can calculate the robot's displacement and rotation from its initial position. In other words: we can calculate how far the robot drove and how far it rotated since it's initial position. This process of calculating translation and rotation based on the wheel encoders is known as odometry.

However, it's important to note that odometry is prone to drift. This means small errors from measurement inaccuracies and wheel slippage can accumulate over time, making it unreliable over longer periods. As such, it's not recommended to rely solely on odometry data for long-term navigation. Additionally, keep in mind that odometry data doesn't start at coordinates (0,0).

To obtain the odometry information, do the following:

emc::OdometryData odom;

if (io.readOdometryData(odom))
{
    // ... We got the odom data, now do something useful with it!
}

The OdometryData struct is defined as follows:

struct OdometryData
{
    double x;
    double y;
    double a;
    double timestamp;
};

Here x , y and a define the displacement and rotation of the robot since the previous measurment, according to the wheel rotations. The translation (x, y) is in meters. The rotation, a is in radians between -pi and pi. Like the laser data, the odometry data also contains a timestamp which is in seconds (Unix time).