00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00141
00142
00143
00144
00145
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
00163 extern PlayerTime* GlobalTime;
00164
00165
00166 extern bool player_quiet_startup;
00167 extern bool player_quit;
00168
00169
00170 World* StgDriver::world = NULL;
00171 bool StgDriver::usegui = true;
00172
00173
00174
00175
00176 extern "C"
00177 {
00178 int player_driver_init(DriverTable* table);
00179 }
00180
00181
00182
00183
00184
00185 Driver* StgDriver_Init(ConfigFile* cf, int section)
00186 {
00187
00188 return ((Driver*) (new StgDriver(cf, section)));
00189 }
00190
00191
00192
00193
00194
00195 void StgDriver_Register(DriverTable* table)
00196 {
00197 printf( "\n ** %s plugin v%s **", PROJECT, VERSION );
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
00220
00221 this->last_publish_time = 0;
00222 this->addr = addr;
00223 this->driver = driver;
00224 this->publish_interval_msec = 100;
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;
00244 }
00245
00246
00247 this->mod = driver->LocateModel( model_name,
00248 &addr,
00249 type );
00250
00251
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
00287
00288
00289
00290
00291 StgDriver::StgDriver(ConfigFile* cf, int section)
00292 : Driver(cf, section, false, 4096 ),
00293 devices()
00294 {
00295
00296
00297 int device_count = cf->GetTupleCount( section, "provides" );
00298
00299
00300
00301
00302
00303
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
00336
00337
00338
00339 case PLAYER_BLOBFINDER_CODE:
00340 ifsrc = new InterfaceBlobfinder( player_addr, this, cf, section );
00341 break;
00342
00343
00344
00345
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
00368
00369
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
00382
00383
00384
00385
00386
00387
00388
00389 case PLAYER_GRIPPER_CODE:
00390 ifsrc = new InterfaceGripper( player_addr, this, cf, section );
00391 break;
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
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
00420 if( this->AddInterface( ifsrc->addr))
00421 {
00422 DRIVER_ERROR( "AddInterface() failed" );
00423 this->SetError(-2);
00424 return;
00425 }
00426
00427
00428
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
00443 }
00444
00445 Model* StgDriver::LocateModel( char* basename,
00446 player_devaddr_t* addr,
00447 const std::string& type )
00448 {
00449
00450
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 == "" )
00462 return base_model;
00463
00464
00465
00466
00467
00468
00469
00470 return( base_model->GetUnusedModelOfType( type ) );
00471 }
00472
00474
00475 int StgDriver::Setup()
00476 {
00477 puts("stage driver setup");
00478 world->Start();
00479 return(0);
00480 }
00481
00482
00483
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;
00494 }
00495
00496 return NULL;
00497 }
00498
00499
00500
00501 int StgDriver::Subscribe(QueuePointer &queue,player_devaddr_t addr)
00502 {
00503 if( addr.interf == PLAYER_SIMULATION_CODE )
00504 return 0;
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;
00517 }
00518
00519
00520
00521 int StgDriver::Unsubscribe(QueuePointer &queue,player_devaddr_t addr)
00522 {
00523 if( addr.interf == PLAYER_SIMULATION_CODE )
00524 return 0;
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;
00536 }
00537
00538 StgDriver::~StgDriver()
00539 {
00540 delete world;
00541 puts( "Stage driver destroyed" );
00542 }
00543
00544
00546
00547 int StgDriver::Shutdown()
00548 {
00549
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
00561
00562
00563
00564 int
00565 StgDriver::ProcessMessage(QueuePointer &resp_queue,
00566 player_msghdr * hdr,
00567 void * data)
00568 {
00569
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
00599 if (StgDriver::usegui)
00600 Fl::wait();
00601 else
00602 StgDriver::world->Update();
00603 break;
00604
00605 default:
00606 {
00607
00608
00609
00610
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