00001 00002 // File: stest.c 00003 // Desc: Stage library test program 00004 // Created: 2004.9.15 00005 // Author: Richard Vaughan <vaughan@sfu.ca> 00006 // CVS: $Id: stest.cc,v 1.3 2008-02-01 03:11:02 rtv Exp $ 00007 // License: GPL 00009 00010 #include <stdio.h> 00011 #include <stdlib.h> 00012 #include <string.h> 00013 00014 #include "stage.hh" 00015 using namespace Stg; 00016 00017 typedef struct 00018 { 00019 ModelRanger* laser; 00020 ModelPosition* position; 00021 ModelRanger* ranger; 00022 } robot_t; 00023 00024 // swarmbot 00025 const double VSPEED = 0.3; // meters per second 00026 const double WGAIN = 1.0; // turn speed gain 00027 const double SAFE_DIST = 0.5; // meters 00028 const double SAFE_ANGLE = 0.5; // radians 00029 00030 // forward declare 00031 int RangerUpdate( ModelRanger* mod, robot_t* robot ); 00032 00033 // Stage calls this when the model starts up 00034 extern "C" int Init( Model* mod ) 00035 { 00036 robot_t* robot = new robot_t; 00037 robot->position = (ModelPosition*)mod; 00038 00039 // subscribe to the ranger, which we use for navigating 00040 robot->ranger = (ModelRanger*)mod->GetUnusedModelOfType( "ranger" ); 00041 assert( robot->ranger ); 00042 00043 00044 // ask Stage to call into our ranger update function 00045 robot->ranger->AddCallback( Model::CB_UPDATE, (model_callback_t)RangerUpdate, robot ); 00046 00047 // subscribe to the laser, though we don't use it for navigating 00048 //robot->laser = (ModelLaser*)mod->GetUnusedModelofType( "laser" ); 00049 //assert( robot->laser ); 00050 00051 // start the models updating 00052 robot->position->Subscribe(); 00053 robot->ranger->Subscribe(); 00054 //robot->laser->Subscribe(); 00055 00056 return 0; //ok 00057 } 00058 00059 int RangerUpdate( ModelRanger* rgr, robot_t* robot ) 00060 { 00061 // compute the vector sum of the sonar ranges 00062 double dx=0, dy=0; 00063 00064 const std::vector<ModelRanger::Sensor>& sensors = rgr->GetSensors(); 00065 00066 FOR_EACH( it, sensors ) 00067 { 00068 const ModelRanger::Sensor& s = *it; 00069 dx += s.ranges[0] * cos( s.pose.a ); 00070 dy += s.ranges[0] * sin( s.pose.a ); 00071 00072 //printf( "sensor %d angle= %.2f\n", s, rgr->sensors[s].pose.a ); 00073 } 00074 00075 if( (dx == 0) || (dy == 0) ) 00076 return 0; 00077 00078 double resultant_angle = atan2( dy, dx ); 00079 double forward_speed = 0.0; 00080 double side_speed = 0.0; 00081 double turn_speed = WGAIN * resultant_angle; 00082 00083 //printf( "resultant %.2f turn_speed %.2f\n", resultant_angle, turn_speed ); 00084 00085 // if the front is clear, drive forwards 00086 if( (sensors[0].ranges[0] > SAFE_DIST) && 00087 00088 (sensors[1].ranges[0] > SAFE_DIST/1.5) && 00089 (sensors[2].ranges[0] > SAFE_DIST/3.0) && 00090 (sensors[3].ranges[0] > SAFE_DIST/5.0) && 00091 00092 (sensors[9].ranges[0] > SAFE_DIST/5.0) && 00093 (sensors[10].ranges[0] > SAFE_DIST/3.0) && 00094 (sensors[11].ranges[0] > SAFE_DIST/1.5) && 00095 (fabs( resultant_angle ) < SAFE_ANGLE) ) 00096 { 00097 forward_speed = VSPEED; 00098 } 00099 00100 robot->position->SetSpeed( forward_speed, side_speed, turn_speed ); 00101 00102 return 0; 00103 } 00104