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* ranger; 00020 ModelRanger* laser; 00021 ModelPosition* position; 00022 } robot_t; 00023 00024 00025 const double VSPEED = 0.2; // meters per second 00026 const double WGAIN = 1.0; // turn speed gain 00027 const double SAFE_DIST = 0.75; // meters 00028 const double SAFE_ANGLE = 0.5; // radians 00029 00030 00031 // forward declare 00032 int RangerUpdate( ModelRanger* mod, robot_t* robot ); 00033 00034 // Stage calls this when the model starts up 00035 extern "C" int Init( Model* mod ) 00036 { 00037 robot_t* robot = new robot_t; 00038 robot->position = (ModelPosition*)mod; 00039 assert( robot->position ); 00040 00041 // subscribe to the ranger, which we use for navigating 00042 robot->ranger = (ModelRanger*)mod->GetUnusedModelOfType( "ranger" ); 00043 assert( robot->ranger ); 00044 00045 // ask Stage to call into our ranger update function 00046 robot->ranger->AddCallback( Model::CB_UPDATE, (model_callback_t)RangerUpdate, robot ); 00047 00048 // subscribe to the laser, though we don't use it for navigating 00049 robot->laser = (ModelRanger*)mod->GetUnusedModelOfType( "ranger" ); 00050 assert( robot->laser ); 00051 00052 // start the models updating 00053 robot->ranger->Subscribe(); 00054 robot->position->Subscribe(); 00055 //robot->laser->Subscribe(); 00056 00057 00058 return 0; //ok 00059 } 00060 00061 int RangerUpdate( ModelRanger* rgr, robot_t* robot ) 00062 { 00063 // compute the vector sum of the sonar ranges 00064 double dx=0, dy=0; 00065 00066 const std::vector<ModelRanger::Sensor>& sensors = rgr->GetSensors(); 00067 00068 FOR_EACH( it, sensors ) 00069 { 00070 const ModelRanger::Sensor& s = *it; 00071 dx += s.ranges[0] * cos( s.pose.a ); 00072 dy += s.ranges[0] * sin( s.pose.a ); 00073 00074 //printf( "sensor %d angle= %.2f\n", s, rgr->sensors[s].pose.a ); 00075 } 00076 00077 if( (dx == 0) || (dy == 0) ) 00078 return 0; 00079 00080 double resultant_angle = atan2( dy, dx ); 00081 double forward_speed = 0.0; 00082 double side_speed = 0.0; 00083 double turn_speed = WGAIN * resultant_angle; 00084 00085 //printf( "resultant %.2f turn_speed %.2f\n", resultant_angle, turn_speed ); 00086 00087 // if the front is clear, drive forwards 00088 if( (sensors[3].ranges[0] > SAFE_DIST) && // forwards 00089 (sensors[4].ranges[0] > SAFE_DIST) && 00090 (sensors[5].ranges[0] > SAFE_DIST/2.0) && // 00091 (sensors[6].ranges[0] > SAFE_DIST/4.0) && 00092 (sensors[2].ranges[0] > SAFE_DIST/2.0) && 00093 (sensors[1].ranges[0] > SAFE_DIST/4.0) && 00094 (fabs( resultant_angle ) < SAFE_ANGLE) ) 00095 { 00096 forward_speed = VSPEED; 00097 } 00098 00099 robot->position->SetSpeed( forward_speed, side_speed, turn_speed ); 00100 00101 return 0; 00102 } 00103