Revision: 7258 http://playerstage.svn.sourceforge.net/playerstage/?rev=7258&view=rev Author: thjc Date: 2009-01-09 21:03:45 +0000 (Fri, 09 Jan 2009)
Log Message: ----------- make libstageplugin build with namespace changes Modified Paths: -------------- code/stage/trunk/libstageplugin/p_blobfinder.cc code/stage/trunk/libstageplugin/p_driver.cc code/stage/trunk/libstageplugin/p_driver.h code/stage/trunk/libstageplugin/p_fiducial.cc code/stage/trunk/libstageplugin/p_laser.cc code/stage/trunk/libstageplugin/p_position.cc code/stage/trunk/libstageplugin/p_simulation.cc code/stage/trunk/libstageplugin/p_sonar.cc code/stage/trunk/libstageplugin/stg_time.cc Modified: code/stage/trunk/libstageplugin/p_blobfinder.cc =================================================================== --- code/stage/trunk/libstageplugin/p_blobfinder.cc 2009-01-08 18:38:04 UTC (rev 7257) +++ code/stage/trunk/libstageplugin/p_blobfinder.cc 2009-01-09 21:03:45 UTC (rev 7258) @@ -1,8 +1,8 @@ /* * Player - One Hell of a Robot Server * Copyright (C) 2004, 2005 Richard Vaughan - * - * + * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -28,7 +28,7 @@ // DOCUMENTATION -/** @addtogroup player +/** @addtogroup player @par Blobfinder interface - PLAYER_BLOBFINDER_DATA_BLOBS */ @@ -37,7 +37,7 @@ #include "p_driver.h" -InterfaceBlobfinder::InterfaceBlobfinder( player_devaddr_t addr, +InterfaceBlobfinder::InterfaceBlobfinder( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ) @@ -52,23 +52,23 @@ player_blobfinder_data_t bfd; bzero( &bfd, sizeof(bfd) ); - StgModelBlobfinder* blobmod = (StgModelBlobfinder*)this->mod; - + ModelBlobfinder* blobmod = (ModelBlobfinder*)this->mod; + uint32_t bcount = 0; stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &bcount ); - + if ( bcount > 0 ) { // and set the image width * height bfd.width = blobmod->scan_width; bfd.height = blobmod->scan_height; bfd.blobs_count = bcount; - + bfd.blobs = new player_blobfinder_blob_t[ bcount ]; - + // now run through the blobs, packing them into the player buffer // counting the number of blobs in each channel and making entries - // in the acts header + // in the acts header unsigned int b; for( b=0; b<bcount; b++ ) { @@ -82,29 +82,29 @@ << " color: " << hex << blobs[b].color << dec << endl; */ - + int dx = blobs[b].right - blobs[b].left; int dy = blobs[b].top - blobs[b].bottom; bfd.blobs[b].x = blobs[b].left + dx/2; - bfd.blobs[b].y = blobs[b].bottom + dy/2; + bfd.blobs[b].y = blobs[b].bottom + dy/2; bfd.blobs[b].left = blobs[b].left; bfd.blobs[b].right = blobs[b].right; bfd.blobs[b].top = blobs[b].top; bfd.blobs[b].bottom = blobs[b].bottom; - + bfd.blobs[b].color = blobs[b].color; - bfd.blobs[b].area = dx * dy; - - bfd.blobs[b].range = blobs[b].range; + bfd.blobs[b].area = dx * dy; + + bfd.blobs[b].range = blobs[b].range; } } - + // should change player interface to support variable-lenght blob data - // size_t size = sizeof(bfd) - sizeof(bfd.blobs) + bcount * sizeof(bfd.blobs[0]); - - this->driver->Publish( this->addr, + // size_t size = sizeof(bfd) - sizeof(bfd.blobs) + bcount * sizeof(bfd.blobs[0]); + + this->driver->Publish( this->addr, PLAYER_MSGTYPE_DATA, PLAYER_BLOBFINDER_DATA_BLOBS, &bfd, sizeof(bfd), NULL); @@ -117,7 +117,7 @@ void* data ) { // todo: handle configuration requests - + //else { // Don't know how to handle this message. Modified: code/stage/trunk/libstageplugin/p_driver.cc =================================================================== --- code/stage/trunk/libstageplugin/p_driver.cc 2009-01-08 18:38:04 UTC (rev 7257) +++ code/stage/trunk/libstageplugin/p_driver.cc 2009-01-09 21:03:45 UTC (rev 7258) @@ -1,7 +1,7 @@ /* * Stage plugin driver for Player * Copyright (C) 2004-2005 Richard Vaughan - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -51,7 +51,7 @@ Richard Vaughan [...@ref refs] - + @par Configuration file examples: Creating two models in a Stage worldfile, saved as "example.world": @@ -67,7 +67,7 @@ pose [ 1 1 0 ] # add a laser scanner on top of the robot - laser() + laser() ) # create a position model with a gripper attached to each side @@ -89,15 +89,15 @@ @verbatim # Present a simulation interface on the default port (6665). # Load the Stage plugin driver and create the world described -# in the worldfile "example.world" -# The simulation interface MUST be created before any simulated devices +# in the worldfile "example.world" +# The simulation interface MUST be created before any simulated devices # models can be used. driver -( +( name "stage" provides ["simulation:0"] plugin "libstageplugin" - worldfile "example.world" + worldfile "example.world" ) # Present a position interface on the default port (6665), connected @@ -110,7 +110,7 @@ model "marvin" ) -# Present three interfaces on port 6666, connected to the Stage +# Present three interfaces on port 6666, connected to the Stage # position model "gort" and its two grippers. driver ( @@ -151,7 +151,7 @@ #include "p_driver.h" -const char* copyright_notice = +const char* copyright_notice = "\n * Part of the Player Project [http://playerstage.sourceforge.net]\n" " * Copyright 2000-2007 Richard Vaughan, Brian Gerkey and contributors.\n" " * Released under the GNU General Public License v2.\n" @@ -168,7 +168,7 @@ extern bool player_quit; // init static vars -StgWorldGui* StgDriver::world = NULL; +WorldGui* StgDriver::world = NULL; //int update_request = 0; @@ -187,7 +187,7 @@ // Create and return a new instance of this driver return ((Driver*) (new StgDriver(cf, section))); } - + // A driver registration function, again declared outside of the class so // that it can be invoked without object context. In this function, we add // the driver into the given driver table, indicating which interface the @@ -195,15 +195,15 @@ void StgDriver_Register(DriverTable* table) { printf( "\n ** Stage plugin v%s **", "3.0dev" ); // XX TODO - + if( !player_quiet_startup ) { puts( copyright_notice ); } - + table->AddDriver( (char*)"stage", StgDriver_Init); } - + int player_driver_init(DriverTable* table) { puts(" Stage driver plugin init"); @@ -211,9 +211,9 @@ return(0); } -Interface::Interface( player_devaddr_t addr, +Interface::Interface( player_devaddr_t addr, StgDriver* driver, - ConfigFile* cf, + ConfigFile* cf, int section ) { //puts( "Interface constructor" ); @@ -222,17 +222,17 @@ this->addr = addr; this->driver = driver; this->publish_interval_msec = 100; // todo - do this properly -} +} -InterfaceModel::InterfaceModel( player_devaddr_t addr, +InterfaceModel::InterfaceModel( player_devaddr_t addr, StgDriver* driver, - ConfigFile* cf, + ConfigFile* cf, int section, - stg_model_type_t type ) + stg_model_type_t type ) : Interface( addr, driver, cf, section ) { char* model_name = (char*)cf->ReadString(section, "model", NULL ); - + if( model_name == NULL ) { PRINT_ERR1( "device \"%s\" uses the Stage driver but has " @@ -244,22 +244,22 @@ } // find a model of the right type - this->mod = driver->LocateModel( model_name, - &addr, + this->mod = driver->LocateModel( model_name, + &addr, type ); - + if( !this->mod ) { printf( " ERROR! no model available for this device." " Check your world and config files.\n" ); - + exit(-1); return; } - + if( !player_quiet_startup ) printf( "\"%s\"\n", this->mod->Token() ); -} +} // Constructor. Retrieve options from the configuration file and do any @@ -269,103 +269,103 @@ StgDriver::StgDriver(ConfigFile* cf, int section) : Driver(cf, section, false, 4096 ) -{ +{ // init the array of device ids this->devices = g_ptr_array_new(); - + int device_count = cf->GetTupleCount( section, "provides" ); - + //if( !player_quiet_startup ) - // { - //printf( " Stage driver creating %d %s\n", - // device_count, + // { + //printf( " Stage driver creating %d %s\n", + // device_count, // device_count == 1 ? "device" : "devices" ); // } - + for( int d=0; d<device_count; d++ ) { player_devaddr_t player_addr; - - if (cf->ReadDeviceAddr( &player_addr, section, + + if (cf->ReadDeviceAddr( &player_addr, section, "provides", 0, d, NULL) != 0) { this->SetError(-1); return; - } - + } + if( !player_quiet_startup ) { - printf( " Stage plugin: %d.%s.%d is ", - player_addr.robot, - interf_to_str(player_addr.interf), + printf( " Stage plugin: %d.%s.%d is ", + player_addr.robot, + interf_to_str(player_addr.interf), player_addr.index ); fflush(stdout); } - - + + Interface *ifsrc = NULL; - + switch( player_addr.interf ) { case PLAYER_BLOBFINDER_CODE: ifsrc = new InterfaceBlobfinder( player_addr, this, cf, section ); break; - + case PLAYER_FIDUCIAL_CODE: ifsrc = new InterfaceFiducial( player_addr, this, cf, section ); break; - - case PLAYER_LASER_CODE: + + case PLAYER_LASER_CODE: ifsrc = new InterfaceLaser( player_addr, this, cf, section ); break; - - case PLAYER_POSITION2D_CODE: + + case PLAYER_POSITION2D_CODE: ifsrc = new InterfacePosition( player_addr, this, cf, section ); break; - + case PLAYER_SIMULATION_CODE: ifsrc = new InterfaceSimulation( player_addr, this, cf, section ); - break; - + break; + case PLAYER_SONAR_CODE: ifsrc = new InterfaceSonar( player_addr, this, cf, section ); break; - + case PLAYER_SPEECH_CODE: ifsrc = new InterfaceSpeech( player_addr, this, cf, section ); break; - - // case PLAYER_CAMERA_CODE: + + // case PLAYER_CAMERA_CODE: // ifsrc = new InterfaceCamera( player_addr, this, cf, section ); // break; // case PLAYER_GRAPHICS2D_CODE: // ifsrc = new InterfaceGraphics2d( player_addr, this, cf, section ); - // break; - + // break; + // case PLAYER_GRAPHICS3D_CODE: // ifsrc = new InterfaceGraphics3d( player_addr, this, cf, section ); - // break; - - - + // break; + + + // case PLAYER_LOCALIZE_CODE: // ifsrc = new InterfaceLocalize( player_addr, this, cf, section ); -// break; - +// break; + // case PLAYER_MAP_CODE: // ifsrc = new InterfaceMap( player_addr, this, cf, section ); -// break; - +// break; + // case PLAYER_GRIPPER_CODE: // ifsrc = new InterfaceGripper( player_addr, this, cf, section ); -// break; +// break; // case PLAYER_WIFI_CODE: // ifsrc = new InterfaceWifi( player_addr, this, cf, section ); // break; -// case PLAYER_POWER_CODE: +// case PLAYER_POWER_CODE: // ifsrc = new InterfacePower( player_addr, this, cf, section ); // break; @@ -375,16 +375,16 @@ // case PLAYER_BUMPER_CODE: // ifsrc = new InterfaceBumper( player_addr, this, cf, section ); -// break; +// break; default: PRINT_ERR1( "error: stage driver doesn't support interface type %d\n", player_addr.interf ); this->SetError(-1); - return; + return; } - + if( ifsrc ) { // attempt to add this interface and we're done @@ -394,45 +394,45 @@ this->SetError(-2); return; } - + // store the Interaface in our device list g_ptr_array_add( this->devices, ifsrc ); } else { PRINT_ERR3( "No Stage source found for interface %d:%d:%d", - player_addr.robot, - player_addr.interf, + player_addr.robot, + player_addr.interf, player_addr.index ); - + this->SetError(-3); return; - } + } } //puts( " Stage driver loaded successfully." ); } -StgModel* StgDriver::LocateModel( char* basename, +Model* StgDriver::LocateModel( char* basename, player_devaddr_t* addr, stg_model_type_t type ) -{ - printf( "attempting to find a model under model \"%s\" of type [%d]\n", +{ + printf( "attempting to find a model under model \"%s\" of type [%d]\n", basename, type ); - - StgModel* base_model = world->GetModel( basename ); - + + Model* base_model = world->GetModel( basename ); + if( base_model == NULL ) { - PRINT_ERR1( " Error! can't find a Stage model named \"%s\"", + PRINT_ERR1( " Error! can't find a Stage model named \"%s\"", basename ); return NULL; } - + if( type == MODEL_TYPE_PLAIN ) // if we don't care what type the model is return base_model; - + // printf( "found base model %s\n", base_model->Token() ); - + // we find the first model in the tree that is the right // type (i.e. has the right initialization function) and has not // been used before @@ -443,8 +443,8 @@ //////////////////////////////////////////////////////////////////////////////// // Set up the device. Return 0 if things go well, and -1 otherwise. int StgDriver::Setup() -{ - puts("stage driver setup"); +{ + puts("stage driver setup"); world->Start(); return(0); } @@ -455,9 +455,9 @@ { for( int i=0; i<(int)this->devices->len; i++ ) { - Interface* candidate = + Interface* candidate = (Interface*)g_ptr_array_index( this->devices, i ); - + if( candidate->addr.robot == addr.robot && candidate->addr.interf == addr.interf && candidate->addr.index == addr.index ) @@ -475,9 +475,9 @@ return 0; // ok Interface* device = this->LookupDevice( addr ); - + if( device ) - { + { device->Subscribe(); return Driver::Subscribe(addr); } @@ -494,10 +494,10 @@ return 0; // ok Interface* device = this->LookupDevice( addr ); - + if( device ) { - device->Unsubscribe(); + device->Unsubscribe(); return Driver::Unsubscribe(addr); } else @@ -540,9 +540,9 @@ // Driver::ProcessMessages() once for each message. // Driver::ProcessMessages() is called by StgDriver::Update(), which is // called periodically by player. -int -StgDriver::ProcessMessage(QueuePointer &resp_queue, - player_msghdr * hdr, +int +StgDriver::ProcessMessage(QueuePointer &resp_queue, + player_msghdr * hdr, void * data) { // find the right interface to handle this config @@ -554,8 +554,8 @@ else { PRINT_WARN3( "can't find interface for device %d.%d.%d", - this->device_addr.robot, - this->device_addr.interf, + this->device_addr.robot, + this->device_addr.interf, this->device_addr.index ); return(-1); } @@ -570,15 +570,15 @@ for( int i=0; i<(int)this->devices->len; i++ ) { Interface* interface = (Interface*)g_ptr_array_index( this->devices, i ); - + assert( interface ); - + switch( interface->addr.interf ) { case PLAYER_SIMULATION_CODE: - world->Update(); + world->Update(); break; - + default: { // Has enough time elapsed since the last time we published on this @@ -587,7 +587,7 @@ // world's update rate (which appears to be stored as msec). - BPG double currtime; GlobalTime->GetTimeDouble(&currtime); - if((currtime - interface->last_publish_time) >= + if((currtime - interface->last_publish_time) >= (interface->publish_interval_msec / 1e3)) { interface->Publish(); Modified: code/stage/trunk/libstageplugin/p_driver.h =================================================================== --- code/stage/trunk/libstageplugin/p_driver.h 2009-01-08 18:38:04 UTC (rev 7257) +++ code/stage/trunk/libstageplugin/p_driver.h 2009-01-09 21:03:45 UTC (rev 7258) @@ -9,6 +9,7 @@ #include "../libstage/stage_internal.hh" + #define DRIVER_ERROR(X) printf( "Stage driver error: %s\n", X ) // foward declare; @@ -20,46 +21,46 @@ public: // Constructor; need that StgDriver(ConfigFile* cf, int section); - + // Destructor ~StgDriver(void); - + // Must implement the following methods. virtual int Setup(); virtual int Shutdown(); - virtual int ProcessMessage(QueuePointer &resp_queue, - player_msghdr * hdr, + virtual int ProcessMessage(QueuePointer &resp_queue, + player_msghdr * hdr, void * data); virtual int Subscribe(player_devaddr_t addr); virtual int Unsubscribe(player_devaddr_t addr); - + /// The server thread calls this method frequently. We use it to /// check for new commands and configs virtual void Update(); - + /// all player devices share the same Stage world (for now) - static StgWorldGui* world; - + static WorldGui* world; + /// find the device record with this Player id Interface* LookupDevice( player_devaddr_t addr ); - - StgModel* LocateModel( char* basename, + + Model* LocateModel( char* basename, player_devaddr_t* addr, stg_model_type_t type ); - - protected: - + + protected: + /// an array of pointers to Interface objects, defined below - GPtrArray* devices; + GPtrArray* devices; }; class Interface { public: - Interface(player_devaddr_t addr, + Interface(player_devaddr_t addr, StgDriver* driver, - ConfigFile* cf, + ConfigFile* cf, int section ); virtual ~Interface( void ){ /* TODO: clean up*/ }; @@ -67,9 +68,9 @@ player_devaddr_t addr; double last_publish_time; double publish_interval_msec; - + StgDriver* driver; // the driver instance that created this device - + virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr_t* hdr, void* data) { return(-1); } // empty implementation @@ -82,7 +83,7 @@ class InterfaceSimulation : public Interface { - public: + public: InterfaceSimulation( player_devaddr_t addr, StgDriver* driver,ConfigFile* cf, int section ); virtual ~InterfaceSimulation( void ){ /* TODO: clean up*/ }; virtual int ProcessMessage(QueuePointer & resp_queue, @@ -102,8 +103,8 @@ int section, stg_model_type_t type ); - StgModel* mod; - + Model* mod; + virtual ~InterfaceModel( void ){ /* TODO: clean up*/ }; virtual void Subscribe( void ){ this->mod->Subscribe(); }; @@ -113,7 +114,7 @@ class InterfacePosition : public InterfaceModel { - public: + public: InterfacePosition( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfacePosition( void ){ /* TODO: clean up*/ }; virtual void Publish( void ); @@ -121,10 +122,10 @@ player_msghdr_t* hdr, void* data); }; - + class InterfaceGripper : public InterfaceModel { - public: + public: InterfaceGripper( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfaceGripper( void ){ /* TODO: clean up*/ }; virtual int ProcessMessage(QueuePointer & resp_queue, @@ -135,7 +136,7 @@ class InterfaceWifi : public InterfaceModel { - public: + public: InterfaceWifi( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfaceWifi( void ){ /* TODO: clean up*/ }; virtual int ProcessMessage(QueuePointer & resp_queue, @@ -146,7 +147,7 @@ class InterfaceSpeech : public InterfaceModel { - public: + public: InterfaceSpeech( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfaceSpeech( void ){ /* TODO: clean up*/ }; virtual int ProcessMessage(QueuePointer & resp_queue, @@ -159,7 +160,7 @@ { private: int scan_id; - public: + public: InterfaceLaser( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfaceLaser( void ){ /* TODO: clean up*/ }; virtual int ProcessMessage(QueuePointer & resp_queue, @@ -170,20 +171,20 @@ class InterfacePower : public InterfaceModel { - public: + public: InterfacePower( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfacePower( void ){ /* TODO: clean up*/ }; - - virtual int ProcessMessage( QueuePointer & resp_queue, - player_msghdr * hdr, + + virtual int ProcessMessage( QueuePointer & resp_queue, + player_msghdr * hdr, void * data ); - + virtual void Publish( void ); }; class InterfaceFiducial : public InterfaceModel { - public: + public: InterfaceFiducial( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfaceFiducial( void ){ /* TODO: clean up*/ }; @@ -193,62 +194,62 @@ void* data); }; - + class InterfaceBlobfinder : public InterfaceModel { - public: + public: InterfaceBlobfinder( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfaceBlobfinder( void ){ /* TODO: clean up*/ }; - - virtual int ProcessMessage( QueuePointer & resp_queue, - player_msghdr * hdr, + + virtual int ProcessMessage( QueuePointer & resp_queue, + player_msghdr * hdr, void * data ); virtual void Publish( void ); }; class InterfacePtz : public InterfaceModel { - public: + public: InterfacePtz( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfacePtz( void ){ /* TODO: clean up*/ }; - - virtual int ProcessMessage( QueuePointer & resp_queue, - player_msghdr * hdr, + + virtual int ProcessMessage( QueuePointer & resp_queue, + player_msghdr * hdr, void * data ); virtual void Publish( void ); }; class InterfaceSonar : public InterfaceModel { - public: + public: InterfaceSonar( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfaceSonar( void ){ /* TODO: clean up*/ }; - - virtual int ProcessMessage( QueuePointer & resp_queue, - player_msghdr * hdr, + + virtual int ProcessMessage( QueuePointer & resp_queue, + player_msghdr * hdr, void * data ); virtual void Publish( void ); }; - + class InterfaceBumper : public InterfaceModel { - public: + public: InterfaceBumper( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfaceBumper( void ){ /* TODO: clean up*/ }; - - virtual int ProcessMessage( QueuePointer & resp_queue, - player_msghdr * hdr, + + virtual int ProcessMessage( QueuePointer & resp_queue, + player_msghdr * hdr, void * data ); virtual void Publish( void ); }; - + class InterfaceLocalize : public InterfaceModel { - public: - InterfaceLocalize( player_devaddr_t addr, - StgDriver* driver, - ConfigFile* cf, + public: + InterfaceLocalize( player_devaddr_t addr, + StgDriver* driver, + ConfigFile* cf, int section ); virtual ~InterfaceLocalize( void ){ /* TODO: clean up*/ }; @@ -261,22 +262,22 @@ class InterfaceMap : public InterfaceModel { - public: + public: InterfaceMap( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfaceMap( void ){ /* TODO: clean up*/ }; - - virtual int ProcessMessage( QueuePointer & resp_queue, - player_msghdr * hdr, + + virtual int ProcessMessage( QueuePointer & resp_queue, + player_msghdr * hdr, void * data ); //virtual void Publish( void ); - + // called by ProcessMessage to handle individual messages - int HandleMsgReqInfo( QueuePointer & resp_queue, - player_msghdr * hdr, + int HandleMsgReqInfo( QueuePointer & resp_queue, + player_msghdr * hdr, void * data ); - int HandleMsgReqData( QueuePointer & resp_queue, - player_msghdr * hdr, + int HandleMsgReqData( QueuePointer & resp_queue, + player_msghdr * hdr, void * data ); }; @@ -285,7 +286,7 @@ public: InterfaceGraphics3d( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); virtual ~InterfaceGraphics3d( void ); - + virtual int ProcessMessage( QueuePointer & resp_queue, player_msghdr * hdr, void * data ); @@ -298,24 +299,24 @@ GList* commands; // list of drawing commands received since the last CLEAR bool rebuild_displaylist; int displaylist; - + // clear the display void Clear( void ); }; /** Replaces Player's real time clock object */ class StTime : public PlayerTime -{ - private: +{ + private: StgDriver* driver; - - public: + + public: // Constructor StTime( StgDriver* driver ); - + // Destructor virtual ~StTime(); - + // Get the simulator time int GetTime(struct timeval* time); int GetTimeDouble(double* time); Modified: code/stage/trunk/libstageplugin/p_fiducial.cc =================================================================== --- code/stage/trunk/libstageplugin/p_fiducial.cc 2009-01-08 18:38:04 UTC (rev 7257) +++ code/stage/trunk/libstageplugin/p_fiducial.cc 2009-01-09 21:03:45 UTC (rev 7258) @@ -1,8 +1,8 @@ /* * Player - One Hell of a Robot Server * Copyright (C) 2004, 2005 Richard Vaughan - * - * + * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -28,7 +28,7 @@ // DOCUMENTATION -/** +/** @addtogroup player @par Fiducial interface - PLAYER_FIDUCIAL_DATA_SCAN @@ -47,21 +47,21 @@ #include "p_driver.h" -InterfaceFiducial::InterfaceFiducial( player_devaddr_t addr, - StgDriver* driver, - ConfigFile* cf, +InterfaceFiducial::InterfaceFiducial( player_devaddr_t addr, + StgDriver* driver, + ConfigFile* cf, int section ) : InterfaceModel( addr, driver, cf, section, MODEL_TYPE_FIDUCIAL ) { } void InterfaceFiducial::Publish( void ) -{ +{ player_fiducial_data_t pdata; memset( &pdata, 0, sizeof(pdata) ); - - StgModelFiducial* fidmod = (StgModelFiducial*)this->mod; + ModelFiducial* fidmod = (ModelFiducial*)this->mod; + if( fidmod->fiducial_count > 0 ) { stg_fiducial_t* fids = fidmod->fiducials; @@ -69,15 +69,15 @@ pdata.fiducials_count = fidmod->fiducial_count; pdata.fiducials = new player_fiducial_item_t[pdata.fiducials_count]; - + for( int i=0; i<(int)pdata.fiducials_count; i++ ) - { + { pdata.fiducials[i].id = fids[i].id; - - // 2D x,y only + + // 2D x,y only double xpos = fids[i].range * cos(fids[i].bearing); double ypos = fids[i].range * sin(fids[i].bearing); - + pdata.fiducials[i].pose.px = xpos; pdata.fiducials[i].pose.py = ypos; pdata.fiducials[i].pose.pz = 0.0; @@ -86,7 +86,7 @@ pdata.fiducials[i].pose.pyaw = fids[i].geom.a; } } - + // publish this data this->driver->Publish( this->addr, PLAYER_MSGTYPE_DATA, @@ -102,82 +102,84 @@ void* data ) { //printf("got fiducial request\n"); - + // Is it a request to get the geometry? - if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_FIDUCIAL_REQ_GET_GEOM, + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_FIDUCIAL_REQ_GET_GEOM, this->addr)) { Geom geom = mod->GetGeom(); Pose pose = mod->GetPose(); - + // fill in the geometry data formatted player-like player_laser_geom_t pgeom; pgeom.pose.px = pose.x; pgeom.pose.py = pose.y; - pgeom.pose.pz = pose.z; - pgeom.pose.proll = 0.0; - pgeom.pose.ppitch = 0.0; - pgeom.pose.pyaw = pose.a; + pgeom.pose.pz = pose.z; + pgeom.pose.proll = 0.0; + pgeom.pose.ppitch = 0.0; + pgeom.pose.pyaw = pose.a; pgeom.size.sl = geom.size.x; pgeom.size.sw = geom.size.y; - + this->driver->Publish(this->addr, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, + PLAYER_MSGTYPE_RESP_ACK, PLAYER_FIDUCIAL_REQ_GET_GEOM, (void*)&pgeom, sizeof(pgeom), NULL); return(0); } - else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_FIDUCIAL_REQ_SET_ID, + else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_FIDUCIAL_REQ_SET_ID, this->addr)) - { + { if( hdr->size == sizeof(player_fiducial_id_t) ) { - // copy the new ID + // copy the new ID player_fiducial_id_t* incoming = (player_fiducial_id_t*)data; - + // Stage uses a simple int for IDs. int id = incoming->id; - + mod->SetFiducialReturn( id ); - + player_fiducial_id_t pid; pid.id = id; - + // acknowledge, including the new ID this->driver->Publish(this->addr, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, + PLAYER_MSGTYPE_RESP_ACK, PLAYER_FIDUCIAL_REQ_SET_ID, (void*)&pid, sizeof(pid) ); } else { PRINT_ERR2("Incorrect packet size setting fiducial ID (%d/%d)", - (int)hdr->size, (int)sizeof(player_fiducial_id_t) ); + (int)hdr->size, (int)sizeof(player_fiducial_id_t) ); return(-1); // error - NACK is sent automatically } - } - else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_FIDUCIAL_REQ_GET_ID, + } + else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_FIDUCIAL_REQ_GET_ID, this->addr)) { // fill in the data formatted player-like +#warning disabled this bit player_fiducial_id_t pid; - pid.id = mod->FiducialReturn(); - +// pid.id = mod->FiducialReturn(); + pid.id = 0; + // acknowledge, including the new ID this->driver->Publish(this->addr, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, + PLAYER_MSGTYPE_RESP_ACK, PLAYER_FIDUCIAL_REQ_GET_ID, - (void*)&pid, sizeof(pid) ); - } + (void*)&pid, sizeof(pid) ); + } /* case PLAYER_FIDUCIAL_SET_FOV: - + if( len == sizeof(player_fiducial_fov_t) ) { player_fiducial_fov_t* pfov = (player_fiducial_fov_t*)src; - + // convert from player to stage FOV packets stg_fiducial_config_t setcfg; memset( &setcfg, 0, sizeof(setcfg) ); @@ -185,20 +187,20 @@ setcfg.max_range_id = (uint16_t)ntohs(pfov->max_range) / 1000.0; setcfg.max_range_anon = setcfg.max_range_id; setcfg.fov = DTOR((uint16_t)ntohs(pfov->view_angle)); - + //printf( "setting fiducial FOV to min %f max %f fov %f\n", // setcfg.min_range, setcfg.max_range_anon, setcfg.fov ); - + //stg_model_set_config( this->mod, &setcfg, sizeof(setcfg)); - stg_model_set_property( this->mod, "fiducial_cfg", - &setcfg, sizeof(setcfg)); - } + stg_model_set_property( this->mod, "fiducial_cfg", + &setcfg, sizeof(setcfg)); + } else PRINT_ERR2("Incorrect packet size setting fiducial FOV (%d/%d)", - (int)len, (int)sizeof(player_fiducial_fov_t) ); - + (int)len, (int)sizeof(player_fiducial_fov_t) ); + // deliberate no-break - SET_FOV needs the current FOV as a reply - + case PLAYER_FIDUCIAL_GET_FOV: { stg_fiducial_config_t *cfg = (stg_fiducial_config_t*) @@ -210,23 +212,23 @@ pfov.min_range = htons((uint16_t)(1000.0 * cfg->min_range)); pfov.max_range = htons((uint16_t)(1000.0 * cfg->max_range_anon)); pfov.view_angle = htons((uint16_t)RTOD(cfg->fov)); - - if( this->driver->PutReply( this->id, client, PLAYER_MSGTYPE_RESP_ACK, + + if( this->driver->PutReply( this->id, client, PLAYER_MSGTYPE_RESP_ACK, &pfov, sizeof(pfov), NULL ) != 0 ) DRIVER_ERROR("PutReply() failed for " - "PLAYER_FIDUCIAL_GET_FOV or PLAYER_FIDUCIAL_SET_FOV"); + "PLAYER_FIDUCIAL_GET_FOV or PLAYER_FIDUCIAL_SET_FOV"); } break; */ - + else { // Don't know how to handle this message. PRINT_WARN2( "stg_fiducial doesn't support msg with type/subtype %d/%d", hdr->type, hdr->subtype); return(-1); - } - + } + return 0; } Modified: code/stage/trunk/libstageplugin/p_laser.cc =================================================================== --- code/stage/trunk/libstageplugin/p_laser.cc 2009-01-08 18:38:04 UTC (rev 7257) +++ code/stage/trunk/libstageplugin/p_laser.cc 2009-01-09 21:03:45 UTC (rev 7258) @@ -1,8 +1,8 @@ /* * Player - One Hell of a Robot Server * Copyright (C) 2004, 2005 Richard Vaughan - * - * + * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -28,7 +28,7 @@ // DOCUMENTATION ------------------------------------------------------------ -/** @addtogroup player +/** @addtogroup player @par Laser interface - PLAYER_LASER_DATA_SCAN - PLAYER_LASER_REQ_SET_CONFIG @@ -38,26 +38,26 @@ // CODE ---------------------------------------------------------------------- -#include "p_driver.h" +#include "p_driver.h" -InterfaceLaser::InterfaceLaser( player_devaddr_t addr, +InterfaceLaser::InterfaceLaser( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ) : InterfaceModel( addr, driver, cf, section, MODEL_TYPE_LASER ) -{ +{ this->scan_id = 0; } void InterfaceLaser::Publish( void ) { - StgModelLaser* mod = (StgModelLaser*)this->mod; + ModelLaser* mod = (ModelLaser*)this->mod; stg_laser_sample_t* samples = mod->GetSamples(); // don't publish anything until we have some real data if( samples == NULL ) return; - + player_laser_data_t pdata; memset( &pdata, 0, sizeof(pdata) ); @@ -68,17 +68,17 @@ pdata.max_range = cfg.range_bounds.max; pdata.ranges_count = pdata.intensity_count = cfg.sample_count; pdata.id = this->scan_id++; - + pdata.ranges = new float[pdata.ranges_count]; pdata.intensity = new uint8_t[pdata.ranges_count]; - + for( unsigned int i=0; i<cfg.sample_count; i++ ) { //printf( "range %d %d\n", i, samples[i].range); pdata.ranges[i] = samples[i].range; pdata.intensity[i] = (uint8_t)samples[i].reflectance; } - + // Write laser data this->driver->Publish(this->addr, PLAYER_MSGTYPE_DATA, @@ -93,11 +93,11 @@ player_msghdr_t* hdr, void* data) { - StgModelLaser* mod = (StgModelLaser*)this->mod; + ModelLaser* mod = (ModelLaser*)this->mod; // Is it a request to set the laser's config? - if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_LASER_REQ_SET_CONFIG, + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_LASER_REQ_SET_CONFIG, this->addr)) { @@ -109,33 +109,33 @@ // int intensity = plc->intensity; stg_laser_cfg_t cfg = mod->GetConfig(); - - PRINT_DEBUG3( "laser config was: resolution %d, fov %.6f, interval %d\n", + + PRINT_DEBUG3( "laser config was: resolution %d, fov %.6f, interval %d\n", cfg.resolution, cfg.fov, cfg.interval ); - + cfg.fov = plc->max_angle - plc->min_angle; cfg.resolution = (uint32_t) ( cfg.fov / ( cfg.sample_count * plc->resolution ) ); - if ( cfg.resolution < 1 ) + if ( cfg.resolution < 1 ) cfg.resolution = 1; cfg.interval = (stg_usec_t) ( 1.0E6 / plc->scanning_frequency ); - - PRINT_DEBUG3( "setting laser config: resolution %d, fov %.6f, interval %d\n", + + PRINT_DEBUG3( "setting laser config: resolution %d, fov %.6f, interval %d\n", cfg.resolution, cfg.fov, cfg.interval ); - + // Range resolution is currently locked to the world setting // and intensity values are always read. The relevant settings // are ignored. - + mod->SetConfig( cfg ); this->driver->Publish(this->addr, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, + PLAYER_MSGTYPE_RESP_ACK, PLAYER_LASER_REQ_SET_CONFIG); return(0); } else { - PRINT_ERR2("config request len is invalid (%d != %d)", + PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size, (int)sizeof(player_laser_config_t)); return(-1); @@ -145,7 +145,7 @@ else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_GET_CONFIG, this->addr)) - { + { if( hdr->size == 0 ) { stg_laser_cfg_t cfg = mod->GetConfig(); @@ -161,7 +161,7 @@ plc.scanning_frequency = 1.0E6 / cfg.interval; this->driver->Publish(this->addr, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, + PLAYER_MSGTYPE_RESP_ACK, PLAYER_LASER_REQ_GET_CONFIG, (void*)&plc, sizeof(plc), NULL); return(0); @@ -176,7 +176,7 @@ else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_GET_GEOM, this->addr)) - { + { if(hdr->size == 0) { Geom geom = this->mod->GetGeom(); @@ -192,7 +192,7 @@ pgeom.size.sw = geom.size.y; this->driver->Publish(this->addr, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, + PLAYER_MSGTYPE_RESP_ACK, PLAYER_LASER_REQ_GET_GEOM, (void*)&pgeom, sizeof(pgeom), NULL); return(0); Modified: code/stage/trunk/libstageplugin/p_position.cc =================================================================== --- code/stage/trunk/libstageplugin/p_position.cc 2009-01-08 18:38:04 UTC (rev 7257) +++ code/stage/trunk/libstageplugin/p_position.cc 2009-01-09 21:03:45 UTC (rev 7258) @@ -60,7 +60,7 @@ player_msghdr_t* hdr, void* data) { - StgModelPosition* mod = (StgModelPosition*)this->mod; + ModelPosition* mod = (ModelPosition*)this->mod; // Is it a new motor command? if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, @@ -268,7 +268,7 @@ { //puts( "publishing position data" ); - StgModelPosition* mod = (StgModelPosition*)this->mod; + ModelPosition* mod = (ModelPosition*)this->mod; //printf( "stage position data: %.2f,%.2f,%.2f\n", // data->pose.x, data->pose.y, data->pose.a ); Modified: code/stage/trunk/libstageplugin/p_simulation.cc =================================================================== --- code/stage/trunk/libstageplugin/p_simulation.cc 2009-01-08 18:38:04 UTC (rev 7257) +++ code/stage/trunk/libstageplugin/p_simulation.cc 2009-01-09 21:03:45 UTC (rev 7258) @@ -1,8 +1,8 @@ /* * Player - One Hell of a Robot Server * Copyright (C) 2004, 2005 Richard Vaughan - * - * + * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -28,7 +28,7 @@ // DOCUMENTATION ------------------------------------------------------------ -/** @addtogroup player +/** @addtogroup player @par Simulation interface - PLAYER_SIMULATION_REQ_SET_POSE2D - PLAYER_SIMULATION_REQ_GET_POSE2D @@ -62,22 +62,22 @@ //////////////////////////////////////////////////////////////////////////////////// -// +// // SIMULATION INTERFACE // -InterfaceSimulation::InterfaceSimulation( player_devaddr_t addr, +InterfaceSimulation::InterfaceSimulation( player_devaddr_t addr, StgDriver* driver, - ConfigFile* cf, + ConfigFile* cf, int section ) : Interface( addr, driver, cf, section ) { printf( "a Stage world" ); fflush(stdout); //puts( "InterfaceSimulation constructor" ); - + Stg::Init( &player_argc, &player_argv ); const char* worldfile_name = cf->ReadString(section, "worldfile", NULL ); - + if( worldfile_name == NULL ) { PRINT_ERR1( "device \"%s\" uses the Stage driver but has " @@ -87,94 +87,94 @@ worldfile_name ); return; // error } - + char fullname[MAXPATHLEN]; - + if( worldfile_name[0] == '/' ) strcpy( fullname, worldfile_name ); else { char *tmp = strdup(cf->filename); - snprintf( fullname, MAXPATHLEN, - "%s/%s", dirname(tmp), worldfile_name ); + snprintf( fullname, MAXPATHLEN, + "%s/%s", dirname(tmp), worldfile_name ); free(tmp); } - + // a little sanity testing if( !g_file_test( fullname, G_FILE_TEST_EXISTS ) ) { PRINT_ERR1( "worldfile \"%s\" does not exist", worldfile_name ); return; } - + // create a passel of Stage models in the local cache based on the // worldfile - - StgDriver::world = new StgWorldGui( 800,840, "Player/Stage" ); + + StgDriver::world = new WorldGui( 800,840, "Player/Stage" ); assert(StgDriver::world); puts(""); StgDriver::world->Load( fullname ); //printf( " done.\n" ); - + // poke the P/S name into the window title bar // if( StgDriver::world ) // { // char txt[128]; // snprintf( txt, 128, "Player/Stage: %s", StgDriver::world->token ); -// StgDriverstg_world_set_title(StgDriver::world, txt ); +// StgDriverstg_world_set_title(StgDriver::world, txt ); // } // steal the global clock - a bit aggressive, but a simple approach if( GlobalTime ) delete GlobalTime; assert( (GlobalTime = new StTime( driver ) )); - + // start the simulation // printf( " Starting world clock... " ); fflush(stdout); //stg_world_resume( world ); - + StgDriver::world->Start(); - + // this causes Driver::Update() to be called even when the device is // not subscribed - driver->alwayson = TRUE; + driver->alwayson = TRUE; puts( "" ); // end the Stage startup line -} +} int InterfaceSimulation::ProcessMessage(QueuePointer &resp_queue, player_msghdr_t* hdr, void* data) { // Is it a request to get a model's pose in 2D? - if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_SIMULATION_REQ_GET_POSE2D, + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_SIMULATION_REQ_GET_POSE2D, this->addr)) { - player_simulation_pose2d_req_t* req = + player_simulation_pose2d_req_t* req = (player_simulation_pose2d_req_t*)data; - + PRINT_DEBUG1( "Stage: received request for the 2D position of object \"%s\"\n", req->name ); - - // look up the named model - StgModel* mod = StgDriver::world->GetModel( req->name ); - + + // look up the named model + Model* mod = StgDriver::world->GetModel( req->name ); + if( mod ) { Pose pose = mod->GetPose(); - + PRINT_DEBUG3( "Stage: returning location [ %.2f, %.2f, %.2f ]\n", pose.x, pose.y, pose.a ); - + player_simulation_pose2d_req_t reply; memcpy( &reply, req, sizeof(reply)); reply.pose.px = pose.x; reply.pose.py = pose.y; reply.pose.pa = pose.a; - - this->driver->Publish( this->addr, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, + + this->driver->Publish( this->addr, resp_queue, + PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE2D, (void*)&reply, sizeof(reply), NULL ); return(0); @@ -185,28 +185,28 @@ return(-1); } } - + // Is it a request to set a model's pose in 2D? - if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_SIMULATION_REQ_SET_POSE2D, + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_SIMULATION_REQ_SET_POSE2D, this->addr)) { - player_simulation_pose2d_req_t* req = + player_simulation_pose2d_req_t* req = (player_simulation_pose2d_req_t*)data; // look up the named model - StgModel* mod = StgDriver::world->GetModel( req->name ); + Model* mod = StgDriver::world->GetModel( req->name ); if( mod ) { PRINT_DEBUG4( "Stage: moving \"%s\" to [ %.2f, %.2f, %.2f ]\n", req->name, req->pose.px, req->pose.py, req->pose.pa ); - + Pose pose = mod->GetPose(); pose.x = req->pose.px; pose.y = req->pose.py; pose.a = req->pose.pa; - + mod->SetPose( pose ); this->driver->Publish(this->addr, resp_queue, @@ -220,27 +220,27 @@ return(-1); } } - + // Is it a request to get a model's pose in 3D? - else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_SIMULATION_REQ_GET_POSE3D, + else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_SIMULATION_REQ_GET_POSE3D, this->addr)) { - player_simulation_pose3d_req_t* req = + player_simulation_pose3d_req_t* req = (player_simulation_pose3d_req_t*)data; - + PRINT_DEBUG1( "Stage: received request for the 3D position of object \"%s\"\n", req->name ); - - // look up the named model - StgModel* mod = StgDriver::world->GetModel( req->name ); - + + // look up the named model + Model* mod = StgDriver::world->GetModel( req->name ); + if( mod ) { Pose pose = mod->GetPose(); - + PRINT_DEBUG4( "Stage: returning location [ %.2f, %.2f, %.2f, %.2f ]\n", pose.x, pose.y, pose.z, pose.a ); - + player_simulation_pose3d_req_t reply; memcpy( &reply, req, sizeof(reply)); reply.pose.px = pose.x; @@ -250,9 +250,9 @@ reply.pose.ppitch = 0; // currently unused reply.pose.pyaw = pose.a; reply.simtime = mod->GetWorld()->SimTimeNow(); // time in microseconds - - this->driver->Publish( this->addr, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, + + this->driver->Publish( this->addr, resp_queue, + PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE3D, (void*)&reply, sizeof(reply), NULL ); return(0); @@ -263,32 +263,32 @@ return(-1); } } - + // Is it a request to set a model's pose in 3D? - if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_SIMULATION_REQ_SET_POSE3D, + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_SIMULATION_REQ_SET_POSE3D, this->addr)) { - player_simulation_pose3d_req_t* req = + player_simulation_pose3d_req_t* req = (player_simulation_pose3d_req_t*)data; - + // look up the named model - StgModel* mod = StgDriver::world->GetModel( req->name ); - + Model* mod = StgDriver::world->GetModel( req->name ); + if( mod ) { PRINT_DEBUG5( "Stage: moving \"%s\" to [ %.2f, %.2f, %.2f %.2f ]\n", req->name, req->pose.px, req->pose.py, req->pose.pz, req->pose.pyaw ); - + Pose pose = mod->GetPose(); pose.x = req->pose.px; pose.y = req->pose.py; pose.z = req->pose.pz; pose.a = req->pose.pyaw; // roll and pitch are unused - + mod->SetPose( pose ); - + this->driver->Publish(this->addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_SET_POSE3D); @@ -300,23 +300,23 @@ return(-1); } } - - + + // Is it a request to set a model's property? - else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_SIMULATION_REQ_SET_PROPERTY, + else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_SIMULATION_REQ_SET_PROPERTY, this->addr)) { - player_simulation_property_req_t* req = + player_simulation_property_req_t* req = (player_simulation_property_req_t*)data; - - // look up the named model - StgModel* mod = StgDriver::world->GetModel( req->name ); - + + // look up the named model + Model* mod = StgDriver::world->GetModel( req->name ); + if( mod ) { - int ack = - mod->SetProperty( req->prop, + int ack = + mod->SetProperty( req->prop, (void*)req->value ); this->driver->Publish(this->addr, resp_queue, @@ -332,22 +332,22 @@ } // Is it a request to get a model's property? -// else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -// PLAYER_SIMULATION_REQ_GET_PROPERTY, +// else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, +// PLAYER_SIMULATION_REQ_GET_PROPERTY, // this->addr)) // { -// player_simulation_property_req_t* req = +// player_simulation_property_req_t* req = // (player_simulation_property_req_t*)data; -// -// // look up the named model -// StgModel* mod = StgDriver::world->GetModel( req->name ); -// +// +// // look up the named model +// Model* mod = StgDriver::world->GetModel( req->name ); +// // if( mod ) // { -// +// // // This is probably wrong // req->value = (char*)mod->GetProperty( req->prop ); -// +// // this->driver->Publish(this->addr, resp_queue, // PLAYER_MSGTYPE_RESP_ACK, // PLAYER_SIMULATION_REQ_GET_PROPERTY); @@ -359,8 +359,8 @@ // return(-1); // } // } - - + + else { // Don't know how to handle this message. Modified: code/stage/trunk/libstageplugin/p_sonar.cc =================================================================== --- code/stage/trunk/libstageplugin/p_sonar.cc 2009-01-08 18:38:04 UTC (rev 7257) +++ code/stage/trunk/libstageplugin/p_sonar.cc 2009-01-09 21:03:45 UTC (rev 7258) @@ -54,7 +54,7 @@ void InterfaceSonar::Publish( void ) { - StgModelRanger* mod = (StgModelRanger*)this->mod; + ModelRanger* mod = (ModelRanger*)this->mod; if( mod->samples == NULL ) return; @@ -99,7 +99,7 @@ PLAYER_SONAR_REQ_GET_GEOM, this->addr) ) { - StgModelRanger* mod = (StgModelRanger*)this->mod; + ModelRanger* mod = (ModelRanger*)this->mod; size_t rcount = mod->sensor_count; Modified: code/stage/trunk/libstageplugin/stg_time.cc =================================================================== --- code/stage/trunk/libstageplugin/stg_time.cc 2009-01-08 18:38:04 UTC (rev 7257) +++ code/stage/trunk/libstageplugin/stg_time.cc 2009-01-09 21:03:45 UTC (rev 7258) @@ -47,31 +47,31 @@ int StTime::GetTime(struct timeval* time) { PRINT_DEBUG( "get time" ); - + assert( this->driver ); - - StgWorld* world = driver->world; - + + World* world = driver->world; + stg_usec_t usec = world->SimTimeNow(); time->tv_sec = (int)floor(usec/million); time->tv_usec = (int)rint(fmod(usec,million) * million); - + PRINT_DEBUG2( "time now %ld sec %ld usec", time->tv_sec, time->tv_usec ); - + return 0; } int StTime::GetTimeDouble(double* time) { PRINT_DEBUG( "get time (double)" ); - + assert( this->driver ); - - StgWorld* world = driver->world; - + + World* world = driver->world; + *time = world->SimTimeNow() / million; - + PRINT_DEBUG1( "time now %f sec ", *time); - + return 0; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------------ Check out the new SourceForge.net Marketplace. It is the best place to buy or sell services for just about anything Open Source. http://p.sf.net/sfu/Xq1LFB _______________________________________________ Playerstage-commit mailing list Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit