Skip to content
This repository

Bumper model with Player interface #33

Closed
wants to merge 7 commits into from

1 participant

Rich
Rich

I've restored the bumper interface to Stage with some patches laying around on the sourceforge tracker. It shares a commit with the "camera" branch that fixes issues with Player 3.0.2, but the bumper commit can be pulled.

Tested with the new roomba.cfg and roomba.world files. These include the irobot.inc file, which I updated to restore bumper functionality.

and others added some commits December 12, 2012
Rich Add ifdefs for compatibility with Player < 3.1
The Stage plugin contains some features (graphics2d multiline,
etc.) that were added in Player 3.1.  This commit adds some
code to let the latest version of Stage compile against Player
3.0.2 and earlier.
98b921f
Rich 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.
bc7dbdd
Rich Add the rest of the bumper patch.
I missed some files with the last bumper commit by mistake.  This commit
contains the rest of the bumper model code
1473a1f
corrected opengl drawing error
radians/degrees issue plus order of opengl transforms
b278ec8
Rich Add Z component to roomba model a810d84
Rich Merge remote-tracking branch 'upstream/master' into bumper b1bec07
Rich Merge remote-tracking branch 'upstream/master' into bumper
Conflicts:
	libstageplugin/CMakeLists.txt
ddee9e0
Rich jpgr87 closed this February 16, 2014
Rich jpgr87 deleted the branch February 16, 2014
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Showing 7 unique commits by 2 authors.

Dec 12, 2012
Rich Add ifdefs for compatibility with Player < 3.1
The Stage plugin contains some features (graphics2d multiline,
etc.) that were added in Player 3.1.  This commit adds some
code to let the latest version of Stage compile against Player
3.0.2 and earlier.
98b921f
Rich 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.
bc7dbdd
Feb 11, 2013
Rich Add the rest of the bumper patch.
I missed some files with the last bumper commit by mistake.  This commit
contains the rest of the bumper model code
1473a1f
Feb 12, 2013
corrected opengl drawing error
radians/degrees issue plus order of opengl transforms
b278ec8
Rich Add Z component to roomba model a810d84
Apr 19, 2013
Rich Merge remote-tracking branch 'upstream/master' into bumper b1bec07
Feb 16, 2014
Rich Merge remote-tracking branch 'upstream/master' into bumper
Conflicts:
	libstageplugin/CMakeLists.txt
ddee9e0
This page is out of date. Refresh to see the latest.
1  libstage/CMakeLists.txt
@@ -16,6 +16,7 @@ set( stageSrcs
16 16
 	model_actuator.cc
17 17
 	model_blinkenlight.cc
18 18
 	model_blobfinder.cc
  19
+	model_bumper.cc
19 20
 	model_callbacks.cc
20 21
 	model_camera.cc
21 22
 	model_draw.cc
263  libstage/model_bumper.cc
... ...
@@ -0,0 +1,263 @@
  1
+///////////////////////////////////////////////////////////////////////////
  2
+//
  3
+// File: model_bumper.cc
  4
+// Author: Richard Vaughan
  5
+// Date: 28 March 2006
  6
+//
  7
+// CVS info:
  8
+//  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/src/model_bumper.c,v $
  9
+//  $Author: rtv $
  10
+//  $Revision: 1.1 $
  11
+//
  12
+///////////////////////////////////////////////////////////////////////////
  13
+
  14
+/**
  15
+@ingroup model
  16
+@defgroup model_bumper Bumper/Whisker  model
  17
+The bumper model simulates an array of binary touch sensors.
  18
+
  19
+<h2>Worldfile properties</h2>
  20
+
  21
+@par Summary and default values
  22
+
  23
+@verbatim
  24
+bumper
  25
+(
  26
+  # bumper properties
  27
+  bcount 1
  28
+  bpose[0] [ 0 0 0 0 ]
  29
+  blength 0.1
  30
+)
  31
+@endverbatim
  32
+
  33
+@par Notes
  34
+
  35
+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.
  36
+
  37
+@par Details
  38
+- bcount int
  39
+  - the number of bumper transducers
  40
+- bpose[\<transducer index\>] [float float float float]
  41
+  - [x y z theta]
  42
+  - pose of the center of the transducer relative to its parent.
  43
+- blength float
  44
+  - sets the length in meters of all transducers in the array
  45
+- blength[\<transducer index\>] float
  46
+  - length in meters of a specific transducer. This is applied after the global setting above.
  47
+
  48
+*/
  49
+
  50
+#include "stage.hh"
  51
+#include "worldfile.hh"
  52
+#include "option.hh"
  53
+using namespace Stg;
  54
+
  55
+#include <math.h>
  56
+
  57
+static const watts_t BUMPER_WATTS = 0.1; // bumper power consumption
  58
+static const Color BUMPER_HIT_RGB(1, 0, 0, 1); // red
  59
+static const Color BUMPER_NOHIT_RGB(0, 1, 0, 1);// green
  60
+static const meters_t BUMPER_HIT_THICKNESS = 0.02;
  61
+static const meters_t BUMPER_NOHIT_THICKNESS = 0.01;
  62
+
  63
+Option ModelBumper::showBumperData( "Show Bumper Data", "show_bumper", "", true, NULL );
  64
+
  65
+ModelBumper::ModelBumper( World* world, 
  66
+			  Model* parent,
  67
+			  const std::string& type )
  68
+  : Model( world, parent, type ),
  69
+    bumpervis()
  70
+{
  71
+  PRINT_DEBUG2( "Constructing ModelBumper %d (%s)\n", 
  72
+		id, type.c_str() );
  73
+
  74
+  // Set up sensible defaults
  75
+
  76
+  // assert that Update() is reentrant for this derived model
  77
+  thread_safe = true;
  78
+
  79
+  bumpers = NULL;
  80
+  samples = NULL;
  81
+  bumper_count = 0;
  82
+
  83
+	AddVisualizer( &bumpervis, true );
  84
+
  85
+}
  86
+
  87
+ModelBumper::~ModelBumper()
  88
+{
  89
+  if( bumpers )
  90
+	 delete[] bumpers;
  91
+  if( samples )
  92
+	 delete[] samples;
  93
+}
  94
+
  95
+void ModelBumper::Startup( void )
  96
+{
  97
+  Model::Startup();
  98
+
  99
+  PRINT_DEBUG( "bumper startup" );
  100
+
  101
+  this->SetWatts( BUMPER_WATTS );
  102
+}
  103
+
  104
+
  105
+void ModelBumper::Shutdown( void )
  106
+{
  107
+  PRINT_DEBUG( "bumper shutdown" );
  108
+
  109
+  this->SetWatts( 0 );
  110
+
  111
+  if( this->samples )
  112
+    {
  113
+      delete[] samples;
  114
+      samples = NULL;
  115
+    }
  116
+
  117
+  Model::Shutdown();
  118
+}
  119
+
  120
+void ModelBumper::Load( void )
  121
+{
  122
+  Model::Load();
  123
+
  124
+  if( wf->PropertyExists( wf_entity, "bcount" ) )
  125
+    {
  126
+      PRINT_DEBUG( "Loading bumper array" );
  127
+
  128
+      // Load the geometry of a bumper array
  129
+      bumper_count = wf->ReadInt( wf_entity, "bcount", 0);
  130
+      assert( bumper_count > 0 );
  131
+		
  132
+      char key[256];
  133
+		
  134
+      if( bumpers ) delete [] bumpers;
  135
+      bumpers = new BumperConfig[bumper_count];
  136
+
  137
+      meters_t common_length;
  138
+      common_length = wf->ReadLength( wf_entity, "blength", 0 );
  139
+
  140
+      // set all transducers with the common settings
  141
+      for( unsigned int i = 0; i < bumper_count; i++)
  142
+	{
  143
+          bumpers[i].length = common_length;
  144
+	}
  145
+
  146
+      // allow individual configuration of transducers
  147
+      for( unsigned int i = 0; i < bumper_count; i++)
  148
+	{
  149
+	  snprintf(key, sizeof(key), "bpose[%d]", i);
  150
+          wf->ReadTuple( wf_entity, key, 0, 4, "llla",
  151
+                         &bumpers[i].pose.x,
  152
+                         &bumpers[i].pose.y,
  153
+                         &bumpers[i].pose.z,
  154
+                         &bumpers[i].pose.a );
  155
+
  156
+	  snprintf(key, sizeof(key), "blength[%d]", i);
  157
+	  bumpers[i].length = wf->ReadLength( wf_entity, key, bumpers[i].length );
  158
+	}
  159
+
  160
+      PRINT_DEBUG1( "loaded %d bumpers configs", (int)bumper_count );	  
  161
+    }
  162
+}
  163
+
  164
+static bool bumper_match( Model* candidate, 
  165
+			  Model* finder, const void* dummy )
  166
+{
  167
+  // Ignore myself, my children, and my ancestors.
  168
+  return( //candidate->vis.obstacle_return && 
  169
+	  !finder->IsRelated( candidate ) );
  170
+}	
  171
+
  172
+void ModelBumper::Update( void )
  173
+{
  174
+  Model::Update();
  175
+
  176
+  if( (bumpers == NULL) || (bumper_count < 1 )) {
  177
+    return;
  178
+  }
  179
+
  180
+  if( samples ==  NULL ) {
  181
+    samples = new BumperSample[bumper_count];
  182
+  }
  183
+  assert( samples );
  184
+
  185
+  for( unsigned int t=0; t<bumper_count; t++ )
  186
+    {
  187
+      // change the pose of bumper to act as a sensor rotated of PI/2, positioned at
  188
+      // an extremity of the bumper, and with a range of "length"
  189
+      Pose bpose;
  190
+      bpose.a = bumpers[t].pose.a + M_PI/2.0;
  191
+      bpose.x = bumpers[t].pose.x - bumpers[t].length/2.0 * cos(bpose.a);
  192
+      bpose.y = bumpers[t].pose.y - bumpers[t].length/2.0 * sin(bpose.a);
  193
+
  194
+      RaytraceResult ray = Raytrace( bpose,
  195
+					    bumpers[t].length,
  196
+					    bumper_match,
  197
+					    NULL,
  198
+					    false );
  199
+					    
  200
+      samples[t].hit = ray.mod;
  201
+      if (ray.mod) {
  202
+        samples[t].hit_point = point_t(ray.pose.x, ray.pose.y);
  203
+      }
  204
+    }   
  205
+}
  206
+
  207
+void ModelBumper::Print( char* prefix )
  208
+{
  209
+  Model::Print( prefix );
  210
+
  211
+  printf( "\tBumpers[ " );
  212
+
  213
+  for( unsigned int i=0; i<bumper_count; i++ )
  214
+    printf( "%d ", samples[i].hit?1:0 );
  215
+  puts( " ]" );
  216
+}
  217
+
  218
+ModelBumper::BumperVis::BumperVis()
  219
+	: Visualizer( "Bumper hits", "show_bumper_hits" )
  220
+{ 
  221
+	// Nothing to do here
  222
+}
  223
+
  224
+ModelBumper::BumperVis::~BumperVis()
  225
+{
  226
+	// Nothing to do here
  227
+}
  228
+
  229
+void ModelBumper::BumperVis::Visualize( Model* mod, Camera* cam )
  230
+{
  231
+  ModelBumper* bump = dynamic_cast<ModelBumper*>(mod);
  232
+
  233
+
  234
+  if( ! (bump->samples && bump->bumpers && bump->bumper_count) ){
  235
+    return;
  236
+  }
  237
+
  238
+  if (! bump->showBumperData ) {
  239
+    return;
  240
+  }
  241
+  /*
  242
+   * draw the sensitive area of the bumper, with a color indicating if it is hit.
  243
+   */
  244
+  for( unsigned int t=0; t< bump->bumper_count; t++ )
  245
+    {
  246
+      //This is how wide the active area rectangle will be.
  247
+      float thickness=0.01;
  248
+      glPushMatrix();
  249
+      //draw the active area with a different color if it is hit
  250
+      if (bump->samples[t].hit) {
  251
+        glColor3f(1.0f, 0.0f, 0.0f);
  252
+      } else {
  253
+        glColor3f(0.0f, 1.0f, 0.0f);
  254
+      }
  255
+      const float rad2deg=180.0/M_PI;
  256
+      glTranslatef(bump->bumpers[t].pose.x, bump->bumpers[t].pose.y,0);
  257
+      glRotatef(bump->bumpers[t].pose.a*rad2deg, 0, 0, 1);
  258
+      glRectf(-thickness/2.0f, -bump->bumpers[t].length/2.0,
  259
+               thickness/2.0f,  bump->bumpers[t].length/2.0 );
  260
+      glPopMatrix();
  261
+    }
  262
+
  263
+}
60  libstage/stage.hh
@@ -2641,20 +2641,56 @@ namespace Stg
2641 2641
   };
2642 2642
 
2643 2643
 
2644  
-  // \todo BUMPER MODEL --------------------------------------------------------
2645 2644
 
2646  
-  //   typedef struct
2647  
-  //   {
2648  
-  //     Pose pose;
2649  
-  //     meters_t length;
2650  
-  //   } bumper_config_t;
2651  
-
2652  
-  //   typedef struct
2653  
-  //   {
2654  
-  //     Model* hit;
2655  
-  //     point_t hit_point;
2656  
-  //   } bumper_sample_t;
  2645
+  // BUMPER MODEL --------------------------------------------------------
  2646
+  /// %ModelBumper class
  2647
+  class ModelBumper : public Model
  2648
+  {
  2649
+  public:
  2650
+    class BumperConfig
  2651
+    {
  2652
+     public:
  2653
+     Pose pose;
  2654
+     meters_t length;
  2655
+    };
  2656
+  
  2657
+   class BumperSample
  2658
+   {
  2659
+    public:
  2660
+    Model* hit;
  2661
+    point_t hit_point;
  2662
+   };
  2663
+  
  2664
+  
  2665
+  public:
  2666
+    ModelBumper( World* world,
  2667
+    Model* parent,
  2668
+    const std::string& type );
  2669
+    virtual ~ModelBumper();
2657 2670
 
  2671
+    virtual void Load();
  2672
+    
  2673
+    uint32_t bumper_count;
  2674
+    BumperConfig* bumpers;
  2675
+    BumperSample* samples;
  2676
+    
  2677
+  protected:
  2678
+    virtual void Startup();
  2679
+    virtual void Shutdown();
  2680
+    virtual void Update();
  2681
+    virtual void Print (char *prefix);
  2682
+    
  2683
+    class BumperVis : public Visualizer
  2684
+    {
  2685
+    public:
  2686
+      BumperVis();
  2687
+      virtual ~BumperVis();
  2688
+      virtual void Visualize( Model* mod, Camera* cam);
  2689
+    } bumpervis;
  2690
+    
  2691
+  private:
  2692
+   static Option showBumperData;
  2693
+  };
2658 2694
 
2659 2695
   // FIDUCIAL MODEL --------------------------------------------------------
2660 2696
 
1  libstage/typetable.cc
@@ -23,6 +23,7 @@ void Stg::RegisterModels()
23 23
   Register( "actuator",       Creator<ModelActuator> );
24 24
   Register( "blinkenlight",   Creator<ModelBlinkenlight> );
25 25
   Register( "blobfinder",     Creator<ModelBlobfinder> );
  26
+  Register( "bumper",         Creator<ModelBumper> );
26 27
   Register( "camera",         Creator<ModelCamera> );
27 28
   Register( "fiducial",       Creator<ModelFiducial> );
28 29
   Register( "gripper",        Creator<ModelGripper> );
1  libstageplugin/CMakeLists.txt
@@ -8,6 +8,7 @@ set( stagepluginSrcs
8 8
 	     p_driver.cc
9 9
 	     p_actarray.cc
10 10
 	     p_blobfinder.cc
  11
+	     p_bumper.cc
11 12
 	     p_camera.cc
12 13
 	     p_gripper.cc
13 14
 	     p_simulation.cc
138  libstageplugin/p_bumper.cc
... ...
@@ -0,0 +1,138 @@
  1
+/*
  2
+ *  Player - One Hell of a Robot Server
  3
+ *  Copyright (C) 2004, 2005 Richard Vaughan
  4
+ *                      
  5
+ * 
  6
+ *  This program is free software; you can redistribute it and/or modify
  7
+ *  it under the terms of the GNU General Public License as published by
  8
+ *  the Free Software Foundation; either version 2 of the License, or
  9
+ *  (at your option) any later version.
  10
+ *
  11
+ *  This program is distributed in the hope that it will be useful,
  12
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14
+ *  GNU General Public License for more details.
  15
+ *
  16
+ *  You should have received a copy of the GNU General Public License
  17
+ *  along with this program; if not, write to the Free Software
  18
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  19
+ *
  20
+ */
  21
+
  22
+/*
  23
+ * Desc: A plugin driver for Player that gives access to Stage devices.
  24
+ * Author: Richard Vaughan
  25
+ * Date: 10 December 2004
  26
+ * CVS: $Id$
  27
+ */
  28
+
  29
+// DOCUMENTATION ------------------------------------------------------------
  30
+
  31
+/** @addtogroup player 
  32
+@par Bumper interface
  33
+- PLAYER_BUMPER_DATA_STATE
  34
+- PLAYER_BUMPER_GET_GEOM
  35
+*/
  36
+
  37
+// CODE ----------------------------------------------------------------------
  38
+
  39
+#include "p_driver.h"
  40
+using namespace Stg;
  41
+//
  42
+// BUMPER INTERFACE
  43
+//
  44
+
  45
+InterfaceBumper::InterfaceBumper( player_devaddr_t addr, 
  46
+				StgDriver* driver,
  47
+				ConfigFile* cf,
  48
+				int section )
  49
+  : InterfaceModel( addr, driver, cf, section, "bumper" )
  50
+{
  51
+}
  52
+
  53
+void InterfaceBumper::Publish( void )
  54
+{
  55
+
  56
+  ModelBumper* mod = (ModelBumper*) this->mod;
  57
+  if (mod->samples == NULL)
  58
+  	return;
  59
+  	
  60
+  ModelBumper::BumperSample* sdata = (ModelBumper::BumperSample*) mod->samples;
  61
+  
  62
+  int bumper_count = mod->bumper_count;
  63
+  
  64
+  player_bumper_data_t pdata;
  65
+  memset( &pdata, 0, sizeof(pdata) );
  66
+  
  67
+  if( bumper_count > 0 )
  68
+  {      
  69
+    
  70
+    pdata.bumpers_count = bumper_count;
  71
+    pdata.bumpers = new uint8_t[bumper_count];
  72
+    
  73
+    for( int i=0; i<(int)bumper_count; i++ )
  74
+    {
  75
+			pdata.bumpers[i] = sdata[i].hit ? 1 : 0;
  76
+    } 
  77
+  
  78
+  this->driver->Publish( this->addr,
  79
+			 PLAYER_MSGTYPE_DATA,
  80
+			 PLAYER_BUMPER_DATA_STATE,
  81
+			 &pdata, sizeof(pdata), NULL);
  82
+	}
  83
+}
  84
+
  85
+
  86
+int InterfaceBumper::ProcessMessage( QueuePointer &resp_queue,
  87
+				     player_msghdr_t* hdr,
  88
+				     void* data )
  89
+{  
  90
+	if( Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 
  91
+			    PLAYER_BUMPER_REQ_GET_GEOM, 
  92
+			    this->addr) )
  93
+	{
  94
+		ModelBumper *mod = (ModelBumper*) this->mod;
  95
+		
  96
+		int bumper_count = mod->bumper_count;
  97
+		ModelBumper::BumperConfig* bumpers = (ModelBumper::BumperConfig*)mod->bumpers;
  98
+		assert( bumpers );
  99
+
  100
+		// convert the bumper data into Player-format bumper poses	
  101
+		player_bumper_geom_t pgeom;
  102
+		memset( &pgeom, 0, sizeof(pgeom) );
  103
+      
  104
+		pgeom.bumper_def_count = bumper_count;
  105
+		pgeom.bumper_def = new player_bumper_define_t[bumper_count];
  106
+      
  107
+		for( int i=0; i<(int)bumper_count; i++ )
  108
+		{
  109
+			// fill in the geometry data formatted player-like
  110
+			pgeom.bumper_def[i].pose.px = bumpers[i].pose.x;	  
  111
+			pgeom.bumper_def[i].pose.py = bumpers[i].pose.y;
  112
+			pgeom.bumper_def[i].pose.pz = bumpers[i].pose.z;  
  113
+			pgeom.bumper_def[i].pose.proll = 0;	    
  114
+			pgeom.bumper_def[i].pose.ppitch = 0;	    
  115
+			pgeom.bumper_def[i].pose.pyaw = bumpers[i].pose.a;	    
  116
+			pgeom.bumper_def[i].length =  bumpers[i].length;	    
  117
+			pgeom.bumper_def[i].radius = 0; 
  118
+		}
  119
+    
  120
+    this->driver->Publish( this->addr, resp_queue, 
  121
+		     PLAYER_MSGTYPE_RESP_ACK, 
  122
+		     PLAYER_BUMPER_REQ_GET_GEOM,
  123
+		     (void*)&pgeom, sizeof(pgeom), NULL );
  124
+		     
  125
+		delete[] pgeom.bumper_def;
  126
+  }
  127
+	else
  128
+  {
  129
+    // Don't know how to handle this message.
  130
+    PRINT_WARN2( "bumper doesn't support msg with type/subtype %d/%d",
  131
+	   hdr->type, hdr->subtype);
  132
+    return(-1);
  133
+  }
  134
+  
  135
+  // Everything's ok
  136
+  return (0);
  137
+}
  138
+
135  libstageplugin/p_bumper.cc.TODO
... ...
@@ -1,135 +0,0 @@
1  
-/*
2  
- *  Player - One Hell of a Robot Server
3  
- *  Copyright (C) 2004, 2005 Richard Vaughan
4  
- *                      
5  
- * 
6  
- *  This program is free software; you can redistribute it and/or modify
7  
- *  it under the terms of the GNU General Public License as published by
8  
- *  the Free Software Foundation; either version 2 of the License, or
9  
- *  (at your option) any later version.
10  
- *
11  
- *  This program is distributed in the hope that it will be useful,
12  
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
13  
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  
- *  GNU General Public License for more details.
15  
- *
16  
- *  You should have received a copy of the GNU General Public License
17  
- *  along with this program; if not, write to the Free Software
18  
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
19  
- *
20  
- */
21  
-
22  
-/*
23  
- * Desc: A plugin driver for Player that gives access to Stage devices.
24  
- * Author: Richard Vaughan
25  
- * Date: 10 December 2004
26  
- * CVS: $Id$
27  
- */
28  
-
29  
-// DOCUMENTATION ------------------------------------------------------------
30  
-
31  
-/** @addtogroup player 
32  
-@par Bumper interface
33  
-- PLAYER_BUMPER_DATA_STATE
34  
-- PLAYER_BUMPER_GET_GEOM
35  
-*/
36  
-
37  
-// CODE ----------------------------------------------------------------------
38  
-
39  
-#include "p_driver.h"
40  
-
41  
-//
42  
-// BUMPER INTERFACE
43  
-//
44  
-extern "C" { 
45  
-int bumper_init( model_t* mod );
46  
-}
47  
-
48  
-InterfaceBumper::InterfaceBumper( player_devaddr_t id, 
49  
-				StgDriver* driver,
50  
-				ConfigFile* cf,
51  
-				int section )
52  
-  : InterfaceModel( id, driver, cf, section, bumper_init )
53  
-{
54  
-}
55  
-
56  
-void InterfaceBumper::Publish( void )
57  
-{
58  
-  
59  
-  size_t len = mod->data_len;
60  
-  bumper_sample_t* sdata = (bumper_sample_t*)mod->data;
61  
-
62  
-  player_bumper_data_t pdata;
63  
-  memset( &pdata, 0, sizeof(pdata) );
64  
-  
65  
-  if( len > 0 )
66  
-    {      
67  
-      size_t bcount = len / sizeof(bumper_sample_t);
68  
-      
69  
-      // limit the number of samples to Player's maximum
70  
-      if( bcount > PLAYER_BUMPER_MAX_SAMPLES )
71  
-	bcount = PLAYER_BUMPER_MAX_SAMPLES;
72  
-      
73  
-      pdata.bumpers_count = bcount;
74  
-      
75  
-      for( int i=0; i<(int)bcount; i++ )
76  
-	pdata.bumpers[i] = sdata[i].hit ? 1 : 0;
77  
-    } 
78  
-  
79  
-  this->driver->Publish( this->addr, NULL,
80  
-			 PLAYER_MSGTYPE_DATA,
81  
-			 PLAYER_BUMPER_DATA_STATE,
82  
-			 &pdata, sizeof(pdata), NULL); 
83  
-}
84  
-
85  
-
86  
-int InterfaceBumper::ProcessMessage( MessageQueue* resp_queue,
87  
-				     player_msghdr_t* hdr,
88  
-				     void* data )
89  
-{  
90  
-  if( Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 
91  
-			    PLAYER_BUMPER_GET_GEOM, 
92  
-			    this->addr) )
93  
-    {
94  
-      size_t cfglen = mod->cfg_len;
95  
-      bumper_config_t* cfgs = (bumper_config_t*)mod->cfg;
96  
-      assert( cfgs );
97  
-      
98  
-      size_t bcount = cfglen / sizeof(bumper_config_t);
99  
-      
100  
-      // convert the bumper data into Player-format bumper poses	
101  
-      player_bumper_geom_t pgeom;
102  
-      memset( &pgeom, 0, sizeof(pgeom) );
103  
-      
104  
-      // limit the number of samples to Player's maximum
105  
-      if( bcount > PLAYER_BUMPER_MAX_SAMPLES )
106  
-	bcount = PLAYER_BUMPER_MAX_SAMPLES;
107  
-      
108  
-      pgeom.bumper_def_count = bcount;
109  
-      
110  
-      for( int i=0; i<(int)bcount; i++ )
111  
-	{
112  
-	  // fill in the geometry data formatted player-like
113  
-	  pgeom.bumper_def[i].pose.px = cfgs[i].pose.x;	  
114  
-	  pgeom.bumper_def[i].pose.py = cfgs[i].pose.y;	  
115  
-	  pgeom.bumper_def[i].pose.pa = cfgs[i].pose.a;	    
116  
-	  pgeom.bumper_def[i].length = cfgs[i].length;	    
117  
-	  pgeom.bumper_def[i].radius = 0; 
118  
-	}
119  
-      
120  
-      this->driver->Publish( this->addr, resp_queue, 
121  
-			     PLAYER_MSGTYPE_RESP_ACK, 
122  
-			     PLAYER_BUMPER_GET_GEOM,
123  
-			     (void*)&pgeom, sizeof(pgeom), NULL );
124  
-      
125  
-      return 0; // ok
126  
-    }
127  
-  else
128  
-    {
129  
-      // Don't know how to handle this message.
130  
-      PRINT_WARN2( "bumper doesn't support msg with type/subtype %d/%d",
131  
-		   hdr->type, hdr->subtype);
132  
-      return(-1);
133  
-    }    
134  
-}
135  
-
7  libstageplugin/p_driver.cc
@@ -478,10 +478,9 @@ StgDriver::StgDriver(ConfigFile* cf, int section)
478 478
 //  	  ifsrc = new InterfacePtz( player_addr,  this, cf, section );
479 479
 //  	  break;
480 480
 
481  
-// 	case PLAYER_BUMPER_CODE:
482  
-// 	  ifsrc = new InterfaceBumper( player_addr,  this, cf, section );
483  
-// 	  break;
484  
-
  481
+ 	case PLAYER_BUMPER_CODE:
  482
+ 	  ifsrc = new InterfaceBumper( player_addr,  this, cf, section );
  483
+ 	  break;
485 484
 
486 485
 	default:
487 486
 	  PRINT_ERR1( "error: stage driver doesn't support interface type %d\n",
12  worlds/irobot.inc
@@ -22,14 +22,16 @@ define roomba position
22 22
     point[13] [ 0.086 -0.208 ]
23 23
     point[14] [ 0.159 -0.159 ]
24 24
     point[15] [ 0.208 -0.086 ]
  25
+    z [0 0.1]
25 26
   )
26 27
 
27 28
   # this bumper array VERY crudely approximates the Roomba's bumpers
28  
-#  bumper( bcount 2  
29  
-#	  blength 0.33
30  
-#          bpose[0] [0.12  0.12  45]
31  
-#          bpose[1] [0.12 -0.12 -45] 
32  
-#        )
  29
+  bumper( size [.01 .01 .01 0]
  30
+          bcount 2  
  31
+          blength .33
  32
+          bpose[0] [0.12  0.12 .02  45]
  33
+          bpose[1] [0.12 -0.12 .02 -45] 
  34
+        )
33 35
 
34 36
   color "gray50"
35 37
 )
25  worlds/roomba.cfg
... ...
@@ -0,0 +1,25 @@
  1
+# Desc: Player sample configuration file for controlling Stage devices
  2
+#       Based on simple.cfg
  3
+# Author:  Rich Mattes
  4
+# Date: 12 December 2012
  5
+
  6
+# load the Stage plugin simulation driver
  7
+driver
  8
+(		
  9
+  name "stage"
  10
+  provides [ "simulation:0" ]
  11
+  plugin "stageplugin"
  12
+
  13
+  # load the named file into the simulator
  14
+  worldfile "roomba.world"	
  15
+)
  16
+
  17
+# Create a Stage driver that connects the roomba's sensors
  18
+# to Player interfaces
  19
+driver
  20
+( 
  21
+  name "stage"
  22
+  provides [ "position2d:0" "bumper:0" ]
  23
+  model "roomba0" 
  24
+)
  25
+
45  worlds/roomba.world
... ...
@@ -0,0 +1,45 @@
  1
+# Desc: Sample Stage world file with a simulated Roomba, based on simple.world
  2
+# Author:  Rich Mattes
  3
+# Date: 12 December 2012
  4
+
  5
+include "irobot.inc"
  6
+include "map.inc"
  7
+
  8
+# time to pause (in GUI mode) or quit (in headless mode (-g)) the simulation
  9
+quit_time 3600 # 1 hour of simulated time
  10
+
  11
+paused 1
  12
+
  13
+resolution 0.02
  14
+
  15
+# configure the GUI window
  16
+window
  17
+(
  18
+  size [ 635.000 666.000 ] # in pixels
  19
+  scale 36.995   # pixels per meter
  20
+  center [ -0.040  -0.274 ]
  21
+  rotate [ 0  0 ]
  22
+  			
  23
+  show_data 1              # 1=on 0=off
  24
+)
  25
+
  26
+# load an environment bitmap
  27
+floorplan
  28
+( 
  29
+  name "cave"
  30
+  size [16.000 16.000 0.800]
  31
+  pose [0 0 0 0]
  32
+  bitmap "bitmaps/cave.png"
  33
+)
  34
+
  35
+
  36
+roomba
  37
+(		  
  38
+  # can refer to the robot by this name
  39
+  name "r0"
  40
+  pose [ -6.946 -6.947 0 45.000 ] 
  41
+
  42
+  # report error-free position in world coordinates
  43
+  localization "gps"
  44
+  localization_origin [ 0 0 0 0 ]
  45
+)
Commit_comment_tip

Tip: You can add notes to lines in a file. Hover to the left of a line to make a note

Something went wrong with that request. Please try again.