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.

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();
+    }
+
+}

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);
+}
+