MRC/FullExample
Full Example
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, first it includes the structure of the class, described in the header file. In this file, the two method 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;
}
Create a file called driveControl.cpp and include the following:
#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
Create a file called driveControl.cpp and include the following:
#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);
}
#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();
}
// Use this to ensure an execution rate of 20 Hertz.
r.sleep();
}
return 0;
}