Revision: 6866
http://playerstage.svn.sourceforge.net/playerstage/?rev=6866&view=rev
Author: rtv
Date: 2008-07-14 23:06:08 -0700 (Mon, 14 Jul 2008)
Log Message:
-----------
added fiducial interface support to player plugin
Modified Paths:
--------------
code/stage/trunk/libstageplugin/CMakeLists.txt
code/stage/trunk/libstageplugin/p_driver.cc
code/stage/trunk/libstageplugin/p_fiducial.cc
Removed Paths:
-------------
code/stage/trunk/libstageplugin/player_interfaces.h
Modified: code/stage/trunk/libstageplugin/CMakeLists.txt
===================================================================
--- code/stage/trunk/libstageplugin/CMakeLists.txt 2008-07-14 18:49:45 UTC
(rev 6865)
+++ code/stage/trunk/libstageplugin/CMakeLists.txt 2008-07-15 06:06:08 UTC
(rev 6866)
@@ -6,13 +6,13 @@
p_driver.cc
p_simulation.cc
p_laser.cc
+ p_fiducial.cc
p_position.cc
p_sonar.cc
stg_time.cc
)
# p_graphics3d.cc
-# p_fiducial.cc
# p_blobfinder.cc
Modified: code/stage/trunk/libstageplugin/p_driver.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_driver.cc 2008-07-14 18:49:45 UTC (rev
6865)
+++ code/stage/trunk/libstageplugin/p_driver.cc 2008-07-15 06:06:08 UTC (rev
6866)
@@ -311,9 +311,9 @@
//ifsrc = new InterfaceBlobfinder( player_addr, this, cf, section );
//break;
- case PLAYER_SIMULATION_CODE:
- ifsrc = new InterfaceSimulation( player_addr, this, cf, section );
- break;
+ case PLAYER_SIMULATION_CODE:
+ ifsrc = new InterfaceSimulation( player_addr, this, cf, section );
+ break;
case PLAYER_LASER_CODE:
ifsrc = new InterfaceLaser( player_addr, this, cf, section );
@@ -335,10 +335,10 @@
// ifsrc = new InterfacePtz( player_addr, this, cf, section );
// break;
-// case PLAYER_FIDUCIAL_CODE:
-// ifsrc = new InterfaceFiducial( player_addr, this, cf, section );
-// break;
-
+ case PLAYER_FIDUCIAL_CODE:
+ ifsrc = new InterfaceFiducial( player_addr, this, cf, section );
+ break;
+
// case PLAYER_LOCALIZE_CODE:
// ifsrc = new InterfaceLocalize( player_addr, this, cf, section );
// break;
Modified: code/stage/trunk/libstageplugin/p_fiducial.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_fiducial.cc 2008-07-14 18:49:45 UTC
(rev 6865)
+++ code/stage/trunk/libstageplugin/p_fiducial.cc 2008-07-15 06:06:08 UTC
(rev 6866)
@@ -46,84 +46,72 @@
#include "p_driver.h"
-extern "C" {
-int fiducial_init( stg_model_t* mod );
-}
InterfaceFiducial::InterfaceFiducial( player_devaddr_t addr,
StgDriver* driver,
ConfigFile* cf,
int section )
- : InterfaceModel( addr, driver, cf, section, fiducial_init )
+ : InterfaceModel( addr, driver, cf, section, MODEL_TYPE_FIDUCIAL )
{
- // nothing to do...
}
void InterfaceFiducial::Publish( void )
-{
- size_t len = mod->data_len;
- stg_fiducial_t* fids = (stg_fiducial_t*)mod->data;
-
+{
player_fiducial_data_t pdata;
memset( &pdata, 0, sizeof(pdata) );
- if( len > 0 )
+ StgModelFiducial* fidmod = (StgModelFiducial*)this->mod;
+
+ if( fidmod->fiducial_count > 0 )
{
- size_t fcount = len / sizeof(stg_fiducial_t);
- assert( fcount > 0 );
-
- pdata.fiducials_count = fcount;
-
- //printf( "reporting %d fiducials\n",
- // fcount );
+ stg_fiducial_t* fids = fidmod->fiducials;
+ assert( fids );
- if( fcount > PLAYER_FIDUCIAL_MAX_SAMPLES )
- {
- PRINT_WARN2( "A Stage model has detected more fiducials than"
- " will fit in Player's buffer (%d/%d)\n",
- fcount, PLAYER_FIDUCIAL_MAX_SAMPLES );
- fcount = PLAYER_FIDUCIAL_MAX_SAMPLES;
- }
+ pdata.fiducials_count = fidmod->fiducial_count;
- for( int i=0; i<(int)fcount; i++ )
- {
- pdata.fiducials[i].id = fids[i].id;
-
- // 2D x,y only
- double xpos = fids[i].range * cos(fids[i].bearing);
- double ypos = fids[i].range * sin(fids[i].bearing);
-
+ //printf( "reporting %d fiducials\n",
+ // fcount );
+
+ for( int i=0; i<(int)pdata.fiducials_count; i++ )
+ {
+ pdata.fiducials[i].id = fids[i].id;
+
+ // 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].pos[0] = xpos;
- pdata.fiducials[i].pos[1] = ypos;
-
- // yaw only
- pdata.fiducials[i].rot[2] = fids[i].geom.a;
- // player can't handle per-fiducial size.
- // we leave uncertainty (upose) at zero
- */
-
+ pdata.fiducials[i].pos[0] = xpos;
+ pdata.fiducials[i].pos[1] = ypos;
+
+ // yaw only
+ pdata.fiducials[i].rot[2] = fids[i].geom.a;
+ // player can't handle per-fiducial size.
+ // we leave uncertainty (upose) at zero
+ */
+
// I'm guessing at this, but the above doesn't compile, because
// there's no 'pos' or 'rot' fields - BPG
- pdata.fiducials[i].pose.px = xpos;
- pdata.fiducials[i].pose.py = ypos;
- pdata.fiducials[i].pose.pz = 0.0;
- pdata.fiducials[i].pose.proll = 0.0;
- pdata.fiducials[i].pose.ppitch = 0.0;
- pdata.fiducials[i].pose.pyaw = fids[i].geom.a;
- }
+
+ pdata.fiducials[i].pose.px = fids[i].pose.x;
+ pdata.fiducials[i].pose.py = fids[i].pose.y;
+ pdata.fiducials[i].pose.pz = fids[i].pose.z;
+ pdata.fiducials[i].pose.proll = 0.0;
+ pdata.fiducials[i].pose.ppitch = 0.0;
+ pdata.fiducials[i].pose.pyaw = fids[i].geom.a;
+ }
}
// publish this data
- this->driver->Publish( this->addr, NULL,
- PLAYER_MSGTYPE_DATA,
- PLAYER_FIDUCIAL_DATA_SCAN,
- &pdata, sizeof(pdata), NULL);
+ this->driver->Publish( this->addr,
+
PLAYER_MSGTYPE_DATA,
+
PLAYER_FIDUCIAL_DATA_SCAN,
+ &pdata,
sizeof(pdata), NULL);
}
-int InterfaceFiducial::ProcessMessage(MessageQueue* resp_queue,
- player_msghdr_t* hdr,
- void* data )
+int InterfaceFiducial::ProcessMessage(QueuePointer& resp_queue,
+
player_msghdr_t* hdr,
+
void* data )
{
//printf("got fiducial request\n");
@@ -132,76 +120,76 @@
PLAYER_FIDUCIAL_REQ_GET_GEOM,
this->addr))
{
- stg_geom_t geom;
- stg_model_get_geom( this->mod, &geom );
+ stg_geom_t geom = mod->GetGeom();
+ stg_pose_t pose = mod->GetPose();
- stg_pose_t pose;
- stg_model_get_pose( this->mod, &pose );
-
// 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.pa = 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_FIDUCIAL_REQ_GET_GEOM,
- (void*)&pgeom, sizeof(pgeom), NULL);
+
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,
- this->addr))
+
PLAYER_FIDUCIAL_REQ_SET_ID,
+
this->addr))
{
if( hdr->size == sizeof(player_fiducial_id_t) )
- {
- // 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;
-
- stg_model_set_fiducial_return( this->mod, 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_FIDUCIAL_REQ_SET_ID,
- (void*)&pid, sizeof(pid) );
- }
+ {
+ // 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_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) );
- return(-1); // error - NACK is sent automatically
- }
+ {
+ PRINT_ERR2("Incorrect packet size setting fiducial ID
(%d/%d)",
+ (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,
- this->addr))
+
PLAYER_FIDUCIAL_REQ_GET_ID,
+
this->addr))
{
// fill in the data formatted player-like
player_fiducial_id_t pid;
- pid.id = mod->fiducial_return;
+ pid.id = mod->FiducialReturn();
// acknowledge, including the new ID
this->driver->Publish(this->addr, resp_queue,
- PLAYER_MSGTYPE_RESP_ACK,
- PLAYER_FIDUCIAL_REQ_GET_ID,
- (void*)&pid, sizeof(pid) );
+
PLAYER_MSGTYPE_RESP_ACK,
+
PLAYER_FIDUCIAL_REQ_GET_ID,
+
(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;
-
+
+ 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) );
@@ -242,15 +230,15 @@
}
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);
+ hdr->type, hdr->subtype);
return(-1);
}
-
+
return 0;
}
Deleted: code/stage/trunk/libstageplugin/player_interfaces.h
===================================================================
--- code/stage/trunk/libstageplugin/player_interfaces.h 2008-07-14 18:49:45 UTC
(rev 6865)
+++ code/stage/trunk/libstageplugin/player_interfaces.h 2008-07-15 06:06:08 UTC
(rev 6866)
@@ -1,155 +0,0 @@
-#ifndef _STAGE_PLAYER_INTERFACES_H
-#define _STAGE_PLAYER_INTERFACES_H
-
-#include "player_driver.h"
-
-class Interface
-{
- public:
- Interface( player_device_id_t id, StgDriver* driver,ConfigFile* cf, int
section );
-
- virtual ~Interface( void ){ /* TODO: clean up*/ };
-
- player_device_id_t id;
- stg_model_t* mod;
-
- StgDriver* driver; // the driver instance that created this device
-
- size_t data_len;
- size_t cmd_len;
- size_t req_qlen;
- size_t rep_qlen;
-
- // pure virtual methods
- virtual void Command( void* buffer, size_t len ){}; // empty implementation
- virtual void Configure( void* client, void* buffer, size_t len ){}; // empty
implementation
- virtual void Publish( void ){}; // empty implementation
-};
-
-
-class InterfaceSimulation : public Interface
-{
- public:
- InterfaceSimulation( player_device_id_t id, StgDriver* driver,ConfigFile*
cf, int section );
- virtual ~InterfaceSimulation( void ){ /* TODO: clean up*/ };
-
- //virtual void Command( void* buffer, size_t len );
- virtual void Configure( void* client, void* buffer, size_t len );
-};
-
-
-class InterfaceModel : public Interface
-{
- public:
- InterfaceModel( player_device_id_t id,
- StgDriver* driver,
- ConfigFile* cf,
- int section,
- stg_model_type_t modtype );
-
- virtual ~InterfaceModel( void ){ /* TODO: clean up*/ };
-};
-
-class InterfacePosition : public InterfaceModel
-{
- public:
- InterfacePosition( player_device_id_t id, StgDriver* driver, ConfigFile* cf,
int section );
- virtual ~InterfacePosition( void ){ /* TODO: clean up*/ };
- virtual void Command( void* buffer, size_t len );
- virtual void Configure( void* client, void* buffer, size_t len );
- virtual void Publish( void );
-};
-
-class InterfaceLaser : public InterfaceModel
-{
- public:
- InterfaceLaser( player_device_id_t id, StgDriver* driver, ConfigFile* cf,
int section );
- virtual ~InterfaceLaser( void ){ /* TODO: clean up*/ };
- //virtual void Command( void* buffer, size_t len );
- virtual void Configure( void* client, void* buffer, size_t len );
- virtual void Publish( void );
-};
-
-class InterfaceFiducial : public InterfaceModel
-{
- public:
- InterfaceFiducial( player_device_id_t id, StgDriver* driver, ConfigFile* cf,
int section );
- virtual ~InterfaceFiducial( void ){ /* TODO: clean up*/ };
- //virtual void Command( void* buffer, size_t len );
- virtual void Configure( void* client, void* buffer, size_t len );
- virtual void Publish( void );
-};
-
-class InterfaceBlobfinder : public InterfaceModel
-{
- public:
- InterfaceBlobfinder( player_device_id_t id, StgDriver* driver, ConfigFile*
cf, int section );
- virtual ~InterfaceBlobfinder( void ){ /* TODO: clean up*/ };
- //virtual void Command( void* buffer, size_t len );
- virtual void Configure( void* client, void* buffer, size_t len );
- virtual void Publish( void );
-};
-
-class InterfaceSonar : public InterfaceModel
-{
- public:
- InterfaceSonar( player_device_id_t id, StgDriver* driver, ConfigFile* cf,
int section );
- virtual ~InterfaceSonar( void ){ /* TODO: clean up*/ };
- //virtual void Command( void* buffer, size_t len );
- virtual void Configure( void* client, void* buffer, size_t len );
- virtual void Publish( void );
-};
-
-
-// declare a raft of functions that interface between Player and Stage
interfaces
-
-
-// POSITION INTERFACE
-//void PositionCommand( Interface* device, void* buffer, size_t len );
-//void PositionData( Interface* device, void* data, size_t len );
-//int PositionData( stg_model_t* mod, char* name, void* data, size_t len,
void* userp );
-//void PositionConfig( Interface* device, void* client, void* buffer, size_t
len );
-
-// LOCALIZE INTERFACE
-//void LocalizeCommand( Interface* device, void* buffer, size_t len );
-//void LocalizeData( Interface* device, void* data, size_t len );
-//void LocalizeConfig( Interface* device, void* client, void* buffer, size_t
len );
-
-// SONAR INTERFACE
-//void SonarData( Interface* device, void* data, size_t len );
-//void SonarConfig( Interface* device, void* client, void* buffer, size_t len
);
-
-// ENERGY INTERFACE
-//void EnergyData( Interface* device, void* data, size_t len );
-//void EnergyConfig( Interface* device, void* client, void* buffer, size_t len
);
-
-// SIMULATION INTERFACE
-//void SimulationData( Interface* device, void* data, size_t len );
-//void SimulationConfig( player_device_id_t id, void* client, void* buffer,
size_t len);
-
-// BLOBFINDER INTERFACE
-//void BlobfinderData( Interface* device, void* data, size_t len );
-//void BlobfinderConfig( Interface* device, void* client, void* buffer, size_t
len);
-
-// LASER INTERFACE
-//void LaserConfig( Interface* device, void* client, void* buffer, size_t len
);
-//int LaserData( stg_model_t* mod, char* name, void* data, size_t len, void*
userp );
-
-// GRIPPER INTERFACE
-//void GripperCommand( Interface* device, void* src, size_t len );
-//void GripperConfig( Interface* device, void* client, void* buffer, size_t
len );
-//void GripperData( Interface* device, void* data, size_t len );
-
-// FIDUCIAL INTERFACE
-//void FiducialData( Interface* device, void* data, size_t len );
-//int FiducialData( stg_model_t* mod, char* name, void* data, size_t len,
void* userp );
-//void FiducialConfig( Interface* device, void* client, void* buffer, size_t
len );
-
-// MAP INTERFACE
-//void MapData( Interface* device, void* data, size_t len );
-//void MapConfig( Interface* device, void* client, void* buffer, size_t len);
-//void MapConfigInfo( Interface* device, void* client, void* buffer, size_t
len);
-//void MapConfigData( Interface* device, void* client, void* buffer, size_t
len);
-
-
-#endif
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
