diff --git a/examples/ctrl/CMakeLists.txt b/examples/ctrl/CMakeLists.txt index 04d36a94f..3fb74ed96 100644 --- a/examples/ctrl/CMakeLists.txt +++ b/examples/ctrl/CMakeLists.txt @@ -6,8 +6,9 @@ SET( PLUGINS wander # wander_pioneer pioneer_flocking - rasterize - dynamic + rasterize + lasernoise + dynamic ) # need plaer's wavefront planning library for this one diff --git a/examples/ctrl/lasernoise.cc b/examples/ctrl/lasernoise.cc index 8702df885..91b92e1c0 100644 --- a/examples/ctrl/lasernoise.cc +++ b/examples/ctrl/lasernoise.cc @@ -1,8 +1,8 @@ /* - File lasernoise.cc: laser noise plugin demo for Stage + File rangernoise.cc: ranger noise plugin demo for Stage Author: Richard Vaughan Date: 3 March 2008 - CVS: $Id: lasernoise.cc,v 1.1 2008-03-04 02:09:56 rtv Exp $ + CVS: $Id: rangernoise.cc,v 1.1 2008-03-04 02:09:56 rtv Exp $ */ #include "stage.hh" @@ -20,17 +20,17 @@ double simple_normal_deviate( double mean, double stddev ) return ( stddev * (x - 6.0) + mean ); } -// process the laser data -int LaserUpdate( ModelLaser* mod, void* dummy ) +// process the ranger data +int RangerUpdate( ModelRanger* mod, void* dummy ) { // get the data uint32_t sample_count=0; - const std::vector& scan = mod->GetSamples(); + std::vector& scan = mod->GetRangesMutable(); if( scan.size()>0 ) FOR_EACH( it, scan ) - it->range *= simple_normal_deviate( 1.0, DEVIATION ); + *it *= simple_normal_deviate( 1.0, DEVIATION ); return 0; // run again } @@ -39,10 +39,10 @@ int LaserUpdate( ModelLaser* mod, void* dummy ) // the model that gets called just after the sensor update is done. extern "C" int Init( Model* mod ) { - mod->AddUpdateCallback( (model_callback_t)LaserUpdate, NULL ); - + mod->AddCallback( Model::CB_UPDATE, (model_callback_t)RangerUpdate, NULL ); + // add this so we can see the effects immediately, without needing - // anyone else to subscribe to the laser + // anyone else to subscribe to the ranger mod->Subscribe(); return 0; // ok diff --git a/libstage/stage.hh b/libstage/stage.hh index 88c6854c0..b5007b91c 100644 --- a/libstage/stage.hh +++ b/libstage/stage.hh @@ -2756,9 +2756,20 @@ namespace Stg const std::vector& GetSensors() const { return sensors; } - /** returns a vector of range samples from the indicated sensor - (defaults to zero) */ + /** returns a const reference to the 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 mutable reference to the vector of range samples from + the indicated sensor (defaults to zero). Mutating the range data in place allows controllers to act as filters. */ + std::vector& GetRangesMutable( unsigned int sensor=0) { if( sensor < sensors.size() ) return sensors[sensor].ranges; diff --git a/worlds/simple.world b/worlds/simple.world index 9b92e9d32..8ce96e755 100644 --- a/worlds/simple.world +++ b/worlds/simple.world @@ -41,8 +41,16 @@ pioneer2dx pose [ -6.432 -5.895 0 45.000 ] # pioneer2dx's sonars will be ranger:0 and the laser will be ranger:1 - sicklaser( pose [ 0 0 0 0 ] ) + sicklaser( + pose [ 0 0 0 0 ] + + # uncomment the ctrl line to filter the data through + # the plugin controller implemented in examples/ctr/lasernoise.cc + # ctrl "lasernoise" + ) + # demonstrate a plugin controller, implemented in examples/ctrl/wander.cc + # you probably should comment this out when using simple.cfg with Player ctrl "wander" # report error-free position in world coordinates