MRC/Tutorials/The EMC environment
The main 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. 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:
- 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.
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! 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 won't 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;
}
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 what the smallest and largest measurable distances are. If a distance reading is below range_min or above range_max, that reading is invalid. The values angle_min and angle_max determine the angle of the first and last beam in the measurement. The value angle_increment is the angle difference between two beams. Note that it is actually superfluous, as it can be derived from angle_min, angle_max and the number of beams.
The actual sensor readings are stored in ranges. It is an std::vector, a vector of values which, in this case, stores floats. Each vector element corresponds to one measured distance in meters at a particular angle. That angle can be calculated from angle_min, angle_increment and the index of the element in the vector.
Finally, the timestamp specifies at which point in time the data was measured. The timestamp is in Unix time, i.e., the number of seconds since 1 January 1970. Note that the absolute value is not necessarily important, but that the timestamp can be handy to keep track of laser data over time, or to synchronize it with other input data (e.g., the odometry data)
Odometry Data
The robot has a holonomic wheel base which consists of two wheels in a diff drive configuration and a rotational joint which rotates the entire body. The specific configuration of the wheels allows the robot to move both forwards and sideways, and enables it to rotate around its axis. Both wheels and the joint have an encoder which keeps track of the rotations of that wheel. By using all three encoders and knowing the wheel configuration, the displacement and rotation of the robot can be calculated. In other words: we can calculate how far the robot drove and how far it rotated since it's initial position. This translation and rotation based on the wheel encoders is called odometry. However, note that this information is highly sensitive to drift: small errors caused by measurement errors and wheel slip are accumulated over time. Therefore, relying on odometry data alone over longer periods of time is not recommended! Also note that the odometry data does not 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).