From 1d44d2b5348575a7a712acfc30b317d4e0642f48 Mon Sep 17 00:00:00 2001 From: Richard Vaughan Date: Mon, 22 Nov 2010 21:18:12 -0800 Subject: [PATCH] working on fixing player plugin --- avonstage/avonstage.cc | 2 +- libstage/model.cc | 81 +++++----- libstage/stage.hh | 52 +++---- libstageplugin/p_driver.cc | 2 +- libstageplugin/p_driver.h | 4 +- libstageplugin/p_ranger.cc | 308 +++++++++++++++++++------------------ 6 files changed, 230 insertions(+), 219 deletions(-) diff --git a/avonstage/avonstage.cc b/avonstage/avonstage.cc index 4fc0d9943..9d70c7c61 100644 --- a/avonstage/avonstage.cc +++ b/avonstage/avonstage.cc @@ -179,7 +179,7 @@ int RangerData( Stg::Model* mod, av_msg_t* data ) t.pose[5] = s.pose.a; const std::vector& ranges = s.ranges; - const std::vector& intensities = s.intensities; + const std::vector& intensities = s.intensities; assert( ranges.size() == intensities.size() ); diff --git a/libstage/model.cc b/libstage/model.cc index 477275f66..864e54951 100644 --- a/libstage/model.cc +++ b/libstage/model.cc @@ -339,55 +339,54 @@ void Model::GuiState::Load( Worldfile* wf, int wf_entity ) // constructor Model::Model( World* world, Model* parent, - const std::string& type ) : + const std::string& type ) : Ancestor(), alwayson(false), blockgroup(), blocks_dl(0), boundary(false), - callbacks(__CB_TYPE_COUNT), // one slot in the vector for each type + callbacks(__CB_TYPE_COUNT), // one slot in the vector for each type color( 1,0,0 ), // red data_fresh(false), disabled(false), - cv_list(), - flag_list(), - geom(), - has_default_block( true ), - id( Model::count++ ), - interval((usec_t)1e5), // 100msec - interval_energy((usec_t)1e5), // 100msec - interval_pose((usec_t)1e5), // 100msec - last_update(0), - log_state(false), - map_resolution(0.1), - mass(0), - parent(parent), - pose(), - power_pack( NULL ), - pps_charging(), - rastervis(), - rebuild_displaylist(true), - say_string(), - stack_children( true ), - stall(false), - subs(0), - thread_safe( false ), - trail(trail_length), - trail_index(0), - type(type), - event_queue_num( 0 ), - used(false), - velocity(), - velocity_enable( false ), - watts(0.0), - watts_give(0.0), - watts_take(0.0), - wf(NULL), - wf_entity(0), - world(world), - world_gui( dynamic_cast( world ) ) -{ - //assert( modelsbyid ); + cv_list(), + flag_list(), + geom(), + has_default_block( true ), + id( Model::count++ ), + interval((usec_t)1e5), // 100msec + interval_energy((usec_t)1e5), // 100msec + interval_pose((usec_t)1e5), // 100msec + last_update(0), + log_state(false), + map_resolution(0.1), + mass(0), + parent(parent), + pose(), + power_pack( NULL ), + pps_charging(), + rastervis(), + rebuild_displaylist(true), + say_string(), + stack_children( true ), + stall(false), + subs(0), + thread_safe( false ), + trail(trail_length), + trail_index(0), + type(type), + event_queue_num( 0 ), + used(false), + velocity(), + velocity_enable( false ), + watts(0.0), + watts_give(0.0), + watts_take(0.0), + wf(NULL), + wf_entity(0), + world(world), + world_gui( dynamic_cast( world ) ) +{ assert( world ); PRINT_DEBUG3( "Constructing model world: %s parent: %s type: %s \n", diff --git a/libstage/stage.hh b/libstage/stage.hh index 8d9d13db4..fcb82c77d 100644 --- a/libstage/stage.hh +++ b/libstage/stage.hh @@ -2793,7 +2793,7 @@ namespace Stg static Option showTransducers; Vis( World* world ); - virtual ~Vis( void ){} + virtual ~Vis( void ){} virtual void Visualize( Model* mod, Camera* cam ); } vis; @@ -2808,7 +2808,7 @@ namespace Stg Color col; std::vector ranges; - std::vector intensities; + std::vector intensities; Sensor() : pose( 0,0,0,0 ), size( 0.02, 0.02, 0.02 ), // teeny transducer @@ -2824,32 +2824,32 @@ namespace Stg void Load( Worldfile* wf, int entity ); }; - /** returns a const reference to a vector of range and reflectance samples */ - const std::vector& GetSensors() const - { return sensors; } - - /** returns a vector of range samples from the indicated sensor - (defaults to zero) */ - const std::vector& GetRanges( unsigned int sensor=0) const - { - if( sensor < sensors.size() ) - return sensors[sensor].ranges; - - PRINT_ERR1( "invalid sensor index specified (%d)", sensor ); - exit(-1); - } + /** returns a const reference to a vector of range and reflectance samples */ + const std::vector& GetSensors() const + { return sensors; } + + /** returns a vector of range samples from the indicated sensor + (defaults to zero) */ + const std::vector& GetRanges( unsigned int sensor=0) const + { + if( sensor < sensors.size() ) + return sensors[sensor].ranges; - /** returns a vector of intensitye samples from the indicated sensor - (defaults to zero) */ - const std::vector& GetIntensities( unsigned int sensor=0) const - { - if( sensor < sensors.size() ) - return sensors[sensor].intensities; - - PRINT_ERR1( "invalid sensor index specified (%d)", sensor ); - exit(-1); - } + PRINT_ERR1( "invalid sensor index specified (%d)", sensor ); + exit(-1); + } + + /** returns a vector of intensitye samples from the indicated sensor + (defaults to zero) */ + const std::vector& GetIntensities( unsigned int sensor=0) const + { + if( sensor < sensors.size() ) + return sensors[sensor].intensities; + PRINT_ERR1( "invalid sensor index specified (%d)", sensor ); + exit(-1); + } + void LoadSensor( Worldfile* wf, int entity ); private: diff --git a/libstageplugin/p_driver.cc b/libstageplugin/p_driver.cc index f39481168..0d2c8663e 100644 --- a/libstageplugin/p_driver.cc +++ b/libstageplugin/p_driver.cc @@ -106,7 +106,7 @@ driver driver ( name "stage" - provides ["position2d:0" "laser:0"] + provides ["position2d:0" "ranger:0"] model "marvin" ) diff --git a/libstageplugin/p_driver.h b/libstageplugin/p_driver.h index f65656f60..67909040f 100644 --- a/libstageplugin/p_driver.h +++ b/libstageplugin/p_driver.h @@ -176,8 +176,8 @@ class InterfaceRanger : public InterfaceModel private: int scan_id; public: - InterfaceLaser( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); - virtual ~InterfaceLaser( void ){ /* TODO: clean up*/ }; + InterfaceRanger( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); + virtual ~InterfaceRanger( void ){ /* TODO: clean up*/ }; virtual int ProcessMessage(QueuePointer & resp_queue, player_msghdr_t* hdr, void* data); diff --git a/libstageplugin/p_ranger.cc b/libstageplugin/p_ranger.cc index 86aa7d60b..f475c3125 100644 --- a/libstageplugin/p_ranger.cc +++ b/libstageplugin/p_ranger.cc @@ -53,49 +53,59 @@ InterfaceRanger::InterfaceRanger( player_devaddr_t addr, void InterfaceRanger::Publish( void ) { ModelRanger* rgr = dynamic_cast(this->mod); - - const std::vector& samples = rgr->GetSamples(); - uint32_t sample_count = samples.size(); - // don't publish anything until we have some real data - if( sample_count == 0 ) - return; - - // Write range data - player_ranger_data_range_t prange; - memset( &prange, 0, sizeof(prange) ); - prange.ranges = &samples[0]; - prange.ranges_count = samples.size(); - - this->driver->Publish(this->addr, - PLAYER_MSGTYPE_DATA, - PLAYER_RANGER_DATA_RANGE, - (void*)&prange, sizeof(prange), NULL); - - - // Write intensity data - player_ranger_data_intns_t pintens; - memset( &pintensity, 0, sizeof(pintens) ); - pntens.intensities = &intens[0]; - - pintens.intensities_count = intens.size(); - this->driver->Publish(this->addr, - PLAYER_MSGTYPE_DATA, - PLAYER_RANGER_DATA_INTNS, - (void*)&pintens, sizeof(pintens), NULL); - - // todo: shouldn't this be published? - - const ModelRanger::Config& cfg = rgr->cfg; - pdata.min_angle = -cfg.fov/2.0; - pdata.max_angle = +cfg.fov/2.0; - pdata.resolution = cfg.fov / cfg.sample_count; - pdata.max_range = cfg.range_bounds.max; - pdata.ranges_count = pdata.intensity_count = cfg.sample_count; - pdata.id = this->scan_id++; + { // limit scope + // Write range data + const std::vector& ranges = rgr->GetRanges(); + uint32_t range_count = ranges.size(); + + player_ranger_data_range_t prange; + memset( &prange, 0, sizeof(prange) ); + + if( range_count > 0 ) + { + prange.ranges = (meters_t*)&ranges[0]; + prange.ranges_count = ranges.size(); + } + + this->driver->Publish(this->addr, + PLAYER_MSGTYPE_DATA, + PLAYER_RANGER_DATA_RANGE, + (void*)&prange, sizeof(prange), NULL); + } + + { // limit scope + // Write intensity data + const std::vector& intensities = rgr->GetIntensities(); + uint32_t intensity_count = intensities.size(); + + player_ranger_data_intns_t pintens; + memset( &pintens, 0, sizeof(pintens) ); + + if( intensity_count > 0 ) + { + pintens.intensities = (double_t*)&intensities[0]; + pintens.intensities_count = intensities.size(); + } + + this->driver->Publish(this->addr, + PLAYER_MSGTYPE_DATA, + PLAYER_RANGER_DATA_INTNS, + (void*)&pintens, sizeof(pintens), NULL); + } - pdata.ranges = new float[pdata.ranges_count]; - pdata.intensity = new uint8_t[pdata.ranges_count]; + // todo: shouldn't this be published? + +// const ModelRanger::Config& cfg = rgr->cfg; +// pdata.min_angle = -cfg.fov/2.0; +// pdata.max_angle = +cfg.fov/2.0; +// pdata.resolution = cfg.fov / cfg.sample_count; +// 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]; } int InterfaceRanger::ProcessMessage(QueuePointer & resp_queue, @@ -104,118 +114,120 @@ int InterfaceRanger::ProcessMessage(QueuePointer & resp_queue, { ModelRanger* mod = (ModelRanger*)this->mod; - // Is it a request to set the ranger's config? + // // Is it a request to set the ranger's config? if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_RANGER_REQ_SET_CONFIG, - this->addr)) - { - - player_ranger_config_t* plc = (player_ranger_config_t*)data; - - if( hdr->size == sizeof(player_ranger_config_t) ) - { - // TODO - // int intensity = plc->intensity; - - ModelRanger::Config cfg = mod->GetConfig(); - - PRINT_DEBUG3( "ranger 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 ) - cfg.resolution = 1; - cfg.interval = (usec_t) ( 1.0E6 / plc->scanning_frequency ); - - PRINT_DEBUG3( "setting ranger 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_RANGER_REQ_SET_CONFIG); - return(0); - } - else - { - PRINT_ERR2("config request len is invalid (%d != %d)", - (int)hdr->size, (int)sizeof(player_ranger_config_t)); - - return(-1); - } - } + PLAYER_RANGER_REQ_SET_CONFIG, + this->addr)) + { + puts( "WARNING: ranger cfg support is broken - fix coming soon" ); + + player_ranger_config_t* plc = (player_ranger_config_t*)data; + + if( hdr->size == sizeof(player_ranger_config_t) ) + { + // // TODO + // // int intensity = plc->intensity; + + // ModelRanger::Config cfg = mod->GetConfig(); + + // PRINT_DEBUG3( "ranger 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 ) + // cfg.resolution = 1; + // cfg.interval = (usec_t) ( 1.0E6 / plc->scanning_frequency ); + + // PRINT_DEBUG3( "setting ranger 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_RANGER_REQ_SET_CONFIG); + return(0); + } + else + { + PRINT_ERR2("config request len is invalid (%d != %d)", + (int)hdr->size, (int)sizeof(player_ranger_config_t)); + return(-1); + } + } // Is it a request to get the ranger's config? else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_RANGER_REQ_GET_CONFIG, - this->addr)) - { - if( hdr->size == 0 ) - { - ModelRanger::Config cfg = mod->GetConfig(); - - player_ranger_config_t plc; - memset(&plc,0,sizeof(plc)); - plc.min_angle = -cfg.fov/2.0; - plc.max_angle = +cfg.fov/2.0; - plc.resolution = cfg.fov / ( cfg.sample_count * cfg.resolution ); - plc.max_range = cfg.range_bounds.max; - plc.range_res = 1.0; // todo - plc.intensity = 1; // todo - plc.scanning_frequency = 1.0E6 / cfg.interval; - - this->driver->Publish(this->addr, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, - PLAYER_RANGER_REQ_GET_CONFIG, - (void*)&plc, sizeof(plc), NULL); - return(0); - } - else - { - PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0); - return(-1); - } - } - // Is it a request to get the ranger's geom? - else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_RANGER_REQ_GET_GEOM, - this->addr)) - { - if(hdr->size == 0) - { - Geom geom = this->mod->GetGeom(); - - Pose pose = this->mod->GetPose(); - - // fill in the geometry data formatted player-like - player_ranger_geom_t pgeom; - pgeom.pose.px = pose.x; - pgeom.pose.py = pose.y; - 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_RANGER_REQ_GET_GEOM, - (void*)&pgeom, sizeof(pgeom), NULL); - return(0); - } - else - { - PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0); - return(-1); - } - } - + PLAYER_RANGER_REQ_GET_CONFIG, + this->addr)) + { + puts( "WARNING: ranger GET CFG support is broken - fix coming soon" ); + + if( hdr->size == 0 ) + { + + // ModelRanger::Config cfg = mod->GetConfig(); + + // player_ranger_config_t plc; + // memset(&plc,0,sizeof(plc)); +// plc.min_angle = -cfg.fov/2.0; +// plc.max_angle = +cfg.fov/2.0; +// plc.resolution = cfg.fov / ( cfg.sample_count * cfg.resolution ); +// plc.max_range = cfg.range_bounds.max; +// plc.range_res = 1.0; // todo +// plc.intensity = 1; // todo +// plc.scanning_frequency = 1.0E6 / cfg.interval; + +// this->driver->Publish(this->addr, resp_queue, +// PLAYER_MSGTYPE_RESP_ACK, +// PLAYER_RANGER_REQ_GET_CONFIG, +// (void*)&plc, sizeof(plc), NULL); + return(0); + } + else + { + PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0); + return(-1); + } + } + else // Is it a request to get the ranger's geom? + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_RANGER_REQ_GET_GEOM, + this->addr)) + { + if(hdr->size == 0) + { + Geom geom = this->mod->GetGeom(); + Pose pose = this->mod->GetPose(); + + // fill in the geometry data formatted player-like + player_ranger_geom_t pgeom; + pgeom.pose.px = pose.x; + pgeom.pose.py = pose.y; + 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_RANGER_REQ_GET_GEOM, + (void*)&pgeom, sizeof(pgeom), NULL); + return(0); + } + else + { + PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0); + return(-1); + } + } + // Don't know how to handle this message. PRINT_WARN2( "stage ranger doesn't support message %d:%d.", - hdr->type, hdr->subtype); + hdr->type, hdr->subtype); return(-1); }