00001 #ifndef _STAGE_PLAYER_DRIVER_H
00002 #define _STAGE_PLAYER_DRIVER_H
00003
00004 #include <unistd.h>
00005 #include <string.h>
00006 #include <math.h>
00007
00008 #include <libplayercore/playercore.h>
00009
00010 #include "../libstage/stage.hh"
00011
00012 #define DRIVER_ERROR(X) printf( "Stage driver error: %s\n", X )
00013
00014
00015 class Interface;
00016 class StgTime;
00017
00018 class StgDriver : public Driver
00019 {
00020 public:
00021
00022 StgDriver(ConfigFile* cf, int section);
00023
00024
00025 ~StgDriver(void);
00026
00027
00028 virtual int Setup();
00029 virtual int Shutdown();
00030 virtual int ProcessMessage(QueuePointer &resp_queue,
00031 player_msghdr * hdr,
00032 void * data);
00033 virtual int Subscribe(QueuePointer &queue, player_devaddr_t addr);
00034 virtual int Unsubscribe(QueuePointer &queue, player_devaddr_t addr);
00035
00038 virtual void Update();
00039
00041 static Stg::World* world;
00042 static bool usegui;
00043
00045 Interface* LookupDevice( player_devaddr_t addr );
00046
00047 Stg::Model* LocateModel( char* basename,
00048 player_devaddr_t* addr,
00049 const std::string& type );
00050
00051 protected:
00052
00054 std::vector<Interface*> devices;
00055 };
00056
00057
00058 class Interface
00059 {
00060 public:
00061 Interface(player_devaddr_t addr,
00062 StgDriver* driver,
00063 ConfigFile* cf,
00064 int section );
00065
00066 virtual ~Interface( void ){ };
00067
00068 player_devaddr_t addr;
00069 double last_publish_time;
00070 double publish_interval_msec;
00071
00072 StgDriver* driver;
00073
00074 virtual int ProcessMessage(QueuePointer &resp_queue,
00075 player_msghdr_t* hdr,
00076 void* data) { return(-1); }
00077
00078 virtual void Publish( void ){};
00079 virtual void Subscribe( void ){};
00080 virtual void Unsubscribe( void ){};
00081 virtual void Subscribe( QueuePointer &queue ){};
00082 virtual void Unsubscribe( QueuePointer &queue ){};
00083 };
00084
00085 class InterfaceSimulation : public Interface
00086 {
00087 public:
00088 InterfaceSimulation( player_devaddr_t addr, StgDriver* driver,ConfigFile* cf, int section );
00089 virtual ~InterfaceSimulation( void ){ };
00090 virtual int ProcessMessage(QueuePointer & resp_queue,
00091 player_msghdr_t* hdr,
00092 void* data);
00093 };
00094
00095
00096 class InterfaceModel
00097
00098 : public Interface
00099 {
00100 public:
00101 InterfaceModel( player_devaddr_t addr,
00102 StgDriver* driver,
00103 ConfigFile* cf,
00104 int section,
00105 const std::string& type );
00106
00107 virtual ~InterfaceModel( void ){ Unsubscribe(); };
00108
00109 virtual void Subscribe( void );
00110 virtual void Unsubscribe( void );
00111
00112 protected:
00113 Stg::Model* mod;
00114
00115 private:
00116 bool subscribed;
00117 };
00118
00119
00120 class InterfacePosition : public InterfaceModel
00121 {
00122 public:
00123 InterfacePosition( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00124 virtual ~InterfacePosition( void ){ };
00125 virtual void Publish( void );
00126 virtual int ProcessMessage(QueuePointer & resp_queue,
00127 player_msghdr_t* hdr,
00128 void* data);
00129 };
00130
00131 class InterfaceGripper : public InterfaceModel
00132 {
00133 public:
00134 InterfaceGripper( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00135 virtual ~InterfaceGripper( void ){ };
00136 virtual int ProcessMessage(QueuePointer & resp_queue,
00137 player_msghdr_t* hdr,
00138 void* data);
00139 virtual void Publish( void );
00140 };
00141
00142 class InterfaceWifi : public InterfaceModel
00143 {
00144 public:
00145 InterfaceWifi( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00146 virtual ~InterfaceWifi( void ){ };
00147 virtual int ProcessMessage(QueuePointer & resp_queue,
00148 player_msghdr_t* hdr,
00149 void* data);
00150 virtual void Publish( void );
00151 };
00152
00153 class InterfaceSpeech : public InterfaceModel
00154 {
00155 public:
00156 InterfaceSpeech( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00157 virtual ~InterfaceSpeech( void ){ };
00158 virtual int ProcessMessage(QueuePointer & resp_queue,
00159 player_msghdr_t* hdr,
00160 void* data);
00161 virtual void Publish( void );
00162 };
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178 class InterfaceRanger : public InterfaceModel
00179 {
00180 private:
00181 int scan_id;
00182 public:
00183 InterfaceRanger( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00184 virtual ~InterfaceRanger( void ){ };
00185 virtual int ProcessMessage(QueuePointer & resp_queue,
00186 player_msghdr_t* hdr,
00187 void* data);
00188 virtual void Publish( void );
00189 };
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213 class InterfacePower : public InterfaceModel
00214 {
00215 public:
00216 InterfacePower( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00217 virtual ~InterfacePower( void ){ };
00218
00219 virtual int ProcessMessage( QueuePointer & resp_queue,
00220 player_msghdr * hdr,
00221 void * data );
00222
00223 virtual void Publish( void );
00224 };
00225
00226 class InterfaceFiducial : public InterfaceModel
00227 {
00228 public:
00229 InterfaceFiducial( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00230 virtual ~InterfaceFiducial( void ){ };
00231
00232 virtual void Publish( void );
00233 virtual int ProcessMessage(QueuePointer & resp_queue,
00234 player_msghdr_t* hdr,
00235 void* data);
00236 };
00237
00238
00239 class InterfaceActArray : public InterfaceModel
00240 {
00241 public:
00242 InterfaceActArray( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00243 virtual ~InterfaceActArray( void ){ };
00244
00245 virtual int ProcessMessage( QueuePointer & resp_queue,
00246 player_msghdr * hdr,
00247 void * data );
00248 virtual void Publish( void );
00249 };
00250
00251 class InterfaceBlobfinder : public InterfaceModel
00252 {
00253 public:
00254 InterfaceBlobfinder( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00255 virtual ~InterfaceBlobfinder( void ){ };
00256
00257 virtual int ProcessMessage( QueuePointer & resp_queue,
00258 player_msghdr * hdr,
00259 void * data );
00260 virtual void Publish( void );
00261 };
00262
00263 class InterfacePtz : public InterfaceModel
00264 {
00265 public:
00266 InterfacePtz( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00267 virtual ~InterfacePtz( void ){ };
00268
00269 virtual int ProcessMessage( QueuePointer & resp_queue,
00270 player_msghdr * hdr,
00271 void * data );
00272 virtual void Publish( void );
00273 };
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289 class InterfaceBumper : public InterfaceModel
00290 {
00291 public:
00292 InterfaceBumper( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00293 virtual ~InterfaceBumper( void ){ };
00294
00295 virtual int ProcessMessage( QueuePointer & resp_queue,
00296 player_msghdr * hdr,
00297 void * data );
00298 virtual void Publish( void );
00299 };
00300
00301 class InterfaceLocalize : public InterfaceModel
00302 {
00303 public:
00304 InterfaceLocalize( player_devaddr_t addr,
00305 StgDriver* driver,
00306 ConfigFile* cf,
00307 int section );
00308
00309 virtual ~InterfaceLocalize( void ){ };
00310
00311 virtual void Publish( void );
00312 virtual int ProcessMessage(QueuePointer & resp_queue,
00313 player_msghdr_t* hdr,
00314 void* data);
00315 };
00316
00317 class InterfaceMap : public InterfaceModel
00318 {
00319 public:
00320 InterfaceMap( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00321 virtual ~InterfaceMap( void ){ };
00322
00323 virtual int ProcessMessage( QueuePointer & resp_queue,
00324 player_msghdr * hdr,
00325 void * data );
00326
00327
00328
00329
00330 int HandleMsgReqInfo( QueuePointer & resp_queue,
00331 player_msghdr * hdr,
00332 void * data );
00333 int HandleMsgReqData( QueuePointer & resp_queue,
00334 player_msghdr * hdr,
00335 void * data );
00336 };
00337
00338 class PlayerGraphics2dVis;
00339 class InterfaceGraphics2d : public InterfaceModel
00340 {
00341 public:
00342 InterfaceGraphics2d( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00343 virtual ~InterfaceGraphics2d( void );
00344
00345 void Subscribe(QueuePointer &queue);
00346 void Unsubscribe(QueuePointer &queue);
00347
00348 virtual int ProcessMessage( QueuePointer & resp_queue,
00349 player_msghdr * hdr,
00350 void * data );
00351
00352 PlayerGraphics2dVis * vis;
00353 };
00354
00355 class PlayerGraphics3dVis;
00356 class InterfaceGraphics3d : public InterfaceModel
00357 {
00358 public:
00359 InterfaceGraphics3d( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
00360 virtual ~InterfaceGraphics3d( void );
00361
00362 void Subscribe(QueuePointer &queue);
00363 void Unsubscribe(QueuePointer &queue);
00364
00365 virtual int ProcessMessage( QueuePointer & resp_queue,
00366 player_msghdr * hdr,
00367 void * data );
00368
00369 PlayerGraphics3dVis * vis;
00370 };
00371
00373 class StTime : public PlayerTime
00374 {
00375 private:
00376 StgDriver* driver;
00377
00378 public:
00379
00380 StTime( StgDriver* driver );
00381
00382
00383 virtual ~StTime();
00384
00385
00386 int GetTime(struct timeval* time);
00387 int GetTimeDouble(double* time);
00388 };
00389
00390
00391 #endif