Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00008
00009 #include <stdio.h>
00010 #include <stdlib.h>
00011 #include <string.h>
00012
00013 #include "stage.hh"
00014 using namespace Stg;
00015
00016 typedef struct
00017 {
00018 ModelPosition* position;
00019 ModelRanger* ranger;
00020 ModelFiducial* fiducial;
00021
00022 ModelFiducial::Fiducial* closest;
00023 radians_t closest_bearing;
00024 meters_t closest_range;
00025 radians_t closest_heading_error;
00026
00027 } robot_t;
00028
00029
00030 const double VSPEED = 0.4;
00031 const double EXPAND_WGAIN = 0.3;
00032 const double FLOCK_WGAIN = 0.3;
00033 const double SAFE_DIST = 1.0;
00034 const double SAFE_ANGLE = 0.5;
00035
00036
00037
00038 int RangerUpdate( ModelRanger* mod, robot_t* robot );
00039 int FiducialUpdate( ModelFiducial* fid, robot_t* robot );
00040
00041
00042 extern "C" int Init( Model* mod )
00043 {
00044 robot_t* robot = new robot_t;
00045 robot->position = (ModelPosition*)mod;
00046
00047
00048 robot->ranger = (ModelRanger*)mod->GetUnusedModelOfType( "ranger" );
00049 assert( robot->ranger );
00050
00051
00052 robot->ranger->AddCallback( Model::CB_UPDATE, (model_callback_t)RangerUpdate, robot );
00053
00054 robot->fiducial = (ModelFiducial*)mod->GetUnusedModelOfType( "fiducial" ) ;
00055 assert( robot->fiducial );
00056 robot->fiducial->AddCallback( Model::CB_UPDATE, (model_callback_t)FiducialUpdate, robot );
00057
00058 robot->fiducial->Subscribe();
00059 robot->ranger->Subscribe();
00060 robot->position->Subscribe();
00061
00062 return 0;
00063 }
00064
00065 int RangerUpdate( ModelRanger* rgr, robot_t* robot )
00066 {
00067
00068 double dx=0, dy=0;
00069
00070 const std::vector<ModelRanger::Sensor>& sensors = rgr->GetSensors();
00071
00072
00073 for( unsigned int i=0; i < 8; i++ )
00074 {
00075 dx += sensors[i].ranges[0] * cos( sensors[i].pose.a );
00076 dy += sensors[i].ranges[0] * sin( sensors[i].pose.a );
00077 }
00078
00079 if( (dx == 0) || (dy == 0) )
00080 return 0;
00081
00082 double resultant_angle = atan2( dy, dx );
00083 double forward_speed = 0.0;
00084 double side_speed = 0.0;
00085 double turn_speed = EXPAND_WGAIN * resultant_angle;
00086
00087
00088 if( (sensors[3].ranges[0] > SAFE_DIST) &&
00089 (sensors[4].ranges[0] > SAFE_DIST) &&
00090 (sensors[5].ranges[0] > SAFE_DIST ) &&
00091 (sensors[6].ranges[0] > SAFE_DIST/2.0) &&
00092 (sensors[2].ranges[0] > SAFE_DIST ) &&
00093 (sensors[1].ranges[0] > SAFE_DIST/2.0) &&
00094 (fabs( resultant_angle ) < SAFE_ANGLE) )
00095 {
00096 forward_speed = VSPEED;
00097
00098
00099 if( robot->closest )
00100 turn_speed += FLOCK_WGAIN * robot->closest_heading_error;
00101 }
00102 else
00103 {
00104
00105 if( fabs(turn_speed) < 0.1 )
00106 turn_speed = drand48();
00107 }
00108
00109 robot->position->SetSpeed( forward_speed, side_speed, turn_speed );
00110
00111 return 0;
00112 }
00113
00114
00115 int FiducialUpdate( ModelFiducial* fid, robot_t* robot )
00116 {
00117
00118
00119 double dist = 1e6;
00120
00121 robot->closest = NULL;
00122
00123 FOR_EACH( it, fid->GetFiducials() )
00124 {
00125 ModelFiducial::Fiducial* other = &(*it);
00126
00127 if( other->range < dist )
00128 {
00129 dist = other->range;
00130 robot->closest = other;
00131 }
00132 }
00133
00134 if( robot->closest )
00135 {
00136 robot->closest_bearing = robot->closest->bearing;
00137 robot->closest_range = robot->closest->range;
00138 robot->closest_heading_error = robot->closest->geom.a;
00139 }
00140
00141 return 0;
00142 }