From 7aa3b14f3fa752047c934293325584edb8b4ef95 Mon Sep 17 00:00:00 2001 From: Richard Vaughan Date: Thu, 22 Jul 2010 13:10:04 +0100 Subject: [PATCH] simplified ranger API --- examples/ctrl/fasr.cc | 15 +- examples/ctrl/fasr2.cc | 15 +- examples/ctrl/wander.cc | 12 +- libstage/model_ranger.cc | 267 ++++++++++++----------------- libstage/stage.hh | 73 ++++---- libstageplugin/CMakeLists.txt | 6 +- worlds/benchmark/expand_pioneer.cc | 16 +- worlds/benchmark/expand_swarm.cc | 18 +- 8 files changed, 176 insertions(+), 246 deletions(-) diff --git a/examples/ctrl/fasr.cc b/examples/ctrl/fasr.cc index f4f087c74..ab3aeab12 100644 --- a/examples/ctrl/fasr.cc +++ b/examples/ctrl/fasr.cc @@ -66,8 +66,8 @@ class Robot Model* source, Model* sink ) : pos(pos), - ranger( (ModelRanger*)pos->GetChild( "ranger:0" )), laser( (ModelRanger*)pos->GetChild( "ranger:1" )), + ranger( (ModelRanger*)pos->GetChild( "ranger:0" )), fiducial( (ModelFiducial*)pos->GetUnusedModelOfType( "fiducial" )), //blobfinder( (ModelBlobfinder*)pos->GetUnusedModelOfType( "blobfinder" )), //gripper( (ModelGripper*)pos->GetUnusedModelOfType( "gripper" )), @@ -212,33 +212,32 @@ class Robot double minright = 1e6; // Get the data from the first sensor of the laser - const std::vector& scan = - laser->GetSamples(); + const std::vector& scan = laser->GetRanges(); uint32_t sample_count = scan.size(); for (uint32_t i = 0; i < sample_count; i++) { - if( verbose ) printf( "%.3f ", scan[i].range ); + if( verbose ) printf( "%.3f ", scan[i] ); if( (i > (sample_count/4)) && (i < (sample_count - (sample_count/4))) - && scan[i].range < minfrontdistance) + && scan[i] < minfrontdistance) { if( verbose ) puts( " obstruction!" ); obstruction = true; } - if( scan[i].range < stopdist ) + if( scan[i] < stopdist ) { if( verbose ) puts( " stopping!" ); stop = true; } if( i > sample_count/2 ) - minleft = std::min( minleft, scan[i].range ); + minleft = std::min( minleft, scan[i] ); else - minright = std::min( minright, scan[i].range ); + minright = std::min( minright, scan[i] ); } if( verbose ) diff --git a/examples/ctrl/fasr2.cc b/examples/ctrl/fasr2.cc index 40a0b83e7..ae38c5ec4 100644 --- a/examples/ctrl/fasr2.cc +++ b/examples/ctrl/fasr2.cc @@ -563,7 +563,7 @@ class Robot const std::vector& sensors = sonar->GetSensors(); for( unsigned int s = BACK_SENSOR_FIRST; s <= BACK_SENSOR_LAST; ++s ) - if( sensors[s].samples[0].range < wait_distance) + if( sensors[s].ranges[0] < wait_distance) { pos->Say( "Waiting..." ); pos->SetXSpeed( 0.0 ); @@ -616,34 +616,33 @@ class Robot // Get the data //const std::vector& scan = laser->GetSamples(); - const std::vector& scan = - laser->GetSensors()[0].samples; + const std::vector& scan = laser->GetRanges(); uint32_t sample_count = scan.size(); for (uint32_t i = 0; i < sample_count; i++) { - if( verbose ) printf( "%.3f ", scan[i].range ); + if( verbose ) printf( "%.3f ", scan[i] ); if( (i > (sample_count/4)) && (i < (sample_count - (sample_count/4))) - && scan[i].range < minfrontdistance) + && scan[i] < minfrontdistance) { if( verbose ) puts( " obstruction!" ); obstruction = true; } - if( scan[i].range < stopdist ) + if( scan[i] < stopdist ) { if( verbose ) puts( " stopping!" ); stop = true; } if( i > sample_count/2 ) - minleft = std::min( minleft, scan[i].range ); + minleft = std::min( minleft, scan[i] ); else - minright = std::min( minright, scan[i].range ); + minright = std::min( minright, scan[i] ); } if( verbose ) diff --git a/examples/ctrl/wander.cc b/examples/ctrl/wander.cc index 77aadac42..93a26c7e4 100644 --- a/examples/ctrl/wander.cc +++ b/examples/ctrl/wander.cc @@ -54,7 +54,7 @@ extern "C" int Init( Model* mod, CtrlArgs* args ) int LaserUpdate( Model* mod, robot_t* robot ) { // get the data - const std::vector& scan = robot->laser->GetSamples(); + const std::vector& scan = robot->laser->GetRanges(); uint32_t sample_count = scan.size(); if( sample_count < 1 ) return 0; @@ -70,26 +70,26 @@ int LaserUpdate( Model* mod, robot_t* robot ) for (uint32_t i = 0; i < sample_count; i++) { - if( verbose ) printf( "%.3f ", scan[i].range ); + if( verbose ) printf( "%.3f ", scan[i] ); if( (i > (sample_count/3)) && (i < (sample_count - (sample_count/3))) - && scan[i].range < minfrontdistance) + && scan[i] < minfrontdistance) { if( verbose ) puts( " obstruction!" ); obstruction = true; } - if( scan[i].range < stopdist ) + if( scan[i] < stopdist ) { if( verbose ) puts( " stopping!" ); stop = true; } if( i > sample_count/2 ) - minleft = std::min( minleft, scan[i].range ); + minleft = std::min( minleft, scan[i] ); else - minright = std::min( minright, scan[i].range ); + minright = std::min( minright, scan[i] ); } if( verbose ) diff --git a/libstage/model_ranger.cc b/libstage/model_ranger.cc index 366b64f10..3852e5da4 100644 --- a/libstage/model_ranger.cc +++ b/libstage/model_ranger.cc @@ -90,9 +90,8 @@ static const Color RANGER_CONFIG_COLOR( 0,0,0.5 ); //static const Color RANGER_GEOM_COLOR( 1,0,1 ); // static members -Option ModelRanger::Vis::showData( "Ranger ranges", "show_ranger", "", true, NULL ); Option ModelRanger::Vis::showTransducers( "Ranger transducers", "show_ranger_transducers", "", false, NULL ); -Option ModelRanger::Vis::showArea( "Ranger scans", "show_ranger", "", true, NULL ); +Option ModelRanger::Vis::showArea( "Ranger area", "show_ranger_ranges", "", true, NULL ); Option ModelRanger::Vis::showStrikes( "Ranger strikes", "show_ranger_strikes", "", false, NULL ); Option ModelRanger::Vis::showFov( "Ranger FOV", "show_ranger_fov", "", false, NULL ); Option ModelRanger::Vis::showBeams( "Ranger beams", "show_ranger_beams", "", false, NULL ); @@ -119,19 +118,6 @@ ModelRanger::ModelRanger( World* world, this->SetGeom( Geom( Pose(), RANGER_SIZE )); - // sensors.resize( RANGER_SENSORCOUNT ); - -// for( unsigned int c(0); cReadAngle( entity, "fov", s.cfg.fov ); - s.cfg.sample_count = wf->ReadInt( entity, "samples", s.cfg.sample_count ); + s.Load( wf, entity ); sensors.push_back(s); } + +void ModelRanger::Sensor::Load( Worldfile* wf, int entity ) +{ + //static int c=0; + // printf( "ranger %s loading sensor %d\n", token.c_str(), c++ ); + + pose.Load( wf, entity, "pose" ); + size.Load( wf, entity, "size" ); + range.Load( wf, entity, "range" ); + col.Load( wf, entity ); + fov = wf->ReadAngle( entity, "fov", fov ); + sample_count = wf->ReadInt( entity, "samples", sample_count ); + + ranges.clear(); + intensities.clear(); +} + void ModelRanger::Load( void ) { Model::Load(); @@ -198,31 +195,31 @@ void ModelRanger::Update( void ) void ModelRanger::Sensor::Update( ModelRanger* mod ) { //printf( "update sensor\n" ); - - samples.resize( cfg.sample_count ); - double bearing( cfg.sample_count > 1 ? -cfg.fov/2.0 : 0.0 ); + ranges.resize( sample_count ); + intensities.resize( sample_count ); + + double bearing( sample_count > 1 ? -fov/2.0 : 0.0 ); // make the first and last rays exactly at the extremes of the FOV - double sample_incr( cfg.fov / std::max(cfg.sample_count-1, (unsigned int)1) ); + double sample_incr( fov / std::max(sample_count-1, (unsigned int)1) ); // find the global origin of our first emmitted ray - Pose rayorg( cfg.pose );//mod->GetPose() ); + Pose rayorg( pose );//mod->GetPose() ); rayorg.a += bearing; - rayorg.z += cfg.size.z/2.0; + rayorg.z += size.z/2.0; rayorg = mod->LocalToGlobal(rayorg); // set up a ray to trace - Ray ray( mod, rayorg, cfg.range.max, ranger_match, NULL, true ); + Ray ray( mod, rayorg, range.max, ranger_match, NULL, true ); World* world = mod->GetWorld(); // trace the ray, incrementing its heading for each sample - for( unsigned int t(0); tRaytrace( ray ) ); - samples[t].range = r.range; - - samples[t].reflectance = r.mod ? r.mod->vis.ranger_return : 0.0; + ranges[t] = r.range; + intensities[t] = r.mod ? r.mod->vis.ranger_return : 0.0; // point the ray to the next angle ray.origin.a += sample_incr; @@ -230,14 +227,14 @@ void ModelRanger::Sensor::Update( ModelRanger* mod ) //printf( "ranger %s sensor %p pose %s sample %d range %.2f ref %.2f\n", // mod->Token(), // this, - // cfg.pose.String().c_str(), + // pose.String().c_str(), // t, - // samples[t].range, - // samples[t].reflectance ); + // ranges[t].range, + // ranges[t].reflectance ); } } -std::string ModelRanger::Sensor::Cfg::String() +std::string ModelRanger::Sensor::String() const { char buf[256]; snprintf( buf, 256, "[ samples %u, range [%.2f %.2f] ]", @@ -247,9 +244,8 @@ std::string ModelRanger::Sensor::Cfg::String() void ModelRanger::Sensor::Visualize( ModelRanger::Vis* vis, ModelRanger* rgr ) const { - size_t sample_count( samples.size() ); + size_t sample_count( this->sample_count ); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); //glTranslatef( 0,0, ranger->GetGeom().size.z/2.0 ); // shoot the ranger beam out at the right height // pack the ranger hit points into a vertex array for fast rendering @@ -260,46 +256,51 @@ void ModelRanger::Sensor::Visualize( ModelRanger::Vis* vis, ModelRanger* rgr ) c pts[0] = 0.0; pts[1] = 0.0; - Color c( cfg.col ); - c.a = 0.15; // transparent version of sensor color - rgr->PushColor( c ); glDepthMask( GL_FALSE ); glPointSize( 2 ); glPushMatrix(); - Gl::pose_shift( cfg.pose ); - - if( sample_count == 1 ) + Gl::pose_shift( pose ); + + if( vis->showTransducers ) { - // only one sample, so we fake up some beam width for beauty - const double sidelen = samples[0].range; - const double da = cfg.fov/2.0; + // draw the sensor body as a rectangle + rgr->PushColor( col ); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + glRectf( -size.x/2.0, -size.y/2.0, size.x/2.0, size.y/2.0 ); + rgr->PopColor(); + } - sample_count = 3; - - pts[2] = sidelen*cos(-da ); - pts[3] = sidelen*sin(-da ); - - pts[4] = sidelen*cos(+da ); - pts[5] = sidelen*sin(+da ); - } - else + Color c( col ); + c.a = 0.15; // transparent version of sensor color + rgr->PushColor( c ); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + if( ranges.size() ) // if we have some data { - for( unsigned int s(0); s 0 ) + pts[2] = sidelen*cos(-da ); + pts[3] = sidelen*sin(-da ); + + pts[4] = sidelen*cos(+da ); + pts[5] = sidelen*sin(+da ); + } + else + { + for( unsigned int s(0); sshowStrikes ) { + // TODO - paint the stike point in a color based on intensity +// // if the sample is unusually bright, draw a little blob +// if( intensities[s] > 0.0 ) +// { +// // this happens rarely so we can do it in immediate mode +// glBegin( GL_POINTS ); +// glVertex2f( pts[2*s+2], pts[2*s+3] ); +// glEnd(); +// } + // draw the beam strike points c.a = 0.8; rgr->PushColor( c ); @@ -325,9 +336,9 @@ void ModelRanger::Sensor::Visualize( ModelRanger::Vis* vis, ModelRanger* rgr ) c { for( unsigned int s(0); sRegisterOption( &showStrikes ); world->RegisterOption( &showFov ); world->RegisterOption( &showBeams ); - world->RegisterOption( &showData ); world->RegisterOption( &showTransducers ); } @@ -424,73 +434,6 @@ void ModelRanger::Vis::Visualize( Model* mod, Camera* cam ) const unsigned int sensor_count = sensors.size(); - if( 0 )//showData ) - { - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - glDepthMask( GL_FALSE ); - - // calculate a triangle for each non-zero sensor range - for( unsigned int s=0; sPushColor( sensor.cfg.col ); - - //printf( "s = %u samples %u", s, sample_count ); - - // sensor FOV - double da = sensor.cfg.fov/2.0; - - std::vector pts( sample_count * 9 ); - - // calculate a triangle for each non-zero sensor range - for( unsigned int t=0; tPopColor(); - } - - puts(""); - - // restore state - glDepthMask( GL_TRUE ); - } - if( showTransducers ) { glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); @@ -502,12 +445,12 @@ void ModelRanger::Vis::Visualize( Model* mod, Camera* cam ) glPointSize( 4 ); glBegin( GL_POINTS ); - glVertex3f( rngr.cfg.pose.x, rngr.cfg.pose.y, rngr.cfg.pose.z ); + glVertex3f( rngr.pose.x, rngr.pose.y, rngr.pose.z ); glEnd(); char buf[8]; snprintf( buf, 8, "%d", s ); - Gl::draw_string( rngr.cfg.pose.x, rngr.cfg.pose.y, rngr.cfg.pose.z, buf ); + Gl::draw_string( rngr.pose.x, rngr.pose.y, rngr.pose.z, buf ); } ranger->PopColor(); diff --git a/libstage/stage.hh b/libstage/stage.hh index bb1695cc7..878020be9 100644 --- a/libstage/stage.hh +++ b/libstage/stage.hh @@ -2777,7 +2777,6 @@ namespace Stg static Option showStrikes; static Option showFov; static Option showBeams; - static Option showData; static Option showTransducers; Vis( World* world ); @@ -2788,59 +2787,55 @@ namespace Stg class Sensor { public: - class Cfg { - public: - Pose pose; - Size size; - Bounds range; - radians_t fov; - unsigned int sample_count; - Color col; - - std::string String(); - - Cfg() : pose( 0,0,0,0 ), - size( 0.02, 0.02, 0.02 ), // teeny transducer - range( 0.0, 5.0 ), - fov( 0.1 ), - sample_count(1), - col( 0,1,0,0.3 ) {} - - } cfg; - - class Sample - { - public: - meters_t range; - float reflectance; - - Sample() : range(0.0), reflectance(0.0) {} - }; + Pose pose; + Size size; + Bounds range; + radians_t fov; + unsigned int sample_count; + Color col; - std::vector samples; + std::vector ranges; + std::vector intensities; - void Update( ModelRanger* rgr ); + Sensor() : pose( 0,0,0,0 ), + size( 0.02, 0.02, 0.02 ), // teeny transducer + range( 0.0, 5.0 ), + fov( 0.1 ), + sample_count(1), + col( 0,1,0,0.3 ) + {} + void Update( ModelRanger* rgr ); void Visualize( Vis* vis, ModelRanger* rgr ) const; + std::string String() const; + void Load( Worldfile* wf, int entity ); }; /** returns a const reference to a vector of range and reflectance samples */ - const std::vector& GetSensors() const + const std::vector& GetSensors() const { return sensors; } /** returns a vector of range samples from the indicated sensor (defaults to zero) */ - const std::vector& GetSamples( unsigned int sensor=0) const + const std::vector& GetRanges( unsigned int sensor=0) const { if( sensor < sensors.size() ) - return sensors[sensor].samples; + return sensors[sensor].ranges; + + 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); } - - /** returns a mutable reference to a vector of range and reflectance samples */ - //std::vector& GetSamples(); void LoadSensor( Worldfile* wf, int entity ); @@ -2851,9 +2846,7 @@ namespace Stg virtual void Startup(); virtual void Shutdown(); - virtual void Update(); - //virtual void DataVisualize( Camera* cam ); - + virtual void Update(); }; // BLINKENLIGHT MODEL ---------------------------------------------------- diff --git a/libstageplugin/CMakeLists.txt b/libstageplugin/CMakeLists.txt index b1701c119..ef9e6e766 100644 --- a/libstageplugin/CMakeLists.txt +++ b/libstageplugin/CMakeLists.txt @@ -10,10 +10,9 @@ set( stagepluginSrcs p_blobfinder.cc p_gripper.cc p_simulation.cc - p_laser.cc p_fiducial.cc p_position.cc - p_sonar.cc + p_ranger.cc p_speech.cc p_graphics.cc stg_time.cc @@ -21,13 +20,10 @@ set( stagepluginSrcs add_library( stageplugin MODULE ${stagepluginSrcs} ) - - target_link_libraries( stageplugin stage ${PLAYER_LIBRARIES} ${OPENGL_LIBRARIES} - ) set_source_files_properties( ${stagepluginSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) diff --git a/worlds/benchmark/expand_pioneer.cc b/worlds/benchmark/expand_pioneer.cc index a05360170..8ef1f02ec 100644 --- a/worlds/benchmark/expand_pioneer.cc +++ b/worlds/benchmark/expand_pioneer.cc @@ -68,8 +68,8 @@ int RangerUpdate( ModelRanger* rgr, robot_t* robot ) FOR_EACH( it, sensors ) { const ModelRanger::Sensor& s = *it; - dx += s.samples[0].range * cos( s.cfg.pose.a ); - dy += s.samples[0].range * sin( s.cfg.pose.a ); + dx += s.ranges[0] * cos( s.pose.a ); + dy += s.ranges[0] * sin( s.pose.a ); //printf( "sensor %d angle= %.2f\n", s, rgr->sensors[s].pose.a ); } @@ -85,12 +85,12 @@ int RangerUpdate( ModelRanger* rgr, robot_t* robot ) //printf( "resultant %.2f turn_speed %.2f\n", resultant_angle, turn_speed ); // if the front is clear, drive forwards - if( (sensors[3].samples[0].range > SAFE_DIST) && // forwards - (sensors[4].samples[0].range > SAFE_DIST) && - (sensors[5].samples[0].range > SAFE_DIST/2.0) && // - (sensors[6].samples[0].range > SAFE_DIST/4.0) && - (sensors[2].samples[0].range > SAFE_DIST/2.0) && - (sensors[1].samples[0].range > SAFE_DIST/4.0) && + if( (sensors[3].ranges[0] > SAFE_DIST) && // forwards + (sensors[4].ranges[0] > SAFE_DIST) && + (sensors[5].ranges[0] > SAFE_DIST/2.0) && // + (sensors[6].ranges[0] > SAFE_DIST/4.0) && + (sensors[2].ranges[0] > SAFE_DIST/2.0) && + (sensors[1].ranges[0] > SAFE_DIST/4.0) && (fabs( resultant_angle ) < SAFE_ANGLE) ) { forward_speed = VSPEED; diff --git a/worlds/benchmark/expand_swarm.cc b/worlds/benchmark/expand_swarm.cc index b7eb79bf4..a5fb3db0b 100644 --- a/worlds/benchmark/expand_swarm.cc +++ b/worlds/benchmark/expand_swarm.cc @@ -66,8 +66,8 @@ int RangerUpdate( ModelRanger* rgr, robot_t* robot ) FOR_EACH( it, sensors ) { const ModelRanger::Sensor& s = *it; - dx += s.samples[0].range * cos( s.cfg.pose.a ); - dy += s.samples[0].range * sin( s.cfg.pose.a ); + dx += s.ranges[0] * cos( s.pose.a ); + dy += s.ranges[0] * sin( s.pose.a ); //printf( "sensor %d angle= %.2f\n", s, rgr->sensors[s].pose.a ); } @@ -83,15 +83,15 @@ int RangerUpdate( ModelRanger* rgr, robot_t* robot ) //printf( "resultant %.2f turn_speed %.2f\n", resultant_angle, turn_speed ); // if the front is clear, drive forwards - if( (sensors[0].samples[0].range > SAFE_DIST) && + if( (sensors[0].ranges[0] > SAFE_DIST) && - (sensors[1].samples[0].range > SAFE_DIST/1.5) && - (sensors[2].samples[0].range > SAFE_DIST/3.0) && - (sensors[3].samples[0].range > SAFE_DIST/5.0) && + (sensors[1].ranges[0] > SAFE_DIST/1.5) && + (sensors[2].ranges[0] > SAFE_DIST/3.0) && + (sensors[3].ranges[0] > SAFE_DIST/5.0) && - (sensors[9].samples[0].range > SAFE_DIST/5.0) && - (sensors[10].samples[0].range > SAFE_DIST/3.0) && - (sensors[11].samples[0].range > SAFE_DIST/1.5) && + (sensors[9].ranges[0] > SAFE_DIST/5.0) && + (sensors[10].ranges[0] > SAFE_DIST/3.0) && + (sensors[11].ranges[0] > SAFE_DIST/1.5) && (fabs( resultant_angle ) < SAFE_ANGLE) ) { forward_speed = VSPEED;