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
