Skip to content
Browse files

Applied patch #2793940: Add bumper device to stage

  • Loading branch information...
1 parent c624f33 commit 5039425b69efee4459b5812bff30cb4cde41f2d9 @jpgr87 committed Sep 18, 2010
Showing with 124 additions and 92 deletions.
  1. +1 −0 libstage/CMakeLists.txt
  2. +39 −11 libstage/stage.hh
  3. +1 −0 libstage/typetable.cc
  4. +2 −1 libstageplugin/CMakeLists.txt
  5. +80 −79 libstageplugin/p_bumper.cc
  6. +1 −1 libstageplugin/p_driver.h
View
1 libstage/CMakeLists.txt
@@ -16,6 +16,7 @@ set( stageSrcs
model_actuator.cc
model_blinkenlight.cc
model_blobfinder.cc
+ model_bumper.cc
model_callbacks.cc
model_camera.cc
model_draw.cc
View
50 libstage/stage.hh
@@ -2690,20 +2690,48 @@ namespace Stg
};
- // \todo BUMPER MODEL --------------------------------------------------------
- // typedef struct
- // {
- // Pose pose;
- // stg_meters_t length;
- // } stg_bumper_config_t;
- // typedef struct
- // {
- // Model* hit;
- // stg_point_t hit_point;
- // } stg_bumper_sample_t;
+ // BUMPER MODEL --------------------------------------------------------
+ typedef struct
+ {
+ Pose pose;
+ stg_meters_t length;
+ } stg_bumper_config_t;
+
+ typedef struct
+ {
+ Model* hit;
+ stg_point_t hit_point;
+ } stg_bumper_sample_t;
+ class ModelBumper : public Model
+ {
+ protected:
+
+ virtual void Startup(); // HH: OK
+ virtual void Shutdown(); // HH: OK
+ virtual void Update();
+ virtual void DataVisualize( Camera* cam );
+
+ public:
+ static const char* typestr;
+
+ // constructor
+ ModelBumper( World* world, Model* parent, const std::string& type ); // HH: OK
+ // destructor
+ virtual ~ModelBumper(); // HH: OK
+
+ virtual void Load(); // HH: OK
+ virtual void Print( char* prefix );
+
+ uint32_t bumper_count;
+ stg_bumper_config_t* bumpers;
+ stg_bumper_sample_t* samples;
+
+ private:
+ static Option showBumperData;
+ };
// FIDUCIAL MODEL --------------------------------------------------------
View
1 libstage/typetable.cc
@@ -24,6 +24,7 @@ void Stg::RegisterModels()
Register( "actuator", Creator<ModelActuator> );
Register( "blinkenlight", Creator<ModelBlinkenlight> );
Register( "blobfinder", Creator<ModelBlobfinder> );
+ Register( "bumper", Creator<ModelBumper> );
Register( "camera", Creator<ModelCamera> );
Register( "fiducial", Creator<ModelFiducial> );
Register( "gripper", Creator<ModelGripper> );
View
3 libstageplugin/CMakeLists.txt
@@ -8,7 +8,8 @@ set( stagepluginSrcs
p_driver.cc
p_actarray.cc
p_blobfinder.cc
- p_gripper.cc
+ p_bumper.cc
+ p_gripper.cc
p_simulation.cc
p_laser.cc
p_fiducial.cc
View
159 libstageplugin/p_bumper.cc
@@ -32,104 +32,105 @@
@par Bumper interface
- PLAYER_BUMPER_DATA_STATE
- PLAYER_BUMPER_GET_GEOM
-*/
+ */
// CODE ----------------------------------------------------------------------
#include "p_driver.h"
+using namespace Stg;
//
// BUMPER INTERFACE
//
-extern "C" {
-int bumper_init( stg_model_t* mod );
-}
-
InterfaceBumper::InterfaceBumper( player_devaddr_t id,
- StgDriver* driver,
- ConfigFile* cf,
- int section )
- : InterfaceModel( id, driver, cf, section, bumper_init )
+ StgDriver* driver,
+ ConfigFile* cf,
+ int section )
+: InterfaceModel( id, driver, cf, section, "bumper" )
{
}
void InterfaceBumper::Publish( void )
{
-
- size_t len = mod->data_len;
- stg_bumper_sample_t* sdata = (stg_bumper_sample_t*)mod->data;
-
- player_bumper_data_t pdata;
- memset( &pdata, 0, sizeof(pdata) );
-
- if( len > 0 )
- {
- size_t bcount = len / sizeof(stg_bumper_sample_t);
-
- // limit the number of samples to Player's maximum
- if( bcount > PLAYER_BUMPER_MAX_SAMPLES )
- bcount = PLAYER_BUMPER_MAX_SAMPLES;
-
- pdata.bumpers_count = bcount;
-
- for( int i=0; i<(int)bcount; i++ )
- pdata.bumpers[i] = sdata[i].hit ? 1 : 0;
- }
-
- this->driver->Publish( this->addr, NULL,
- PLAYER_MSGTYPE_DATA,
- PLAYER_BUMPER_DATA_STATE,
- &pdata, sizeof(pdata), NULL);
+
+ ModelBumper* mod = (ModelBumper*)this->mod;
+
+ player_bumper_data_t bumper;
+ memset( &bumper, 0, sizeof(bumper) );
+
+ stg_bumper_sample_t* sdata = (stg_bumper_sample_t*)mod->samples;
+
+ size_t count = mod->bumper_count;
+
+ if( count > 0 )
+ {
+ {
+ bumper.bumpers_count = count;
+ bumper.bumpers = new uint8_t[count];
+
+ for( unsigned int i=0; i<count; i++ )
+ bumper.bumpers[i] = sdata[i].hit ? 1 : 0;
+ }
+ }
+
+ this->driver->Publish( this->addr,
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_SONAR_DATA_RANGES,
+ &bumper, sizeof(bumper), NULL);
+
+ if( bumper.bumpers )
+ delete[] bumper.bumpers;
}
-int InterfaceBumper::ProcessMessage( MessageQueue* resp_queue,
- player_msghdr_t* hdr,
- void* data )
+int InterfaceBumper::ProcessMessage( QueuePointer & resp_queue,
+ player_msghdr_t* hdr,
+ void* data )
{
- if( Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
- PLAYER_BUMPER_GET_GEOM,
- this->addr) )
- {
- size_t cfglen = mod->cfg_len;
- stg_bumper_config_t* cfgs = (stg_bumper_config_t*)mod->cfg;
- assert( cfgs );
-
- size_t bcount = cfglen / sizeof(stg_bumper_config_t);
-
- // convert the bumper data into Player-format bumper poses
- player_bumper_geom_t pgeom;
- memset( &pgeom, 0, sizeof(pgeom) );
-
- // limit the number of samples to Player's maximum
- if( bcount > PLAYER_BUMPER_MAX_SAMPLES )
- bcount = PLAYER_BUMPER_MAX_SAMPLES;
-
- pgeom.bumper_def_count = bcount;
-
- for( int i=0; i<(int)bcount; i++ )
+ if( Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_BUMPER_REQ_GET_GEOM,
+ this->addr) )
+ {
+ ModelBumper* mod = (ModelBumper*)this->mod;
+
+ size_t bcount = mod->bumper_count;
+
+ // convert the bumper data into Player-format bumper poses
+ player_bumper_geom_t pgeom;
+ memset( &pgeom, 0, sizeof(pgeom) );
+
+ // limit the number of samples to Player's maximum
+ if( bcount > 64 )
+ bcount = 64;
+
+ pgeom.bumper_def_count = bcount;
+
+ for( int i=0; i<(int)bcount; i++ )
+ {
+ // fill in the geometry data formatted player-like
+ pgeom.bumper_def[i].pose.px = mod->bumpers[i].pose.x;
+ pgeom.bumper_def[i].pose.py = mod->bumpers[i].pose.y;
+ pgeom.bumper_def[i].pose.pz = mod->bumpers[i].pose.z;
+ pgeom.bumper_def[i].pose.proll = 0;
+ pgeom.bumper_def[i].pose.ppitch = 0;
+ pgeom.bumper_def[i].pose.pyaw = mod->bumpers[i].pose.a;
+ pgeom.bumper_def[i].length = mod->bumpers[i].length;
+ pgeom.bumper_def[i].radius = 0;
+ }
+
+ this->driver->Publish( this->addr, resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_BUMPER_REQ_GET_GEOM,
+ (void*)&pgeom, sizeof(pgeom), NULL );
+
+ return 0; // ok
+ }
+ else
{
- // fill in the geometry data formatted player-like
- pgeom.bumper_def[i].pose.px = cfgs[i].pose.x;
- pgeom.bumper_def[i].pose.py = cfgs[i].pose.y;
- pgeom.bumper_def[i].pose.pa = cfgs[i].pose.a;
- pgeom.bumper_def[i].length = cfgs[i].length;
- pgeom.bumper_def[i].radius = 0;
+ // Don't know how to handle this message.
+ PRINT_WARN2( "stg_bumper doesn't support msg with type/subtype %d/%d",
+ hdr->type, hdr->subtype);
+ return(-1);
}
-
- this->driver->Publish( this->addr, resp_queue,
- PLAYER_MSGTYPE_RESP_ACK,
- PLAYER_BUMPER_GET_GEOM,
- (void*)&pgeom, sizeof(pgeom), NULL );
-
- return 0; // ok
- }
- else
- {
- // Don't know how to handle this message.
- PRINT_WARN2( "stg_bumper doesn't support msg with type/subtype %d/%d",
- hdr->type, hdr->subtype);
- return(-1);
- }
}
View
2 libstageplugin/p_driver.h
@@ -279,7 +279,7 @@ class InterfaceBumper : public InterfaceModel
virtual ~InterfaceBumper( void ){ /* TODO: clean up*/ };
virtual int ProcessMessage( QueuePointer & resp_queue,
- player_msghdr * hdr,
+ player_msghdr_t * hdr,
void * data );
virtual void Publish( void );
};

0 comments on commit 5039425

Please sign in to comment.
Something went wrong with that request. Please try again.