Fixing and testing

- fixed documentation header in model_ranger.cc
- added test scene worlds/sensor_noise_demo.world
- noise values are loaded as 3-tuple
- refactored a bit
- noise is not applied for readings with maximum range

libstage/model_ranger.cc

@@ -32,6 +32,7 @@
    size [  x y z ]
    fov a
    range [min max]
+   noise [range_const range_prop angular]
    )
 
    # generic model properties with non-default values
@@ -57,6 +58,8 @@
    - sview [float float float]
    - [range_min range_max fov] 
    - minimum range and maximum range in meters, field of view angle in degrees. Currently fov has no effect on the sensor model, other than being shown in the confgiuration graphic for the ranger device.
+   - noise [range_const range_prop angular]
+   - noise for range readings: constant noise in meters, proportional noise, angular noise in degrees
    - sview[\<transducer index\>] [float float float]
    - per-transducer version of the sview property. Overrides the common setting.
 
@@ -145,9 +148,8 @@ void ModelRanger::Sensor::Load( Worldfile* wf, int entity )
   range.Load( wf, entity, "range" );
   fov = wf->ReadAngle( entity, "fov", fov );
   sample_count = wf->ReadInt( entity, "samples", sample_count );
-  angleNoise = wf->ReadFloat( entity, "anoise", angleNoise);
-  rangeNoise = wf->ReadFloat( entity, "rnoise", rangeNoise);
-  rangeNoiseConst = wf->ReadFloat( entity, "rcnoise", rangeNoiseConst);
+
+  wf->ReadTuple( entity, "noise", 0, 3, "lfa", &range_noise_const, &range_noise, &angle_noise);
   color.Load( wf, entity );
 }
 
@@ -234,12 +236,16 @@ void ModelRanger::Sensor::Update( ModelRanger* mod )
   for( size_t t(0); t<sample_count; t++ )
     {
 	  float savedAngle = ray.origin.a;
-	  float distortedAngle = ray.origin.a + sample_incr*angleNoise*simpleNoise()*0.5;
+	  float distortedAngle = ray.origin.a + sample_incr*angle_noise*simpleNoise()*0.5;
 	  ray.origin.a = distortedAngle;
       const RaytraceResult res = mod->world->Raytrace( ray);
       ray.origin.a = savedAngle;
 
-      ranges[t] = res.range + res.range*rangeNoise*simpleNoise() + generateGaussianNoise(rangeNoiseConst);
+      /// Apply noise only if it is in valid range
+      if(res.range < this->range.max)
+    	  ranges[t] = res.range + res.range*range_noise*simpleNoise() + generateGaussianNoise(range_noise_const);
+      else
+    	  ranges[t] = res.range;
 
       intensities[t] = res.mod ? res.mod->vis.ranger_return : 0.0;
       bearings[t] = start_angle + ((double)t) * sample_incr;

libstage/stage.hh

@@ -2749,9 +2749,9 @@ namespace Stg
       Size size;
       Bounds range;
       radians_t fov;
-      double angleNoise;			// specify variance for ranger angle
-      double rangeNoise;			// specify variance for range readings
-      double rangeNoiseConst;		// specify fariance for constant noise (not depending on range)
+      double angle_noise;			//< variance for ranger angle
+      double range_noise;			//< variance for range readings
+      double range_noise_const;		//< fariance for constant noise (not depending on range)
       unsigned int sample_count;
       Color color;
 
@@ -2763,9 +2763,9 @@ namespace Stg
 		 size( 0.02, 0.02, 0.02 ), // teeny transducer
 		 range( 0.0, 5.0 ),
 		 fov( 0.1 ),
-		 angleNoise(0.0),
-		 rangeNoise(0.0),
-		 rangeNoiseConst(0.0),
+		 angle_noise(0.0),
+		 range_noise(0.0),
+		 range_noise_const(0.0),
 		 sample_count(1),
 		 color( Color(0,0,1,0.15)),
 		 ranges(),

worlds/sensor_noise_demo.world

@@ -0,0 +1,51 @@
+# simple.world - basic world file example
+# Authors: Richard Vaughan
+# $Id: ctrl_demo.world,v 1.2 2008-03-04 02:21:49 rtv Exp $
+
+include "pioneer.inc"
+include "map.inc"
+include "sick.inc"
+
+window
+( 
+  size [ 500.000 500.000 ] 
+  center [-0.077 -0.231] 
+  rotate [ 0 0 ]
+  scale 26.000 
+  show_data 1 # make sure we can see the effect of the controller
+)
+
+floorplan
+( 
+  name "cave"
+  size [16.000 16.000 0.500]
+  bitmap "bitmaps/cave.png"
+)
+
+define SICK_noisy ranger
+(
+	color "blue"
+  	size [ 0.156 0.155 0.19 ] # dimensions from LMS200 data sheet
+
+	sensor
+	(				
+	  # factory settings for LMS200	
+	  range [ 0.0 8.0 ]
+	  pose [ 0 0 0.1 0 ]
+	  fov 180
+	  samples 180
+	  #samples 90 # still useful but much faster to compute
+	  color_rgba [0 0 1 0.15]
+	  noise [ 0.01 0.00 0.5 ]
+	)
+)
+
+pioneer2dx
+(
+  pose [ -7.000 -7.000 0 45.000 ]
+  SICK_noisy
+  ( 
+    alwayson 1 # don't wait for a subscriber
+  ) 
+)
+