Documentation / v1.0 / Core / Tasks
From emss Framework - iRobot Create C++ Framework
You are here: Documentation / v1.0 / Core / Tasks
Contents |
[edit] Tasks
|
Files: |
Task/Task.h Licensed under GPLv3
|
Core Tasks bind all the components of the emss framework together, enabling the robot to perform actions in its environment. As already mentioned, the Task Manager is responsible for executing the Tasks one by one. Each emss Task must inherit from the class Task and implement the method process(). When a Task is created, its initial status is Waiting. When the Task Manager decides to schedule the Task for execution, its status is set to Running. The process() method of the Task is called by the Task Manager at its given interval (each Task can define its own interval) until the task sets its status as Finished.
[edit] Task Manager
|
Files: |
Task/TaskManager.h Licensed under GPLv3
|
The Task Manager is a separate thread which contains a Task List. The Task List is nothing more than a helper class for sequentially storing and retrieving Tasks, offering useful functions such as getNextSpecificTask(type). As soon as the Task Manager receives a Task, appends it to the Task List and starts to execute it. If the Task Manager receives more than one Task at a time, it appends it to the back of the Task List, where in time will eventually be executed. The Task Manager also allows Tasks to be interrupted. In this case the manager will set the current Task as Interrupted and go to the next Task in the list.
[edit] Test Move Task
|
Files: |
Task/TestMoveTask.h Licensed under GPLv3
|
Several test movements are provided by the Test Move Task. These test movements are used for diagnostic purposes, such as calibration, accuracy observation, and general research. The following self-explaining test moves are available: Square, Rotate 90, Rotate 360, Triangle, Straight, Circle, and Vector Circle.
[edit] Straight Path Move Task
|
Files: |
Task/StraightPathMoveTask.h Licensed under GPLv3
|
This Task moves the robot in a linear fashion to the given destination. Multiple Straight Path Move Tasks can be appended to the Task Manager in order to create a series of chunky movements.
[edit] Spline Navigation Task
|
Files: |
Task/SplineNavigationTask.h Licensed under GPLv3
|
Spline navigation is very trivial. All it does is drive through all the navigational points contained in the Navigation component in the form of a spline by calling the Navigation methods getWheelSpeed(tick,interval). There is some additional functionality added, such as aligning the robot with the spline before navigation, and resetting the navigation points when finished.
void SplineNavigationTask::process() {
// Align the robot and then crawl the spline...
if(tick == 0 &&
core->boolSetting("SPLINENAVIGATIONTASK_ALIGN_ROBOT_WITH_SPLINE_") == true)
((EmssController*) core->controller)->turn(
(int)(- core->movementTracker->rotation() +
core->navigation->getAngleForSplineAlignment()),
core->intSetting("
SPLINENAVIGATIONTASK_ALIGN_ROBOT_WITH_SPLINE_SPEED"));
// Send wheel speeds for current tick to controller
Vector2D wheelSpeed = core->navigation->getWheelSpeed(tick, interval);
((EmssController*) core->controller)->wheelDrive((short)wheelSpeed.x(),
(short)wheelSpeed.y());
// Finished?
if(wheelSpeed == SplineNavigationWheelSpeedFinished) {
// Reset?
if(core->boolSetting("
SPLINENAVIGATIONTASK_RESET_NAV_POINTS_WHEN_DONE") == true)
core->navigation->clearNavPoints();
else
core->navigation->setTickOffset(tick);
status = Task::Finished;
}
tick++;
}
[edit] Discovery Task
|
Files: |
Task/DiscoveryTask.h Licensed under GPLv3
|
This is a simple implementation of the classic robot discovery algorithm. The robot moves straight until it reaches a collision. Upon reaching a collision it rotates until it is free to move. This algorithm works well for certain environments for finding the bounds (i.e. walls), but quickly finds itself stuck in a loop of a specific path.
[edit] Spline Discovery Task
|
Files: |
Task/SplineDiscoveryTask.h Licensed under GPLv3
|
The Spline Discovery Task is the implementation of the theory discussed in Section 3.7. For navigating in an unknown environment a height map is used as mentioned above. This is a rather complex procedure, and is presented along with the code. For further details or clarifications on the algorithm please see Section 3.7.
This first section is important. First we check if there are any nodes in the spline and if the environment is not already explored. If it is already explored the algorithm is not required and will be canceled. Otherwise new terrain cut points are added:
void SplineDiscoveryTask::process() {
// Are there nodes in the spline?
if (node >= core->navigation->wheelLeftSplineX->getNodeCount() - 1) {
if (isExplored() == true) {
((EmssController*) core->controller)->wheelDrive(0, 0);
Task::status = Task::Finished;
return;
} else {
core->navigation->clearNavPoints();
addNewTerrainCutPoints();
node = 0;
return;
}
}
This next part of the code checks the heat map whether it is safe to move ahead. This is done by directly checking ahead of the robot from a given start and end angle. Figure 4-12 presents two areas of collision. If an obstacle in the dark area is detected the robot will immediately stop and turn ninety degrees. This is to avoid a head-on collision with the obstacle. If an obstacle in the light area is detected, new terrain cut points are calculated. This discrimination lets the calculation of new terrain cut points result in a natural collision avoidance per algorithm.
// Look ahead if it is good to move there using a sweep across a start and
// end angle...
double heat = 0.0;
for (int i = 0; i < core->longSetting("SPLINEDISCOVERYTASK_RANGE_MM");
i += 10) {
for (int angle = -(core->longSetting("
SPLINEDISCOVERYTASK_SWEEP_ANGLE") / 2);
angle <= (core->longSetting("
SPLINEDISCOVERYTASK_SWEEP_ANGLE") / 2);
angle++) {
Trafo2D checkpoint = core->movementTracker->transformation *
Trafo2D::rot(-Rad(angle)) * Trafo2D::trans(0, i);
heat += core->heatMap->getChannelValue(
HeatMap::CollisionAreaChannel,
(long) ((checkpoint.trans().x()) / core->scale),
(long) ((checkpoint.trans().y()) / core->scale));
if (i < core->longSetting("
SPLINENAVIGATIONTASK_CRITICAL_AREA_DISTANCE_MM") && heat > 0)
{
collisionInCriticalArea = true;
}
}
}
// Safe to move?
if (heat > 0.0 && lastProcessWasCollision == false) {
// is the collision in the critical area?
if (collisionInCriticalArea == false) {
core->navigation->clearNavPoints();
addNewTerrainCutPoints();
node = 0;
lastProcessWasCollision = true;
} else {
// turn away from the collision
((EmssController*) core->controller)->turn(90,
core->doubleSetting("NAVIGATION_TARGET_SPEED_MMPS"));
core->navigation->clearNavPoints();
addNewTerrainCutPoints();
node = 0;
lastProcessWasCollision = true;
collisionInCriticalArea = false;
}
} else {
lastProcessWasCollision = false;
}
Code 4-19: Spline Discovery Collision Avoidance
With the following code the steering information is sent to the controller:
// Align the robot and then crawl the spline...
Vector2D wheelSpeed = core->navigation->getWheelSpeed(node, interval);
((EmssController*) core->controller)->wheelDrive((short) wheelSpeed.x(),
(short) wheelSpeed.y());
node++;
}
The most interesting part in this class is how the terrain cut points are calculated, as discussed in the
Chapter “Theory”. First the height array is filled with the value for an unknown height:
int heights[mapWidth][mapHeight];
int explorationMap[mapWidth][mapHeight];
long robotX = core->movementTracker->x();
long robotY = core->movementTracker->y();
// Fill array with default values
for (int x = 0; x < mapWidth; x++) {
for (int y = 0; y < mapHeight; y++) {
heights[x][y] = TERRAIN_UNKNOWN_HEIGHT;
}
}
Then the exploration map is filled with the information from the Heat Map. That means that every point is assigned to either discovered, obstacle or unknown:
// Fill the exploration array
for (int x = 0; x < mapWidth; x++) {
for (int y = 0; y < mapHeight; y++) {
long heatMapX = robotX - RELATIVE_EXPLORATION_WIDTH / 2 +
x * EXPLORATION_MAP_GRID_SIZE;
long heatMapY = robotY - RELATIVE_EXPLORATION_HEIGHT / 2 +
y * EXPLORATION_MAP_GRID_SIZE;
if (core->heatMap->getChannelValue(HeatMap::OpenAreaChannel,
heatMapX / core->scale, heatMapY / core->scale) > 0.0) {
explorationMap[x][y] = Discovered;
} else if (core->heatMap->getChannelValue(
HeatMap::CollisionAreaChannel, heatMapX / core->scale,
heatMapY / core->scale) > 0.0) {
explorationMap[x][y] = Obstacle;
} else {
explorationMap[x][y] = Unknown;
}
}
}
The algorithm begins with the robots position and calculates for each point the heights depending on
the exploration map. As a result, points near an obstacle, or who are marked as discovered, get a low
height, and unknown points get higher and higher in height, the further away they are from a known
point.
int h;
QQueue<TerrainPointT<int> > queue;
queue.enqueue(TerrainPointT<int> (mapWidth / 2, mapHeight / 2, 0));
// Calculate the heights
while (!queue.isEmpty()) {
TerrainPointT<int> p = queue.dequeue();
if (p.height + 1 > TERRAIN_MAP_MAX_HEIGHT) {
h = TERRAIN_MAP_MAX_HEIGHT;
} else {
h = p.height + 1;
}
for (int k = -1; k <= 1; k++) {
for (int j = -1; j <= 1; j++) {
if (p.x + k < mapWidth && p.x + k >= 0 &&
p.y + j < mapHeight && p.y + j >= 0 &&
(heights[p.x + k][p.y + j] > h ||
heights[p.x + k][p.y + j] ==
TERRAIN_UNKNOWN_HEIGHT)) {
switch (explorationMap[p.x + k][p.y + j]) {
case Obstacle:
heights[p.x + k][p.y + j] = 0;
queue.enqueue(TerrainPointT<int> (p.x + k,
p.y + j, 0));
break;
case Discovered:
heights[p.x + k][p.y + j] = 0;
queue.enqueue(TerrainPointT<int> (p.x + k,
p.y + j, 0));
break;
case Unknown:
heights[p.x + k][p.y + j] = h;
queue.enqueue(TerrainPointT<int> (p.x + k,
p.y + j, h));
break;
}
}
}
}
}
All points which correlate to the given cut level point are found and added to a queue:
std::vector<QPoint> terrainCutPoints;
// Add the points with the terrain cut height to the spline
while (terrainCutPoints.size() < 3) {
for (int x = 0; x < mapWidth; x++) {
for (int y = 0; y < mapHeight; y++) {
if (heights[x][y] == TERRAIN_CUT_LEVEL) {
long navPointX = robotX -
RELATIVE_EXPLORATION_WIDTH / 2 +
x * EXPLORATION_MAP_GRID_SIZE;
long navPointY = robotY -
RELATIVE_EXPLORATION_HEIGHT / 2 +
y * EXPLORATION_MAP_GRID_SIZE;
terrainCutPoints.push_back(QPoint(navPointX,
navPointY));
}
}
}
cutLevel--;
}
The cut level points are in an unsorted list. To make use of these points it is necessary to sort the list
and remove redundant points. For this reason a minimal distance between the cut points is defined.
All points which do not have this distance to the last point will removed from the list:
// Sort the terrain cut points
for (int i = 0; i < terrainCutPoints.size(); i++) {
int distance = INT_MAX;
int nearestPoint = 0;
for (int k = i + 1; k < terrainCutPoints.size(); k++) {
int temp = std::abs(terrainCutPoints[i].x() -
terrainCutPoints[k].x()) +
std::abs(terrainCutPoints[i].y() -
terrainCutPoints[k].y());
if (temp < distance) {
distance = temp;
nearestPoint = k;
}
}
if (i != terrainCutPoints.size() - 1) {
QPoint temp = terrainCutPoints[i + 1];
terrainCutPoints[i + 1] = terrainCutPoints[nearestPoint];
terrainCutPoints[nearestPoint] = temp;
}
}
Finally, the calculated points are registered in the Navigation component.
// Add the terrain cut point to the navigation spline
QPoint lastPointAdded;
for (int i = 0; i < terrainCutPoints.size(); i++) {
// Take only the cut points with the distance CUT_POINT_DISTANCE from
// the last point
if (i == 0) {
this->core->navigation->addNavPoint(terrainCutPoints[i].x(),
terrainCutPoints[i].y());
lastPointAdded = terrainCutPoints[i];
}
if (i != 0 && i != terrainCutPoints.size() - 1 &&
std::sqrt(std::pow(lastPointAdded.x() -
terrainCutPoints[i].x(), 2.0) +
std::pow(lastPointAdded.y() - terrainCutPoints[i].y(),
2.0)) >core->intSetting("
SPLINEDISCOVERYTASK_TERRAIN_CUT_POINT_DISTANCE_MM")) {
this->core->navigation->addNavPoint(terrainCutPoints[i].x(),
terrainCutPoints[i].y());
lastPointAdded = terrainCutPoints[i];
}
}
