fixing up more examples

CodeFinder2 · Nov 26, 2010 · 791aac4 · 791aac4
1 parent e7ae061
commit 791aac4
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 14 deletions.
diff --git 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
@@ -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<ModelLaser::Sample>& scan = mod->GetSamples();
+	std::vector<meters_t>& 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
@@ -2756,9 +2756,20 @@ namespace Stg
 	 const std::vector<Sensor>& 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<meters_t>& 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<meters_t>& GetRangesMutable( unsigned int sensor=0) 
 	 { 
 		if( sensor < sensors.size() )
 		  return sensors[sensor].ranges;

diff --git 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