MRC/FullExample
Full Example
Here you will find a full example, from a description of the robot behavior, to a Task-Skill-Motion framework to executable code. Note that this example is far from perfect, it is there just to show you the links between various concepts and some hints you could use to decompose your software.
Task-Skill-Motion
We would like to create a behavior for Pico, in which:
- Pico is driving forward, unless a wall is detected.
- If a wall is detected, PICO drives backwards for x metres,
- then turns approx. 90 degrees,
- resumes driving forward.
First, we can describe this behavior in a Task-Skill-Motion Framework, which is shown in the figure, here on the right. This example has no GUI, it does have a user interface, through starting the executable. PICO will then start driving forward, which is selected in the Skill Context. Through monitoring the Detect Wall-skill the Task Monitor might change the skill being active to turn, this is controlled by the task control feedback. As this robot does not store any information about the Environment, the environment context is not present. The Skills control the robot through the Robot Operating System.
Software Executable
The software is decomposed into three parts: main, Detection and DriveControl. The part main is an implementation of the Task Content in the Task-Skill-Motion-framework. The Skills are separated into two parts: those used for detection (Detect Wall) and those for driving (Drive Forward, Drive Backward, stop).
To make the decoupling between Detection and DriveControl explicitly in the implementation, these are implemented in separate classes.
Detection
The header file of the Detection-class, is shown below. The Detection-class contains a pointer to the io-layer inOut, which contains the latest laser data, laser. The methods contained are getSensorData and wallDetected, which together describe the Detect Wall-skill.
#ifndef detection_H
#define detection_H
#define MIN_DIST_TO_WALL 0.4 // [m]
class Detection{
private:
emc::IO *inOut;
emc::LaserData laser;
public:
Detection(emc::IO *io){
inOut = io;
laser = emc::LaserData();
return;
}
bool getSensorData(); // Method to obtain the sensordata
bool wallDetected(); // Method to check if any wall is in the neighbourhood of the robot
};
#endif //detection_H
Beneath you will find the detection.cpp-file. First, this file includes the structure of the class, described in the header file. In this file, the two methods getSensorData and wallDetected are described.
#include "detection.h"
bool Detection::getSensorData() {
if(inOut->readLaserData(laser)) {
return true;
} else {
return false;
}
}
bool Detection::wallDetected() {
for(int i = 0; i < laser.ranges.size(); ++i) {
if(laser.ranges[i] < MIN_DIST_TO_WALL) {
return true;
}
}
return false;
}
Drive Control
Similar to the Detection-class, a DriveControl-class is created within the driveControl.h-file , but now instead of laser data the odomotry data are obtained. Within this class, several drive-skills are mentioned.
#ifndef driveControl_H // Prevent a header file from being included multiple times
#define driveControl_H
class DriveControl
{
private:
emc::IO *inOut;
emc::OdometryData odom; // [x,y,a]
public:
DriveControl(emc::IO *io){
inOut = io;
odom = emc::OdometryData();
return;
}
void driveForward(double Xspeed); // Method to go forward with the robot
double driveBackward(double Xspeed); // Method to go backward with the robot
double rotate(double Aspeed); // Method to rotate with the robot
void stop(); // Method to stop moving with the robot
};
#endif //driveControl_H
Now, the driveControl.cpp-file describes these methods:
#include "driveControl.h"
void DriveControl::driveForward(double Xspeed) {
inOut->readOdometryData(odom);
inOut->sendBaseReference(Xspeed, 0.0, 0.0);
}
double DriveControl::driveBackward(double Xspeed) {
emc::OdometryData odomUpdate;
inOut->readOdometryData(odomUpdate);
inOut->sendBaseReference(-Xspeed, 0.0, 0.0);
double distBackward = odomUpdate.x - odom.x;
odom = odomUpdate;
return distBackward;
}
double DriveControl::rotate(double Aspeed) {
emc::OdometryData odomUpdate;
inOut->readOdometryData(odomUpdate);
inOut->sendBaseReference(0.0, 0.0, Aspeed);
double rotationRelative = odomUpdate.a - odom.a;
odom = odomUpdate;
return rotationRelative;
}
void DriveControl::stop() {
driveForward(0.0);
}
Main Execution
Finally, the main.cpp-file is configured as follows:
#include <emc/io.h>
#include <emc/rate.h>
#include <emc/odom.h>
#include <cmath>
#include "driveControl.cpp"
#include "detection.cpp"
#define EXECUTION_RATE 20 // [Hz]
#define FORWARD_SPEED 0.5 // [m/s]
#define ROTATE_SPEED -1.0 // [rad/s]
#define DIST_BACKWARDS 0.1 // [m]
typedef enum {
drive_forward = 1,
drive_backward,
rotate,
} state_t;
int main(int argc, char *argv[])
{
// Initialization of Robot
emc::Rate r(EXECUTION_RATE);
emc::IO io;
emc::OdometryData odom;
emc::LaserData laser;
// Initialize the Classes
DriveControl pico_drive(&io);
Detection detection(&io);
// Initialize the State of the State Machine
state_t state = drive_forward;
double rotatedAngle = 0.0;
double distanceBackwards = 0.0;
/* Main Execution Loop, this loop keeps running until io.ok returns false,
* hence on a robot error. */
while(io.ok()) {
// Get the Sensor data from the LRF
if(detection.getSensorData()) {
// State Machine
switch(state) {
// case drive_forward: the robot drives forward until a wall is detected.
case drive_forward:
if(detection.wallDetected()) {
// If a wall is detected:
// stop before we hit the wall
pico_drive.stop();
// reset rotatedAngle to 0
rotatedAngle = 0.0;
// reset distanceBackwards to 0
distanceBackwards = 0.0;
// switch state to move backwards
state = drive_backward;
} else {
pico_drive.driveForward(FORWARD_SPEED);
}
break;
// case drive_backward: the robot drives backward
case drive_backward:
// start driving backwards, add distance driven to counter distanceBackwards
distanceBackwards += pico_drive.driveBackward(FORWARD_SPEED);
// if we have driven backwards far enough,
if(fabs(distanceBackwards) >= DIST_BACKWARDS) {
// we start rotating.
state = rotate;
}
break;
case rotate:
// start rotating, add angular displacement to counter rotatedAngle
rotatedAngle += pico_drive.rotate(ROTATE_SPEED);
// if we have rotated enough,
if(fabs(rotatedAngle) >= 0.5*M_PI) {
// start driving again
state = drive_forward;
}
break;
default:
pico_drive.stop();
break;
}
} else {
pico_drive.stop();
}
// Ensure execution rate
r.sleep();
}
return 0;
}
What is Pico doing here? Well, Pico starts using its driving-forward skill! In the meantime, the robot continuously monitors its distance to the wall using its detection skill. When a wall is detected, Pico can not drive forward anymore and needs to turn. Therefore, the state switches and an other set of skills is called. Now, compile the files yourself, see whats happens and relate all the steps to the Taks-Skill-Motion Framework. Do you start to see the elegance of structuring your code within the framework and using classes? Now it is your turn!