Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

Added Stage bumper model and Player interface

This commit adds a bumper model to Stage.  It is based off of the
code provided on Sourceforge by Hadrien Hamel in Sourceforge patch
2793940, and has been updated to work with the Stage 4.1 API.
  • Loading branch information...
commit bc7dbdd3c13fc2c69e0dab94bca9feb16f3bfd5b 1 parent 98b921f
@jpgr87 authored
View
257 libstage/model_bumper.cc
@@ -0,0 +1,257 @@
+///////////////////////////////////////////////////////////////////////////
+//
+// File: model_bumper.cc
+// Author: Richard Vaughan
+// Date: 28 March 2006
+//
+// CVS info:
+// $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/src/model_bumper.c,v $
+// $Author: rtv $
+// $Revision: 1.1 $
+//
+///////////////////////////////////////////////////////////////////////////
+
+/**
+@ingroup model
+@defgroup model_bumper Bumper/Whisker model
+The bumper model simulates an array of binary touch sensors.
+
+<h2>Worldfile properties</h2>
+
+@par Summary and default values
+
+@verbatim
+bumper
+(
+ # bumper properties
+ bcount 1
+ bpose[0] [ 0 0 0 0 ]
+ blength 0.1
+)
+@endverbatim
+
+@par Notes
+
+The bumper model allows configuration of the pose and length parameters of each transducer seperately using bpose[index] and blength[index]. For convenience, the length of all bumpers in the array can be set at once with the blength property. If a blength with no index is specified, this global setting is applied first, then specific blengh[index] properties are applied afterwards. Note that the order in the worldfile is ignored.
+
+@par Details
+- bcount int
+ - the number of bumper transducers
+- bpose[\<transducer index\>] [float float float float]
+ - [x y z theta]
+ - pose of the center of the transducer relative to its parent.
+- blength float
+ - sets the length in meters of all transducers in the array
+- blength[\<transducer index\>] float
+ - length in meters of a specific transducer. This is applied after the global setting above.
+
+*/
+
+#include "stage.hh"
+#include "worldfile.hh"
+#include "option.hh"
+using namespace Stg;
+
+#include <math.h>
+
+static const watts_t BUMPER_WATTS = 0.1; // bumper power consumption
+static const Color BUMPER_HIT_RGB(1, 0, 0, 1); // red
+static const Color BUMPER_NOHIT_RGB(0, 1, 0, 1);// green
+static const meters_t BUMPER_HIT_THICKNESS = 0.02;
+static const meters_t BUMPER_NOHIT_THICKNESS = 0.01;
+
+Option ModelBumper::showBumperData( "Show Bumper Data", "show_bumper", "", true, NULL );
+
+ModelBumper::ModelBumper( World* world,
+ Model* parent,
+ const std::string& type )
+ : Model( world, parent, type ),
+ bumpervis()
+{
+ PRINT_DEBUG2( "Constructing ModelBumper %d (%s)\n",
+ id, type.c_str() );
+
+ // Set up sensible defaults
+
+ // assert that Update() is reentrant for this derived model
+ thread_safe = true;
+
+ bumpers = NULL;
+ samples = NULL;
+ bumper_count = 0;
+
+ AddVisualizer( &bumpervis, true );
+
+}
+
+ModelBumper::~ModelBumper()
+{
+ if( bumpers )
+ delete[] bumpers;
+ if( samples )
+ delete[] samples;
+}
+
+void ModelBumper::Startup( void )
+{
+ Model::Startup();
+
+ PRINT_DEBUG( "bumper startup" );
+
+ this->SetWatts( BUMPER_WATTS );
+}
+
+
+void ModelBumper::Shutdown( void )
+{
+ PRINT_DEBUG( "bumper shutdown" );
+
+ this->SetWatts( 0 );
+
+ if( this->samples )
+ {
+ delete[] samples;
+ samples = NULL;
+ }
+
+ Model::Shutdown();
+}
+
+void ModelBumper::Load( void )
+{
+ Model::Load();
+
+ if( wf->PropertyExists( wf_entity, "bcount" ) )
+ {
+ PRINT_DEBUG( "Loading bumper array" );
+
+ // Load the geometry of a bumper array
+ bumper_count = wf->ReadInt( wf_entity, "bcount", 0);
+ assert( bumper_count > 0 );
+
+ char key[256];
+
+ if( bumpers ) delete [] bumpers;
+ bumpers = new BumperConfig[bumper_count];
+
+ meters_t common_length;
+ common_length = wf->ReadLength( wf_entity, "blength", 0 );
+
+ // set all transducers with the common settings
+ for( unsigned int i = 0; i < bumper_count; i++)
+ {
+ bumpers[i].length = common_length;
+ }
+
+ // allow individual configuration of transducers
+ for( unsigned int i = 0; i < bumper_count; i++)
+ {
+ snprintf(key, sizeof(key), "bpose[%d]", i);
+ wf->ReadTuple( wf_entity, key, 0, 4, "llla",
+ &bumpers[i].pose.x,
+ &bumpers[i].pose.y,
+ &bumpers[i].pose.z,
+ &bumpers[i].pose.a );
+
+ snprintf(key, sizeof(key), "blength[%d]", i);
+ bumpers[i].length = wf->ReadLength( wf_entity, key, bumpers[i].length );
+ }
+
+ PRINT_DEBUG1( "loaded %d bumpers configs", (int)bumper_count );
+ }
+}
+
+static bool bumper_match( Model* candidate,
+ Model* finder, const void* dummy )
+{
+ // Ignore myself, my children, and my ancestors.
+ return( //candidate->vis.obstacle_return &&
+ !finder->IsRelated( candidate ) );
+}
+
+void ModelBumper::Update( void )
+{
+ Model::Update();
+
+ if( (bumpers == NULL) || (bumper_count < 1 ))
+ return;
+
+ if( samples == NULL )
+ samples = new BumperSample[bumper_count];
+ assert( samples );
+
+ for( unsigned int t=0; t<bumper_count; t++ )
+ {
+ // change the pose of bumper to act as a sensor rotated of PI/2, positioned at
+ // an extremity of the bumper, and with a range of "length"
+ Pose bpose;
+ bpose.a = bumpers[t].pose.a + M_PI/2.0;
+ bpose.x = bumpers[t].pose.x - bumpers[t].length/2.0 * cos(bpose.a);
+ bpose.y = bumpers[t].pose.y - bumpers[t].length/2.0 * sin(bpose.a);
+
+ RaytraceResult ray = Raytrace( bpose,
+ bumpers[t].length,
+ bumper_match,
+ NULL,
+ false );
+
+ samples[t].hit = ray.mod;
+ if (ray.mod) {
+ samples[t].hit_point = point_t(ray.pose.x, ray.pose.y);
+ }
+ }
+}
+
+void ModelBumper::Print( char* prefix )
+{
+ Model::Print( prefix );
+
+ printf( "\tBumpers[ " );
+
+ for( unsigned int i=0; i<bumper_count; i++ )
+ printf( "%d ", samples[i].hit?1:0 );
+ puts( " ]" );
+}
+
+ModelBumper::BumperVis::BumperVis()
+ : Visualizer( "Bumper hits", "show_bumper_hits" )
+{
+ // Nothing to do here
+}
+
+ModelBumper::BumperVis::~BumperVis()
+{
+ // Nothing to do here
+}
+
+void ModelBumper::BumperVis::Visualize( Model* mod, Camera* cam )
+{
+ ModelBumper* bump = dynamic_cast<ModelBumper*>(mod);
+
+ double thickness;
+ if( ! (bump->samples && bump->bumpers && bump->bumper_count) ){
+ return;
+ }
+
+ if (! bump->showBumperData ) {
+ return;
+ }
+
+ for( unsigned int t=0; t< bump->bumper_count; t++ )
+ {
+ glPushMatrix();
+ if (bump->samples[t].hit) {
+ thickness = 0.05;
+ glColor3f(1.0f, 0.0f, 0.0f);
+ } else {
+ thickness = 0.05;
+ glColor3f(0.0f, 1.0f, 0.0f);
+ }
+ glRotatef(bump->bumpers[t].pose.a, 0, 0, 1);
+ glTranslatef(bump->bumpers[t].pose.x, bump->bumpers[t].pose.y,0);
+ glRectf(-bump->bumpers[t].length/2.0, -thickness/2.0,
+ bump-> bumpers[t].length/2.0, thickness/2.0);
+ glPopMatrix();
+ }
+
+}
View
138 libstageplugin/p_bumper.cc
@@ -0,0 +1,138 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2004, 2005 Richard Vaughan
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+/*
+ * Desc: A plugin driver for Player that gives access to Stage devices.
+ * Author: Richard Vaughan
+ * Date: 10 December 2004
+ * CVS: $Id$
+ */
+
+// DOCUMENTATION ------------------------------------------------------------
+
+/** @addtogroup player
+@par Bumper interface
+- PLAYER_BUMPER_DATA_STATE
+- PLAYER_BUMPER_GET_GEOM
+*/
+
+// CODE ----------------------------------------------------------------------
+
+#include "p_driver.h"
+using namespace Stg;
+//
+// BUMPER INTERFACE
+//
+
+InterfaceBumper::InterfaceBumper( player_devaddr_t addr,
+ StgDriver* driver,
+ ConfigFile* cf,
+ int section )
+ : InterfaceModel( addr, driver, cf, section, "bumper" )
+{
+}
+
+void InterfaceBumper::Publish( void )
+{
+
+ ModelBumper* mod = (ModelBumper*) this->mod;
+ if (mod->samples == NULL)
+ return;
+
+ ModelBumper::BumperSample* sdata = (ModelBumper::BumperSample*) mod->samples;
+
+ int bumper_count = mod->bumper_count;
+
+ player_bumper_data_t pdata;
+ memset( &pdata, 0, sizeof(pdata) );
+
+ if( bumper_count > 0 )
+ {
+
+ pdata.bumpers_count = bumper_count;
+ pdata.bumpers = new uint8_t[bumper_count];
+
+ for( int i=0; i<(int)bumper_count; i++ )
+ {
+ pdata.bumpers[i] = sdata[i].hit ? 1 : 0;
+ }
+
+ this->driver->Publish( this->addr,
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_BUMPER_DATA_STATE,
+ &pdata, sizeof(pdata), NULL);
+ }
+}
+
+
+int InterfaceBumper::ProcessMessage( QueuePointer &resp_queue,
+ player_msghdr_t* hdr,
+ void* data )
+{
+ if( Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_BUMPER_REQ_GET_GEOM,
+ this->addr) )
+ {
+ ModelBumper *mod = (ModelBumper*) this->mod;
+
+ int bumper_count = mod->bumper_count;
+ ModelBumper::BumperConfig* bumpers = (ModelBumper::BumperConfig*)mod->bumpers;
+ assert( bumpers );
+
+ // convert the bumper data into Player-format bumper poses
+ player_bumper_geom_t pgeom;
+ memset( &pgeom, 0, sizeof(pgeom) );
+
+ pgeom.bumper_def_count = bumper_count;
+ pgeom.bumper_def = new player_bumper_define_t[bumper_count];
+
+ for( int i=0; i<(int)bumper_count; i++ )
+ {
+ // fill in the geometry data formatted player-like
+ pgeom.bumper_def[i].pose.px = bumpers[i].pose.x;
+ pgeom.bumper_def[i].pose.py = bumpers[i].pose.y;
+ pgeom.bumper_def[i].pose.pz = bumpers[i].pose.z;
+ pgeom.bumper_def[i].pose.proll = 0;
+ pgeom.bumper_def[i].pose.ppitch = 0;
+ pgeom.bumper_def[i].pose.pyaw = bumpers[i].pose.a;
+ pgeom.bumper_def[i].length = bumpers[i].length;
+ pgeom.bumper_def[i].radius = 0;
+ }
+
+ this->driver->Publish( this->addr, resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_BUMPER_REQ_GET_GEOM,
+ (void*)&pgeom, sizeof(pgeom), NULL );
+
+ delete[] pgeom.bumper_def;
+ }
+ else
+ {
+ // Don't know how to handle this message.
+ PRINT_WARN2( "bumper doesn't support msg with type/subtype %d/%d",
+ hdr->type, hdr->subtype);
+ return(-1);
+ }
+
+ // Everything's ok
+ return (0);
+}
+
View
135 libstageplugin/p_bumper.cc.TODO
@@ -1,135 +0,0 @@
-/*
- * Player - One Hell of a Robot Server
- * Copyright (C) 2004, 2005 Richard Vaughan
- *
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- */
-
-/*
- * Desc: A plugin driver for Player that gives access to Stage devices.
- * Author: Richard Vaughan
- * Date: 10 December 2004
- * CVS: $Id$
- */
-
-// DOCUMENTATION ------------------------------------------------------------
-
-/** @addtogroup player
-@par Bumper interface
-- PLAYER_BUMPER_DATA_STATE
-- PLAYER_BUMPER_GET_GEOM
-*/
-
-// CODE ----------------------------------------------------------------------
-
-#include "p_driver.h"
-
-//
-// BUMPER INTERFACE
-//
-extern "C" {
-int bumper_init( model_t* mod );
-}
-
-InterfaceBumper::InterfaceBumper( player_devaddr_t id,
- StgDriver* driver,
- ConfigFile* cf,
- int section )
- : InterfaceModel( id, driver, cf, section, bumper_init )
-{
-}
-
-void InterfaceBumper::Publish( void )
-{
-
- size_t len = mod->data_len;
- bumper_sample_t* sdata = (bumper_sample_t*)mod->data;
-
- player_bumper_data_t pdata;
- memset( &pdata, 0, sizeof(pdata) );
-
- if( len > 0 )
- {
- size_t bcount = len / sizeof(bumper_sample_t);
-
- // limit the number of samples to Player's maximum
- if( bcount > PLAYER_BUMPER_MAX_SAMPLES )
- bcount = PLAYER_BUMPER_MAX_SAMPLES;
-
- pdata.bumpers_count = bcount;
-
- for( int i=0; i<(int)bcount; i++ )
- pdata.bumpers[i] = sdata[i].hit ? 1 : 0;
- }
-
- this->driver->Publish( this->addr, NULL,
- PLAYER_MSGTYPE_DATA,
- PLAYER_BUMPER_DATA_STATE,
- &pdata, sizeof(pdata), NULL);
-}
-
-
-int InterfaceBumper::ProcessMessage( MessageQueue* resp_queue,
- player_msghdr_t* hdr,
- void* data )
-{
- if( Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
- PLAYER_BUMPER_GET_GEOM,
- this->addr) )
- {
- size_t cfglen = mod->cfg_len;
- bumper_config_t* cfgs = (bumper_config_t*)mod->cfg;
- assert( cfgs );
-
- size_t bcount = cfglen / sizeof(bumper_config_t);
-
- // convert the bumper data into Player-format bumper poses
- player_bumper_geom_t pgeom;
- memset( &pgeom, 0, sizeof(pgeom) );
-
- // limit the number of samples to Player's maximum
- if( bcount > PLAYER_BUMPER_MAX_SAMPLES )
- bcount = PLAYER_BUMPER_MAX_SAMPLES;
-
- pgeom.bumper_def_count = bcount;
-
- for( int i=0; i<(int)bcount; i++ )
- {
- // fill in the geometry data formatted player-like
- pgeom.bumper_def[i].pose.px = cfgs[i].pose.x;
- pgeom.bumper_def[i].pose.py = cfgs[i].pose.y;
- pgeom.bumper_def[i].pose.pa = cfgs[i].pose.a;
- pgeom.bumper_def[i].length = cfgs[i].length;
- pgeom.bumper_def[i].radius = 0;
- }
-
- this->driver->Publish( this->addr, resp_queue,
- PLAYER_MSGTYPE_RESP_ACK,
- PLAYER_BUMPER_GET_GEOM,
- (void*)&pgeom, sizeof(pgeom), NULL );
-
- return 0; // ok
- }
- else
- {
- // Don't know how to handle this message.
- PRINT_WARN2( "bumper doesn't support msg with type/subtype %d/%d",
- hdr->type, hdr->subtype);
- return(-1);
- }
-}
-
View
26 worlds/roomba.cfg
@@ -0,0 +1,26 @@
+
+# Desc: Player sample configuration file for controlling Stage devices
+# Author: Richard Vaughan
+# Date: 1 December 2004
+
+
+# load the Stage plugin simulation driver
+driver
+(
+ name "stage"
+ provides [ "simulation:0" ]
+ plugin "stageplugin"
+
+ # load the named file into the simulator
+ worldfile "roomba.world"
+)
+
+# Create a Stage driver and attach position2d and laser interfaces
+# to the model "r0"
+driver
+(
+ name "stage"
+ provides [ "position2d:0" "bumper:0" ]
+ model "r0"
+)
+
View
45 worlds/roomba.world
@@ -0,0 +1,45 @@
+# simple.world - basic world file example
+# Authors: Richard Vaughan
+# $Id$
+
+include "irobot.inc"
+include "map.inc"
+
+# time to pause (in GUI mode) or quit (in headless mode (-g)) the simulation
+quit_time 3600 # 1 hour of simulated time
+
+paused 1
+
+resolution 0.02
+
+# configure the GUI window
+window
+(
+ size [ 635.000 666.000 ] # in pixels
+ scale 36.995 # pixels per meter
+ center [ -0.040 -0.274 ]
+ rotate [ 0 0 ]
+
+ show_data 1 # 1=on 0=off
+)
+
+# load an environment bitmap
+floorplan
+(
+ name "cave"
+ size [16.000 16.000 0.800]
+ pose [0 0 0 0]
+ bitmap "bitmaps/cave.png"
+)
+
+
+roomba
+(
+ # can refer to the robot by this name
+ name "r0"
+ pose [ -6.946 -6.947 0 45.000 ]
+
+ # report error-free position in world coordinates
+ localization "gps"
+ localization_origin [ 0 0 0 0 ]
+)
Please sign in to comment.
Something went wrong with that request. Please try again.