p_driver.cc
Go to the documentation of this file.
00001 /*
00002  *  Stage plugin driver for Player
00003  *  Copyright (C) 2004-2005 Richard Vaughan
00004  *
00005  *  This program is free software; you can redistribute it and/or modify
00006  *  it under the terms of the GNU General Public License as published by
00007  *  the Free Software Foundation; either version 2 of the License, or
00008  *  (at your option) any later version.
00009  *
00010  *  This program is distributed in the hope that it will be useful,
00011  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  *  GNU General Public License for more details.
00014  *
00015  *  You should have received a copy of the GNU General Public License
00016  *  along with this program; if not, write to the Free Software
00017  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018  *
00019  */
00020 
00021 /*
00022  * Desc: A plugin driver for Player that gives access to Stage devices.
00023  * Author: Richard Vaughan
00024  * Date: 10 December 2004
00025  * CVS: $Id$
00026  */
00027 
00028 // DOCUMENTATION ------------------------------------------------------------
00029 
00141 // TODO - configs I should implement
00142 //  - PLAYER_BLOBFINDER_SET_COLOR_REQ
00143 //  - PLAYER_BLOBFINDER_SET_IMAGER_PARAMS_REQ
00144 
00145 // CODE ------------------------------------------------------------
00146 
00147 #include <unistd.h>
00148 #include <string.h>
00149 #include <math.h>
00150 #include "config.h"
00151 #include "p_driver.h"
00152 using namespace Stg;
00153 
00154 const char* copyright_notice =
00155 "\n * Part of the Player Project [http://playerstage.sourceforge.net]\n"
00156 " * Copyright 2000-2009 Richard Vaughan, Brian Gerkey and contributors.\n"
00157 " * Released under the GNU General Public License v2.\n";
00158 
00159 #define STG_DEFAULT_WORLDFILE "default.world"
00160 #define DRIVER_ERROR(X) printf( "Stage driver error: %s\n", X )
00161 
00162 // globals from Player
00163 extern PlayerTime* GlobalTime;
00164 //extern int global_argc;
00165 //extern char** global_argv;
00166 extern bool player_quiet_startup;
00167 extern bool player_quit;
00168 
00169 // init static vars
00170 World* StgDriver::world = NULL;
00171 bool StgDriver::usegui = true;
00172 
00173 //int update_request = 0;
00174 
00175 /* need the extern to avoid C++ name-mangling  */
00176 extern "C"
00177 {
00178   int player_driver_init(DriverTable* table);
00179 }
00180 
00181 // A factory creation function, declared outside of the class so that it
00182 // can be invoked without any object context (alternatively, you can
00183 // declare it static in the class).  In this function, we create and return
00184 // (as a generic Driver*) a pointer to a new instance of this driver.
00185 Driver* StgDriver_Init(ConfigFile* cf, int section)
00186 {
00187   // Create and return a new instance of this driver
00188   return ((Driver*) (new StgDriver(cf, section)));
00189 }
00190 
00191 // A driver registration function, again declared outside of the class so
00192 // that it can be invoked without object context.  In this function, we add
00193 // the driver into the given driver table, indicating which interface the
00194 // driver can support and how to create a driver instance.
00195 void StgDriver_Register(DriverTable* table)
00196 {
00197   printf( "\n ** %s plugin v%s **",  PROJECT, VERSION ); // XX TODO
00198 
00199   if( !player_quiet_startup )
00200     {
00201       puts( copyright_notice );
00202     }
00203 
00204   table->AddDriver( (char*)"stage", StgDriver_Init);
00205 }
00206 
00207 int player_driver_init(DriverTable* table)
00208 {
00209   puts(" Stage driver plugin init");
00210   StgDriver_Register(table);
00211   return(0);
00212 }
00213 
00214 Interface::Interface(  player_devaddr_t addr,
00215                        StgDriver* driver,
00216                        ConfigFile* cf,
00217                        int section )
00218 {
00219   //puts( "Interface constructor" );
00220 
00221   this->last_publish_time = 0;
00222   this->addr = addr;
00223   this->driver = driver;
00224   this->publish_interval_msec = 100; // todo - do this properly
00225 }
00226 
00227 InterfaceModel::InterfaceModel(  player_devaddr_t addr,
00228                                                                                         StgDriver* driver,
00229                                                                                         ConfigFile* cf,
00230                                                                                         int section,
00231                                                                                         const std::string& type )
00232   : Interface( addr, driver, cf, section ), mod( NULL ), subscribed( false )
00233 {
00234   char* model_name = (char*)cf->ReadString(section, "model", NULL );
00235 
00236   if( model_name == NULL )
00237     {
00238       PRINT_ERR1( "device \"%s\" uses the Stage driver but has "
00239                   "no \"model\" value defined. You must specify a "
00240                   "model name that matches one of the models in "
00241                   "the worldfile.",
00242                   model_name );
00243       return; // error
00244     }
00245 
00246   // find a model of the right type
00247   this->mod = driver->LocateModel( model_name,
00248                                    &addr,
00249                                    type );
00250 
00251   // Use same update interval as the model
00252   this->publish_interval_msec = this->mod->GetUpdateInterval()/1000;
00253     
00254   if( !this->mod )
00255     {
00256       printf( " ERROR! no model available for this device."
00257               " Check your world and config files.\n" );
00258 
00259       exit(-1);
00260       return;
00261     }
00262 
00263   if( !player_quiet_startup )
00264     printf( "\"%s\"\n", this->mod->Token() );
00265 }
00266 
00267 void InterfaceModel::Subscribe()
00268 {
00269   if( !subscribed && this->mod )
00270     {
00271       this->mod->Subscribe();
00272       subscribed = true;
00273     }
00274 }
00275 
00276 void InterfaceModel::Unsubscribe()
00277 {
00278   if( subscribed )
00279     {
00280       this->mod->Unsubscribe();
00281       subscribed = false;
00282     }
00283 }
00284 
00285 
00286 // Constructor.  Retrieve options from the configuration file and do any
00287 // pre-Setup() setup.
00288 
00289 // configure the underlying driver to queue incoming commands and use a very long queue.
00290 
00291 StgDriver::StgDriver(ConfigFile* cf, int section)
00292         : Driver(cf, section, false, 4096 ),
00293                 devices()
00294 {
00295   // init the array of device ids
00296 
00297   int device_count = cf->GetTupleCount( section, "provides" );
00298 
00299   //if( !player_quiet_startup )
00300   // {
00301       //printf( "  Stage driver creating %d %s\n",
00302       //      device_count,
00303       //      device_count == 1 ? "device" : "devices" );
00304   // }
00305 
00306   for( int d=0; d<device_count; d++ )
00307     {
00308       player_devaddr_t player_addr;
00309 
00310       if (cf->ReadDeviceAddr( &player_addr, section,
00311                               "provides", 0, d, NULL) != 0)
00312         {
00313           this->SetError(-1);
00314           return;
00315         }
00316 
00317       if( !player_quiet_startup )
00318         {
00319           printf( " Stage plugin:  %d.%s.%d is ",
00320                   player_addr.robot,
00321                   interf_to_str(player_addr.interf),
00322                   player_addr.index );
00323           fflush(stdout);
00324         }
00325 
00326 
00327       Interface *ifsrc = NULL;
00328 
00329       switch( player_addr.interf )
00330         {
00331         case PLAYER_ACTARRAY_CODE:
00332           ifsrc = new InterfaceActArray( player_addr,  this, cf, section );
00333           break;
00334 
00335         // case PLAYER_AIO_CODE:
00336         //   ifsrc = new InterfaceAio( player_addr,  this, cf, section );
00337         //   break;
00338 
00339         case PLAYER_BLOBFINDER_CODE:
00340           ifsrc = new InterfaceBlobfinder( player_addr,  this, cf, section );
00341           break;
00342 
00343         // case PLAYER_DIO_CODE:
00344         //      ifsrc = new InterfaceDio(player_addr, this, cf, section);
00345         //      break;
00346 
00347         case PLAYER_FIDUCIAL_CODE:
00348                 ifsrc = new InterfaceFiducial( player_addr,  this, cf, section );
00349                 break;
00350 
00351         case PLAYER_RANGER_CODE:
00352                 ifsrc = new InterfaceRanger( player_addr,  this, cf, section );
00353                 break;
00354 
00355         case PLAYER_POSITION2D_CODE:
00356                 ifsrc = new InterfacePosition( player_addr, this,  cf, section );
00357                 break;
00358 
00359         case PLAYER_SIMULATION_CODE:
00360           ifsrc = new InterfaceSimulation( player_addr, this, cf, section );
00361           break;
00362 
00363         case PLAYER_SPEECH_CODE:
00364                 ifsrc = new InterfaceSpeech( player_addr,  this, cf, section );
00365                 break;
00366 
00367         //      case PLAYER_CAMERA_CODE:
00368         //        ifsrc = new InterfaceCamera( player_addr,  this, cf, section );
00369         //        break;
00370 
00371         case PLAYER_GRAPHICS2D_CODE:
00372                 ifsrc = new InterfaceGraphics2d( player_addr,  this, cf, section );
00373                 break;
00374 
00375         case PLAYER_GRAPHICS3D_CODE:
00376                 ifsrc = new InterfaceGraphics3d( player_addr,  this, cf, section );
00377                 break;
00378 
00379 
00380 
00381 //      case PLAYER_LOCALIZE_CODE:
00382 //        ifsrc = new InterfaceLocalize( player_addr,  this, cf, section );
00383 //        break;
00384 
00385 //      case PLAYER_MAP_CODE:
00386 //        ifsrc = new InterfaceMap( player_addr,  this, cf, section );
00387 //        break;
00388 
00389         case PLAYER_GRIPPER_CODE:
00390           ifsrc = new InterfaceGripper( player_addr,  this, cf, section );
00391           break;
00392 
00393 //      case PLAYER_WIFI_CODE:
00394 //        ifsrc = new InterfaceWifi( player_addr,  this, cf, section );
00395 //        break;
00396 
00397 //      case PLAYER_POWER_CODE:
00398 //        ifsrc = new InterfacePower( player_addr,  this, cf, section );
00399 //        break;
00400 
00401 //      case PLAYER_PTZ_CODE:
00402 //        ifsrc = new InterfacePtz( player_addr,  this, cf, section );
00403 //        break;
00404 
00405 //      case PLAYER_BUMPER_CODE:
00406 //        ifsrc = new InterfaceBumper( player_addr,  this, cf, section );
00407 //        break;
00408 
00409 
00410         default:
00411           PRINT_ERR1( "error: stage driver doesn't support interface type %d\n",
00412                       player_addr.interf );
00413           this->SetError(-1);
00414           return;
00415         }
00416 
00417       if( ifsrc )
00418         {
00419           // attempt to add this interface and we're done
00420           if( this->AddInterface( ifsrc->addr))
00421             {
00422               DRIVER_ERROR( "AddInterface() failed" );
00423               this->SetError(-2);
00424               return;
00425             }
00426 
00427           // store the Interaface in our device list
00428           //g_ptr_array_add( this->devices, ifsrc );
00429                 devices.push_back( ifsrc );
00430         }
00431       else
00432         {
00433           PRINT_ERR3( "No Stage source found for interface %d:%d:%d",
00434                       player_addr.robot,
00435                       player_addr.interf,
00436                       player_addr.index );
00437 
00438           this->SetError(-3);
00439           return;
00440         }
00441     }
00442   //puts( "  Stage driver loaded successfully." );
00443 }
00444 
00445 Model*  StgDriver::LocateModel( char* basename,
00446                                    player_devaddr_t* addr,
00447                                   const std::string& type )
00448 {
00449   //printf( "attempting to find a model under model \"%s\" of type [%d]\n",
00450   //     basename, type );
00451 
00452   Model* base_model = world->GetModel( basename );
00453 
00454   if( base_model == NULL )
00455     {
00456       PRINT_ERR1( " Error! can't find a Stage model named \"%s\"",
00457                                                 basename );
00458       return NULL;
00459     }
00460 
00461   if( type == "" ) // if we don't care what type the model is
00462     return base_model;
00463 
00464   //  printf( "found base model %s\n", base_model->Token() );
00465 
00466   // we find the first model in the tree that is the right
00467   // type (i.e. has the right initialization function) and has not
00468   // been used before
00469   //return( model_match( base_model, addr, typestr, this->devices ) );
00470   return( base_model->GetUnusedModelOfType( type ) );
00471 }
00472 
00474 // Set up the device.  Return 0 if things go well, and -1 otherwise.
00475 int StgDriver::Setup()
00476 {
00477   puts("stage driver setup");
00478   world->Start();
00479   return(0);
00480 }
00481 
00482 // find the device record with this Player id
00483 // todo - faster lookup with a better data structure
00484 Interface* StgDriver::LookupDevice( player_devaddr_t addr )
00485 {
00486         FOR_EACH( it, this->devices )
00487     {
00488       Interface* candidate = *it;
00489                         
00490       if( candidate->addr.robot == addr.robot &&
00491                                         candidate->addr.interf == addr.interf &&
00492                                         candidate->addr.index == addr.index )
00493                                 return candidate; // found
00494     }
00495         
00496   return NULL; // not found
00497 }
00498 
00499 
00500 // subscribe to a device
00501 int StgDriver::Subscribe(QueuePointer &queue,player_devaddr_t addr)
00502 {
00503   if( addr.interf == PLAYER_SIMULATION_CODE )
00504     return 0; // ok
00505 
00506   Interface* device = this->LookupDevice( addr );
00507 
00508   if( device )
00509     {
00510           device->Subscribe();
00511       device->Subscribe(queue);
00512       return Driver::Subscribe(addr);
00513     }
00514 
00515   puts( "failed to find a device" );
00516   return 1; // error
00517 }
00518 
00519 
00520 // unsubscribe to a device
00521 int StgDriver::Unsubscribe(QueuePointer &queue,player_devaddr_t addr)
00522 {
00523   if( addr.interf == PLAYER_SIMULATION_CODE )
00524     return 0; // ok
00525 
00526   Interface* device = this->LookupDevice( addr );
00527 
00528   if( device )
00529     {
00530           device->Unsubscribe();
00531       device->Unsubscribe(queue);
00532       return Driver::Unsubscribe(addr);
00533     }
00534   else
00535     return 1; // error
00536 }
00537 
00538 StgDriver::~StgDriver()
00539 {
00540         delete world;
00541   puts( "Stage driver destroyed" );
00542 }
00543 
00544 
00546 // Shutdown the device
00547 int StgDriver::Shutdown()
00548 {
00549   //puts("Shutting stage driver down");
00550 
00551         FOR_EACH( it, this->devices )
00552                 (*it)->Unsubscribe();
00553 
00554   puts("Stage driver has been shutdown");
00555 
00556   return(0);
00557 }
00558 
00559 
00560 // All incoming messages get processed here.  This method is called by
00561 // Driver::ProcessMessages() once for each message.
00562 // Driver::ProcessMessages() is called by StgDriver::Update(), which is
00563 // called periodically by player.
00564 int
00565 StgDriver::ProcessMessage(QueuePointer &resp_queue,
00566                           player_msghdr * hdr,
00567                           void * data)
00568 {
00569   // find the right interface to handle this config
00570   Interface* in = this->LookupDevice( hdr->addr );
00571   if( in )
00572   {
00573     return(in->ProcessMessage( resp_queue, hdr, data));
00574   }
00575   else
00576   {
00577     PRINT_WARN3( "can't find interface for device %d.%d.%d",
00578                  this->device_addr.robot,
00579                  this->device_addr.interf,
00580                  this->device_addr.index );
00581     return(-1);
00582   }
00583 }
00584 
00585 void StgDriver::Update(void)
00586 {
00587   Driver::ProcessMessages();
00588 
00589         FOR_EACH( it, this->devices )
00590                 {               
00591                         Interface* interface = *it;
00592                         
00593                         assert( interface );
00594                         
00595                         switch( interface->addr.interf )
00596                                 {
00597                                 case PLAYER_SIMULATION_CODE:
00598                                         // one round of FLTK's update loop.
00599                                         if (StgDriver::usegui)
00600                                                 Fl::wait();
00601                                         else
00602                                                 StgDriver::world->Update();
00603                                         break;
00604                                         
00605                                 default:
00606                                         {
00607                                                 // Has enough time elapsed since the last time we published on this
00608                                                 // interface?  This really needs some thought, as each model/interface
00609                                                 // should have a configurable publishing rate. For now, I'm using the
00610                                                 // world's update rate (which appears to be stored as msec).  - BPG
00611                                                 double currtime;
00612                                                 GlobalTime->GetTimeDouble(&currtime);
00613                                                 if((currtime - interface->last_publish_time) >=
00614                                                          (interface->publish_interval_msec / 1e3))
00615                                                         {
00616                                                                 interface->Publish();
00617                                                                 interface->last_publish_time = currtime;
00618                                                         }
00619                                         }
00620                                 }
00621                 }
00622 }
00623 
00624 


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