wander.cc
Go to the documentation of this file.
00001 #include "stage.hh"
00002 using namespace Stg;
00003 
00004 static const double cruisespeed = 0.4; 
00005 static const double avoidspeed = 0.05; 
00006 static const double avoidturn = 0.5;
00007 static const double minfrontdistance = 1.0; // 0.6  
00008 static const bool verbose = false;
00009 static const double stopdist = 0.3;
00010 static const int avoidduration = 10;
00011 
00012 typedef struct
00013 {
00014   ModelPosition* pos;
00015   ModelRanger* laser;
00016   int avoidcount, randcount;
00017 } robot_t;
00018 
00019 int LaserUpdate( Model* mod, robot_t* robot );
00020 int PositionUpdate( Model* mod, robot_t* robot );
00021 
00022 // Stage calls this when the model starts up
00023 extern "C" int Init( Model* mod, CtrlArgs* args )
00024 {
00025   // local arguments
00026   /*  printf( "\nWander controller initialised with:\n"
00027       "\tworldfile string \"%s\"\n" 
00028       "\tcmdline string \"%s\"",
00029       args->worldfile.c_str(),
00030       args->cmdline.c_str() );
00031   */
00032 
00033   robot_t* robot = new robot_t;
00034  
00035   robot->avoidcount = 0;
00036   robot->randcount = 0;
00037   
00038   robot->pos = (ModelPosition*)mod;
00039   
00040   if( verbose )
00041     robot->pos->AddCallback( Model::CB_UPDATE, (model_callback_t)PositionUpdate, robot );
00042   
00043   robot->pos->Subscribe(); // starts the position updates
00044   
00045   robot->laser = (ModelRanger*)mod->GetChild( "ranger:1" );
00046   robot->laser->AddCallback( Model::CB_UPDATE, (model_callback_t)LaserUpdate, robot );
00047   robot->laser->Subscribe(); // starts the ranger updates
00048   
00049   return 0; //ok
00050 }
00051 
00052 
00053 // inspect the ranger data and decide what to do
00054 int LaserUpdate( Model* mod, robot_t* robot )
00055 {
00056   // get the data
00057   const std::vector<meters_t>& scan = robot->laser->GetSensors()[0].ranges;
00058   uint32_t sample_count = scan.size();
00059   if( sample_count < 1 )
00060     return 0;
00061   
00062   bool obstruction = false;
00063   bool stop = false;
00064         
00065   // find the closest distance to the left and right and check if
00066   // there's anything in front
00067   double minleft = 1e6;
00068   double minright = 1e6;
00069 
00070   for (uint32_t i = 0; i < sample_count; i++)
00071     {
00072 
00073       if( verbose ) printf( "%.3f ", scan[i] );
00074 
00075       if( (i > (sample_count/3)) 
00076           && (i < (sample_count - (sample_count/3))) 
00077           && scan[i] < minfrontdistance)
00078         {
00079           if( verbose ) puts( "  obstruction!" );
00080           obstruction = true;
00081         }
00082                 
00083       if( scan[i] < stopdist )
00084         {
00085           if( verbose ) puts( "  stopping!" );
00086           stop = true;
00087         }
00088       
00089       if( i > sample_count/2 )
00090         minleft = std::min( minleft, scan[i] );
00091       else      
00092         minright = std::min( minright, scan[i] );
00093     }
00094   
00095   if( verbose ) 
00096     {
00097       puts( "" );
00098       printf( "minleft %.3f \n", minleft );
00099       printf( "minright %.3f\n ", minright );
00100     }
00101 
00102   if( obstruction || stop || (robot->avoidcount>0) )
00103     {
00104       if( verbose ) printf( "Avoid %d\n", robot->avoidcount );
00105                         
00106       robot->pos->SetXSpeed( stop ? 0.0 : avoidspeed );      
00107       
00108       /* once we start avoiding, select a turn direction and stick
00109          with it for a few iterations */
00110       if( robot->avoidcount < 1 )
00111         {
00112           if( verbose ) puts( "Avoid START" );
00113           robot->avoidcount = random() % avoidduration + avoidduration;
00114                          
00115           if( minleft < minright  )
00116             {
00117               robot->pos->SetTurnSpeed( -avoidturn );
00118               if( verbose ) printf( "turning right %.2f\n", -avoidturn );
00119             }
00120           else
00121             {
00122               robot->pos->SetTurnSpeed( +avoidturn );
00123               if( verbose ) printf( "turning left %2f\n", +avoidturn );
00124             }
00125         }
00126                 
00127       robot->avoidcount--;
00128     }
00129   else
00130     {
00131       if( verbose ) puts( "Cruise" );
00132 
00133       robot->avoidcount = 0;
00134       robot->pos->SetXSpeed( cruisespeed );       
00135       robot->pos->SetTurnSpeed(  0 );
00136     }
00137 
00138   //  if( robot->pos->Stalled() )
00139   //     {
00140   //            robot->pos->SetSpeed( 0,0,0 );
00141   //            robot->pos->SetTurnSpeed( 0 );
00142   // }
00143                         
00144   return 0; // run again
00145 }
00146 
00147 int PositionUpdate( Model* mod, robot_t* robot )
00148 {
00149   Pose pose = robot->pos->GetPose();
00150 
00151   printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
00152           pose.x, pose.y, pose.z, pose.a );
00153 
00154   return 0; // run again
00155 }
00156 


stage
Author(s): Richard Vaughan , Brian Gerkey , Reed Hedges , Andrew Howard , Toby Collett , Pooya Karimian , Jeremy Asher , Alex Couture-Beil , Geoff Biggs , Rich Mattes , Abbas Sadat
autogenerated on Thu Aug 27 2015 15:20:57