BeeSoft User's Guide and Reference

4.5 Using colliServer for Guarded Motion

baseServer, discussed above, operates the robot in non-guarded motion. That is, baseServer includes no built-in collision avoidance software. For many applications, that might be sufficient. But if your robotic application requires that extra margin of positive control and enhanced safety that built in collision avoidance can provide, use BeeSoft’s built in colliServer.

colliServer is a collection of robot movement commands that direct the robot’s movement in guarded motion mode, letting the robot travel autonomously while it automatically tries to avoid collisions with those obstacles it can perceive with its various sensors, while making the most rapid and efficient progress possible towards its intended target point.

The robot naturally wants to maximize its translation, that is, to move ahead as swiftly as possible, while at the same time to minimize the angle between its intended target point and its current heading. The robot is also highly motivated to move towards areas that are free of obstacles. When determining how the robot moves and behaves, colliServer continuously trades off these three robotic desires to maximize the robot’s navigational efficiency, while allowing the programmer the flexibility to decide which robotic movement characteristics to trade off to optimize the robot’s performance in a particular application.

colliServer takes a "dynamic window" approach. That is, it pays attention only to the last few sensor readings, in a small evanescent "window" of time. This facilitates the robot’s paying attention to fast-moving obstacles (such as people walking around), as well as allowing it to avoid obstacles that are not marked on its internal map. On the negative side, while such an approach makes possible optimization in a local navigation sense, the resulting robot movement may not be globally optimal.

4.5.1 The colliServer Parameter File

A parameter file called colli_modes.ini includes a variety of parameters for the different available modes of guarded robotic travel available under colliServer. Each of these parameters can be "weighted" somewhat differently, depending on the factors considered most important for a particular robot task.

The three most important parameters are:

VELOCITY_FACTOR: determines how important maximizing speed shall be. This includes such considerations as how fast can the robot safely travel so that it can always cometo a safe stop without hitting anything. For different directions of motion, different speeds will be optimally safe.

ANGLE_FACTOR: measures how much the robot’s actual heading direction will be allowed to deviate from the optimal heading towards the target point in the next second. Minimizing this factor maximizes progress towards the target point. This factor helps determine how aggressive the robot shall be in coming close to objects in order to reach its target faster.

DISTANCE_FACTOR: determines how intent the robot will be in moving towards uncluttered, or unobstructed, space, the space with the fewest obstacles. The robot always wants to move into free space. This factor helps determine how cautious the robot shall be about keeping its distance from obstacles it perceives.

Depending on how you want the robot to behave, you can set these parameters in any number of combinations. For example, if your experiments find that your robot is paying insufficient attention to reaching its target in an efficient manner (that is, if it tends to scuttle and wander rather aimlessly), try increasing ANGLE_FACTOR. This should help the robot minimize the back-and-forth wandering in favor of forging a more straightforward path towards its target. With an increased ANGLE_FACTOR, the robot will go straighter towards its target, even if it has to slow down and/or come closer to obstacles.

Conversely, if you decrease ANGLE_FACTOR, the robot moves faster into free space. If you set ANGLE_FACTOR to zero, the robot ignores the target point completely.

However you set the three factors, though, it is their relative values that matter. The three factors are not directly comparable, since they are derived on different scales and from different criteria. Therefore, a bit of experimentation is in order before you can determine the optimal combination of factors for what you want your robot to accomplish. It really is a trial and error process.

Expert’s Tip:

For some tips on robotic navigation using colliServer, see the paper Map Learning and High Speed Navigation in RHINO, by Sebastian Thrun, et al. (Institut für Informatik III, University of Bonn and Carnegie Mellon University). Interested roboticists can obtain a copy of this paper through RWI, Inc. customer support.

Fortunately, colli_modes.ini simplifies this task for you by providing several predefined modes that are likely to cover most of the robotic navigation situations you’ll encounter. Each mode, in addition to specifying values for the three factors discussed above, also specifies a number ofother parameters:

TARGET_MAX_TRANS_SPEED: Maximum forwards/backwards speed.

TARGET_MAX_ROT_SPEED: Maximum rotational speed.

TARGET_TRANS_ACCELERATION: Maximum forwards/backwards acceleration.

TARGET_ROT_ACCELERATION: Maximum rotational acceleration.

MIN_DIST: This is the minimum distance, in centimeters, that the robot needs to be clear directly ahead of itself. If the robot does find itself closer than the MIN_DIST to an obstacle, it will back up to maintain its distance.

SECURITY_DIST: This is a value, specified in centimeters, that for purposes of navigation and obstacle avoidance is added to the actual physical circumference of the robot. A larger security renders the robot more protective (of itself and its surroundings); a smaller security distance renders the robot more aggressive in its travels by allowing it to get that much closer to obstacles.

When setting values for these parameters, be aware of the physical upper limits of the robot hardware:

Translation speed: 90 centimeters per second

Rotation speed: 70 degrees per second

Translation acceleration: 50 centimeters per second squared

Rotational acceleration: 50 degrees per second squared

colliServer provides three modes of general interest, although you may see other, more specialized, modes in colli_modes.ini.

DEFAULT_MODE: This is the mode used for most general applications, and causes reasonably balanced robotic behavior.

FAST_TRAVEL_MODE: This mode maximizes speed at the expense of forging the most straightforward path possible.

RANDOM_MODE: In this mode, ANGLE_FACTOR is set to 0.01, effectively ignoring the target point. The robot just moves around randomly, as quickly as possible, rather than systematically explore or head for a target.

Study these predefined modes to see how they are likely to make the robot behave. Forexample, in FAST_TRAVEL_MODE, the robot is allowed a TARGET_MAX_TRANS_SPEED of 70 cm/sec, but requires a MIN_DIST of 20 cm and a SECURITY_DIST of 15 cm. In contrast, DEFAULT_MODE only allows a TARGET_MAX_TRANS_SPEED of 30 cm/sec, but cuts down the MIN_DIST requirement to 15 cm and the SECURITY_DIST to 10 cm.

colli_modes.ini also includes many other parameters you can examine and, when you have a bit more experience, change for various purposes. Many of these are for use in highly specialized situations only.

4.5.2 colliServer Commands: Init and StartUp

void colliRegister (void); - Registers colliServer.

void colliConnect (void); - Connects colliServer to the running system.

4.5.3 colliServer Commands: Controlling the Robot

void colliSetMode (int mode);

Specifies the mode you want to use. DEFAULT_MODE (0), FAST_TRAVEL_MODE (1), RANDOM_MODE or other, user-define or specialized mode. (See colli_modes.ini.)

void colliStopRobot (void); - Stops robot motion.

void colliSetVelocity (float trans_speed, float rot_speed);

Set the robot’s initial velocity values in centimeters per second. The velocity being set here is the maximum velocity, for purposes of the colliServer collision avoidance algorithms. In ordinary operation under colliServer, the robot’s operating velocities at any given instant are determined by how cluttered the robot’s environment is. These velocity values should be left at the defaults unless you have a good reason to change them.

void colliSetAcceleration (float trans_accel, float rot_accel);

Set the robot’s initial acceleration values, in centimeters per second squared.

void colliApproachAbsolute (float x, float y, float approachDist, int newTarget);

Note: This command uses the radians-based coordinate system described in Section 4.2.3, Odometry and Related Commands, above. This command directs the robot to proceed towards an absolute target point, specified in absolute world coordinates, specified by the supplied x and y coordinates. If newTarget =1, the target is considered a completely new target, telling the robot to stop, turn around towards the specified direction, then head towards the new specified target. If newTarget = 0, the target value is assumes to be an incremental step; the robot just proceeds along its way towards it.

The approachDist, in centimeters, is the distance at which the robot shall consider that it has successfully arrived at the specified target. When the robot comes within that distance fromthe target, it will stop. If given a zero approachDist, the robot will simply circle around the target point indefinitely, because it can never gain it exactly. A good general approachDist to use is 1 meter. The collision avoidance algorithms within colliServer determine the actual velocity and the actual direction of motion at any given instant. colliServer re-computes these approximately four times per second.

void colliApproachRelative (float x, float y, float approachDist, int newTarget);

Note: This command uses the radians-based coordinate system described in Section 4.2.3, Odometry and Related Commands, above. This command directs the robot to proceed towards a target point relative to the robot’s current location, specified by the supplied x and y coordinates. If newTarget =1, the target is considered a completely new target, telling the robot to stop, turn around towards the specified direction, then head towards the new specified target. If newTarget = 0, the target value is assumes to be an incremental step; the robot just proceeds along its way towards it.

The approachDist, in centimeters, is the distance at which the robot shall consider that it has successfully arrived at the specified target. When the robot comes within that distance from the target, it will stop. If given a zero approachDist, the robot will simply circle around the target point indefinitely, because it can never gain it exactly. A good general approachDist to use is 1 meter.

4.5.4 A Sample colliServer Program

The following simple program, colliTest.c, starts up the robot under colliServer, directs it towards a target point and quits after thirty seconds. Use this program as a template to write your own programs and to experiment with robotic control using colliServer.

/*===========================================*/
/*===========================================*/
colliTest.c
/*===========================================*/
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include <baseClient.h>
#include <colliClient.h>
#include <rai.h>
/*===========================================*/
int gotOrigin = FALSE;
int statusCount = 0;
int
tactileCallback(tactileType ** newTactile)
{
     int i;
     int j;
     /* Stop the robot if any tactile sensor */
     /* is pushed against. */
     for (i=0; i<NUM_TACTILE_ROWS; i++) {
          for (j=0; j<tactilesInRow[i];j++) {
               If (tactiles[i] [j].value) {
                    printf ("**** Touched on %d, %d\n",i,j);
                    bsetVel (0.0, 0.0);
                    return;
               }
          }
     }
     return;
}
/*===========================================*/
/* This routine issues the colliApproachAbsolute */
/* command the first time it is polled after the */
/* origin is established. */
void
demoPoll (RaiModule * demoModule)
{
     static time = 0;
     if (gotOrigin) {
          if (time == 0) {
            colliApproachAbsolute (-100.0, 0.0, 50.0, 1);
          }
          if (time>300) {
               RaiShutdown ( );
          }
          time++;
     }
     return;
}
/*===========================================*/
void
statusCallback (statusReportType * newStatus)
{
     statusCount++;
     if ((!gotOrigin) && (statusCount < 10)) {
          bSetPosition (0.0, 0.0, 0.0);
          gotOrigin = TRUE;
     }
}
/*===========================================*/
void
createDemoModule ( );
{
     RaiModule* demoModule;
     int ii;
     printf ("Setting up robot program\n");
/* Set robot’s heading; units are radians*512/pi. */
     loadHeading (0);
/* Set the robot’s initial position.*/
/* Units are encoder counts/256. */
     loadPosition (0x80008000);
/* Set up the status reporting timing; */
/* Units are seconds*256. */
     statusReportPeriod (100*256/1000);
     registerStatusCallback (statusCallback);
     registerTactileCallback (tactileCallback);
/* Set the robot’s initial velocities, in cm/sec.  */
     colliSetVelocity (20.0, PI/2.0);
/* Set the robot’s initial acceleration values, */
/* in centimeters per second squared. */
     colliSetAcceleration (60.0, 20.*PI);
/* Set up polling for every 100 milliseconds. */
     demoModule=makeModule ("Demo", NULL);
     addPolling (demoModule, demoPoll, 100);
     fprintf (stderr, "done.\m");
}
/*===========================================*/
void
/* set up the shutdown function. */
commShutdown ( )
{
     printf ("somebody died. Exiting.\n");
     RaiShutdown( );
}
/*===========================================*/
/* This program starts up the robot, goes to */
/* a target point and quits after 30 seconds. */
int
main (int argc, char** argv)
{
     fprintf (stderr, "starting main\n");
     registerBaseClient ( );
     colliRegister ( );
     /* The shutdown function will be called */
     /* if baseServer dies. */
     initClient ("colliTest", commShutdown);
     /* Hook up to running baseServer. */
     findBaseServer ( );
     colliConnect ( );
     /* Initialize, but do not start,*/
     /* the BeeSoft Scheduler. */
     RaiInit ( );
     catchInterrupts ( );
     /* Set up modules to do your communication. */
     initClientModules ( );
     /* Set up your user client application */
     /* to move the robot. */
     createDemoModule ( );
     /* Fire it up! */
     RaiStart ( );
     /* This will return only when RaiShutdown */
     /* is called, when <Ctrl/c> is pressed, */
     /* or when 30 seconds expire. */
     return;
}
/*===========================================*/ 

In This Chapter You’ve Learned...

...about the coordinate systems for directing your robot’s maneuvers. You also know how to command your robot to move forwards and backwards ("translation"), to turn on its axis ("rotation") and how to perform such useful functions as setting speed of robot travel, acceleration and torque.

You’ve discovered the error reports and status reports available from the robot’s base.

You’ve become acquainted with the functions of the watchdog timer, the facility that insures that your robot will halt motion as quickly and safely as possible if your program crashes or you experience unanticipated network problems.

Finally, you’ve learned about the difference between guarded motion, under the control of colliServer, and non-guarded motion, the kind of motion baseServer provides. With the aid of a simple test program, you’ve experimented with the colliServer commands and with the flexibility offered by the parameters in the colliServer parameters file, colli_modes.ini.