Bumper model with Player interface #33

Closed
wants to merge 7 commits into
from
View
@@ -16,6 +16,7 @@ set( stageSrcs
model_actuator.cc
model_blinkenlight.cc
model_blobfinder.cc
+ model_bumper.cc
model_callbacks.cc
model_camera.cc
model_draw.cc
View
@@ -0,0 +1,263 @@
+///////////////////////////////////////////////////////////////////////////
+//
+// 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);
+
+
+ if( ! (bump->samples && bump->bumpers && bump->bumper_count) ){
+ return;
+ }
+
+ if (! bump->showBumperData ) {
+ return;
+ }
+ /*
+ * draw the sensitive area of the bumper, with a color indicating if it is hit.
+ */
+ for( unsigned int t=0; t< bump->bumper_count; t++ )
+ {
+ //This is how wide the active area rectangle will be.
+ float thickness=0.01;
+ glPushMatrix();
+ //draw the active area with a different color if it is hit
+ if (bump->samples[t].hit) {
+ glColor3f(1.0f, 0.0f, 0.0f);
+ } else {
+ glColor3f(0.0f, 1.0f, 0.0f);
+ }
+ const float rad2deg=180.0/M_PI;
+ glTranslatef(bump->bumpers[t].pose.x, bump->bumpers[t].pose.y,0);
+ glRotatef(bump->bumpers[t].pose.a*rad2deg, 0, 0, 1);
+ glRectf(-thickness/2.0f, -bump->bumpers[t].length/2.0,
+ thickness/2.0f, bump->bumpers[t].length/2.0 );
+ glPopMatrix();
+ }
+
+}
View
@@ -2641,20 +2641,56 @@ namespace Stg
};
- // \todo BUMPER MODEL --------------------------------------------------------
- // typedef struct
- // {
- // Pose pose;
- // meters_t length;
- // } bumper_config_t;
-
- // typedef struct
- // {
- // Model* hit;
- // point_t hit_point;
- // } bumper_sample_t;
+ // BUMPER MODEL --------------------------------------------------------
+ /// %ModelBumper class
+ class ModelBumper : public Model
+ {
+ public:
+ class BumperConfig
+ {
+ public:
+ Pose pose;
+ meters_t length;
+ };
+
+ class BumperSample
+ {
+ public:
+ Model* hit;
+ point_t hit_point;
+ };
+
+
+ public:
+ ModelBumper( World* world,
+ Model* parent,
+ const std::string& type );
+ virtual ~ModelBumper();
+ virtual void Load();
+
+ uint32_t bumper_count;
+ BumperConfig* bumpers;
+ BumperSample* samples;
+
+ protected:
+ virtual void Startup();
+ virtual void Shutdown();
+ virtual void Update();
+ virtual void Print (char *prefix);
+
+ class BumperVis : public Visualizer
+ {
+ public:
+ BumperVis();
+ virtual ~BumperVis();
+ virtual void Visualize( Model* mod, Camera* cam);
+ } bumpervis;
+
+ private:
+ static Option showBumperData;
+ };
// FIDUCIAL MODEL --------------------------------------------------------
View
@@ -23,6 +23,7 @@ void Stg::RegisterModels()
Register( "actuator", Creator<ModelActuator> );
Register( "blinkenlight", Creator<ModelBlinkenlight> );
Register( "blobfinder", Creator<ModelBlobfinder> );
+ Register( "bumper", Creator<ModelBumper> );
Register( "camera", Creator<ModelCamera> );
Register( "fiducial", Creator<ModelFiducial> );
Register( "gripper", Creator<ModelGripper> );
@@ -8,6 +8,7 @@ set( stagepluginSrcs
p_driver.cc
p_actarray.cc
p_blobfinder.cc
+ p_bumper.cc
p_camera.cc
p_gripper.cc
p_simulation.cc
Oops, something went wrong.