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.

Behavior

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 several metres,
• then turns approx. 90 degrees,
• resumes driving forward.

Task-Skill-Motion

Task-Skill-Motion Framework for Full Example

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 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 four parts: main, Detection, DriveControl and WorldModel. 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, DriveControl and Worldmodel explicitly in the implementation, these are implemented in separate classes.

Worldmodel

The worldmodel contains all the information the robot has of its information, in this case only the minimum distance from the robot to the wall has to be saved. The Worldmodel-class is therefore a very simple one, its structure is defined in the header file worldmodel.h

#include "config.h"

#ifndef worldModel_H
#define worldModel_H

#include <emc/io.h>
#include <emc/data.h>

class WorldModel
        {
private:    
    double minDistance_;
    
public:
WorldModel(){
		minDistance_ = MAX_RANGE_LRF;
	}
	
    double* getMinimumDistance();
    void setMinimumDistance(emc::LaserData* laser); // Method to determine the minimum distance to a wall
};

#endif //worldModel_H		

The methods getWorldmodel and setWorldmodel are described in the source file worldmodel.cpp

#include "worldModel.h"

double* WorldModel::getMinimumDistance()
{
	return &minDistance_;
}

void WorldModel::setMinimumDistance(emc::LaserData* laser)
{
	 double aux = laser->ranges[0];
	 for(int i = 1; i < laser->ranges.size(); ++i) {
            if(laser->ranges[i] < aux) {
                aux = laser->ranges[i]; 
            }
        }
      minDistance_ = aux;
}

Detection

The header file of the Detection-class, detection.h, 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

#include <emc/io.h>
#include <emc/data.h>

class Detection{
private:
    emc::IO *inOut;

public:
 Detection(emc::IO *io){
		inOut = io;
		laser = emc::LaserData();
		
		return;
	}
	
	emc::LaserData laser;
    bool getSensorData(); // Method to obtain the sensordata
    bool wallDetected(double minDistance);// 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"
#include "config.h"

    bool Detection::getSensorData() {
        if(inOut->readLaserData(laser)) {
            return true;
        } else {
            return false;
        }
    }

    bool Detection::wallDetected(double minDistance) {
		if(minDistance < MIN_DIST_TO_WALL) {
			return true;
		} else {
			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
#define driveControl_H

#include <emc/io.h>
#include <emc/odom.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"
#include "config.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);
    }

Configuration files

To obtain a good overview of all the definitions, a configuration-file, config.h, is created. Copy the following details in it:

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

#define MIN_DIST_TO_WALL 0.4 // [m]
#define MAX_RANGE_LRF 30.0 //[m]

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.h"
#include "detection.h"
#include "worldModel.h"

#include "config.h"

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;
    
    // Initialize the Classes
    DriveControl pico_drive(&io);
    Detection detection(&io);
    WorldModel worldModel;

    // 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()) {
			// Feed the WorldModel
			worldModel.setMinimumDistance(&(detection.laser));
			
        	// State Machine
			switch(state) {
			// case drive_forward: the robot drives forward until a wall is detected.
			case drive_forward:
				if(detection.wallDetected(*(worldModel.getMinimumDistance()))) {
					// 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;
}

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!