Browse files

added laser noise generating plugin as a demo

  • Loading branch information...
1 parent 220e84a commit 2ba0e4489f9dc5c6e144be94d08f50d5767dac35 rtv committed Mar 4, 2008
Showing with 190 additions and 79 deletions.
  1. +20 −0 CMakeLists.txt
  2. +6 −2 examples/ctrl/Makefile.am
  3. +44 −0 examples/ctrl/lasernoise.cc
  4. +23 −48 libstage/CMakeLists.txt
  5. +13 −1 libstage/model_laser.cc
  6. +5 −7 libstage/stage.hh
  7. +25 −21 worlds/889.world
  8. +54 −0 worlds/ctrl_demo.world
View
20 CMakeLists.txt
@@ -1,2 +1,22 @@
PROJECT(Stage)
+
+include(FindPkgConfig)
+pkg_check_modules( GLIB gdk-pixbuf-2.0 )
+
+find_package(OPENGL REQUIRED)
+
+if(APPLE)
+ set(FLTK_INCLUDE_DIR /opt/local/include) # MacPorts' install path
+endif(APPLE)
+find_package(FLTK REQUIRED)
+
+include_directories( .
+ replace
+ libstage
+ ${GLIB_INCLUDE_DIRS}
+ ${FLTK_INCLUDE_DIR}
+ ${OPENGL_INCLUDE_DIR}
+)
+
ADD_SUBDIRECTORY(libstage)
+ADD_SUBDIRECTORY(examples)
View
8 examples/ctrl/Makefile.am
@@ -1,10 +1,10 @@
-# $Id: Makefile.am,v 1.2 2008-03-03 06:02:25 rtv Exp $
+# $Id: Makefile.am,v 1.3 2008-03-04 02:09:56 rtv Exp $
# system-wide compile flags - target-specific flags are added to each target below
AM_CPPFLAGS = -Wall -I. -I../../libstage -I$(top_srcdir)/replace @PIXBUF_CFLAGS@ @FLTK_CFLAGS@ -DRGBFILE=\"$(RGB)\"
-lib_LTLIBRARIES = wander.la fasr.la source.la sink.la
+lib_LTLIBRARIES = wander.la fasr.la source.la sink.la lasernoise.la
# WANDER CONTROLLER
wander_la_SOURCES = wander.cc ../../libstage/stage.hh
@@ -21,3 +21,7 @@ source_la_LDFLAGS = -module
# resource sink
sink_la_SOURCES = sink.cc ../../libstage/stage.hh
sink_la_LDFLAGS = -module
+
+# laser noise
+lasernoise_la_SOURCES = lasernoise.cc ../../libstage/stage.hh
+lasernoise_la_LDFLAGS = -module
View
44 examples/ctrl/lasernoise.cc
@@ -0,0 +1,44 @@
+/*
+ File lasernoise.cc: laser 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 $
+*/
+
+#include "stage.hh"
+using namespace Stg;
+
+const double DEVIATION = 0.05;
+
+double SimpleNormalDeviate( double mean, double stddev )
+{
+ double x = 0.0;
+
+ for( int i=0; i<12; i++ )
+ x += rand()/(RAND_MAX+1.0);
+
+ return ( stddev * (x - 6.0) + mean );
+}
+
+// process the laser data
+int LaserUpdate( StgModelLaser* mod, void* dummy )
+{
+ // get the data
+ uint32_t sample_count=0;
+ stg_laser_sample_t* scan = mod->GetSamples( &sample_count );
+
+ if( scan )
+ for( int i=0; i<sample_count; i++ )
+ scan[i].range *= SimpleNormalDeviate( 1.0, DEVIATION );
+
+ return 0; // run again
+}
+
+// Stage calls this when the model starts up. we just add a callback to
+// the model that gets called just after the sensor update is done.
+extern "C" int Init( StgModel* mod )
+{
+ mod->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, NULL );
+ return 0; // ok
+}
+
View
71 libstage/CMakeLists.txt
@@ -1,59 +1,34 @@
-include(FindPkgConfig)
-pkg_check_modules( GLIB gdk-pixbuf-2.0 )
-
-find_package(OPENGL REQUIRED)
-
-if(APPLE)
- set(FLTK_INCLUDE_DIR /opt/local/include) # MacPorts' install path
-endif(APPLE)
-
-find_package(FLTK REQUIRED)
-
-message("FLTK")
-message(" include ${FLTK_INCLUDE_DIR}")
-message(" libs ${FLTK_LIBRARIES}")
-message("OpenGL")
-message(" include ${OPENGL_INCLUDE_DIR}")
-message(" libs ${OPENGL_LIBRARIES}")
-
-include_directories( .
- ..
- ../replace
- ${GLIB_INCLUDE_DIRS}
- ${FLTK_INCLUDE_DIR}
- ${OPENGL_INCLUDE_DIR}
-)
-
-
link_directories(${GLIB_LIBRARY_DIRS}
${FLTK_LIBRARY_DIRS}
${OPENGL_LIBRARY_DIRS}
)
-add_executable(stage
- ancestor.cc
- block.cc
- canvas.cc
- gl.cc
- glcolorstack.cc
- model.cc
- model_callbacks.cc
- model_fiducial.cc
- model_laser.cc
- model_load.cc
- model_position.cc
- model_ranger.cc
- model_blobfinder.cc
- model_props.cc
- stage.cc
- typetable.cc
- world.cc
- worldfile.cc
- worldgui.cc
- main.cc
+
+add_library( stage SHARED
+ ancestor.cc
+ block.cc
+ canvas.cc
+ gl.cc
+ glcolorstack.cc
+ model.cc
+ model_callbacks.cc
+ model_fiducial.cc
+ model_laser.cc
+ model_load.cc
+ model_position.cc
+ model_ranger.cc
+ model_blobfinder.cc
+ model_props.cc
+ stage.cc
+ typetable.cc
+ world.cc
+ worldfile.cc
+ worldgui.cc
)
+add_executable( stage main.cc )
+
target_link_libraries( stage
View
14 libstage/model_laser.cc
@@ -7,7 +7,7 @@
// CVS info:
// $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_laser.cc,v $
// $Author: rtv $
- // $Revision: 1.6 $
+ // $Revision: 1.7 $
//
///////////////////////////////////////////////////////////////////////////
@@ -285,7 +285,19 @@ void StgModelLaser::Print( char* prefix )
}
+stg_laser_sample_t* StgModelLaser::GetSamples( uint32_t* count )
+{
+ if( count ) *count = sample_count;
+ return samples;
+}
+void StgModelLaser::SetSamples( stg_laser_sample_t* samples, uint32_t count)
+{
+ this->samples = g_renew( stg_laser_sample_t, this->samples, sample_count );
+ memcpy( this->samples, samples, sample_count * sizeof(stg_laser_sample_t));
+ this->sample_count = count;
+ this->data_dirty = true;
+}
void StgModelLaser::DataVisualize( void )
{
View
12 libstage/stage.hh
@@ -26,7 +26,7 @@
* Desc: External header file for the Stage library
* Author: Richard Vaughan (vaughan@sfu.ca)
* Date: 1 June 2003
- * CVS: $Id: stage.hh,v 1.13 2008-03-03 06:02:26 rtv Exp $
+ * CVS: $Id: stage.hh,v 1.14 2008-03-04 02:09:56 rtv Exp $
*/
/*! \file stage.h
@@ -1891,12 +1891,10 @@ namespace Draw
virtual void DataVisualize();
uint32_t GetSampleCount(){ return sample_count; }
-
- stg_laser_sample_t* GetSamples( uint32_t* count=NULL)
- {
- if( count ) *count = sample_count;
- return samples;
- }
+
+ stg_laser_sample_t* GetSamples( uint32_t* count=NULL);
+
+ void SetSamples( stg_laser_sample_t* samples, uint32_t count);
// Get the user-tweakable configuration of the laser
stg_laser_cfg_t GetConfig( );
View
46 worlds/889.world
@@ -6,14 +6,17 @@
include "pioneer.inc"
+
# defines 'map' object used for floorplans
include "map.inc"
+
# defines sick laser
include "sick.inc"
+
# set the resolution of the underlying raytrace model in meters
resolution 0.02
@@ -26,9 +29,9 @@ paused 0
window
(
size [ 745.000 448.000 ]
- center [83.990 58.960]
+ center [88.990 152.960]
rotate [ 0.000 0.000 ]
- scale 15.806
+ scale 59.806
)
# load an environment bitmap
@@ -46,26 +49,27 @@ define redrobot pioneer2dx
ctrl "wander"
)
-redrobot( pose [3.094 7.683 158.268] )
+laser( pose [2.643 3.319 158.268] ctrl "lasernoise" )
+
redrobot( pose [1.738 7.800 -52.629] )
redrobot( pose [5.940 8.773 22.445] )
-redrobot( pose [-1.915 3.332 156.191] )
-redrobot( pose [6.536 7.760 -109.062] )
-redrobot( pose [8.917 7.399 89.455] )
-redrobot( pose [-5.570 4.441 -164.935] )
-redrobot( pose [-9.800 7.910 -103.425] )
-redrobot( pose [-4.405 8.204 -57.759] )
-redrobot( pose [7.597 8.321 -57.823] )
-
-redrobot( pose [-6.370 2.754 -113.456] )
-redrobot( pose [4.135 7.859 -1.177] )
-redrobot( pose [-1.690 6.503 46.585] )
-redrobot( pose [0.513 8.072 135.162] )
-redrobot( pose [-6.532 7.733 -3.588] )
-redrobot( pose [-6.321 4.372 93.535] )
-redrobot( pose [-3.192 7.847 -35.760] )
-redrobot( pose [-1.207 7.991 -17.538] )
-redrobot( pose [-6.692 5.179 -23.903] )
-redrobot( pose [-2.148 4.505 -17.092] )
+redrobot( pose [-6.113 4.912 97.288] )
+#redrobot( pose [6.536 7.760 -109.062] )
+#redrobot( pose [8.917 7.399 89.455] )
+#redrobot( pose [-5.570 4.441 -164.935] )
+#redrobot( pose [-9.800 7.910 -103.425] )
+#redrobot( pose [-4.405 8.204 -57.759] )
+#redrobot( pose [7.597 8.321 -57.823] )
+
+#redrobot( pose [-6.370 2.754 -113.456] )
+#redrobot( pose [4.135 7.859 -1.177] )
+#redrobot( pose [-1.690 6.503 46.585] )
+#redrobot( pose [0.513 8.072 135.162] )
+#redrobot( pose [-6.532 7.733 -3.588] )
+#redrobot( pose [-6.321 4.372 93.535] )
+#redrobot( pose [-3.192 7.847 -35.760] )
+#redrobot( pose [-1.207 7.991 -17.538] )
+#redrobot( pose [-6.692 5.179 -23.903] )
+#redrobot( pose [-2.148 4.505 -17.092] )
View
54 worlds/ctrl_demo.world
@@ -0,0 +1,54 @@
+# simple.world - basic world file example
+# Authors: Richard Vaughan
+# $id$
+
+# defines Pioneer-like robots
+include "pioneer.inc"
+
+
+
+# defines 'map' object used for floorplans
+include "map.inc"
+
+
+
+# defines sick laser
+include "sick.inc"
+
+
+
+# set the resolution of the underlying raytrace model in meters
+resolution 0.02
+
+interval_sim 100 # simulation timestep in milliseconds
+interval_real 100 # real-time interval between simulation updates in milliseconds
+
+paused 0
+
+# configure the GUI window
+window
+(
+ size [ 500.000 500.000 ]
+ center [-6 -7]
+ rotate [ 0.000 0.000 ]
+ scale 26
+)
+
+# load an environment bitmap
+floorplan
+(
+ name "cave"
+ size3 [16 16 0.5]
+ pose [0.000 0.000 0.000]
+ bitmap "bitmaps/cave.png"
+)
+
+pioneer2dx
+(
+ pose [ -7.000 -7.000 45.000 ]
+ sicklaser
+ (
+ ctrl "lasernoise"
+ )
+)
+

0 comments on commit 2ba0e44

Please sign in to comment.