[Playerstage-commit] code/stage/src model_fiducial.cc, NONE, 1.2.2.1 model_ranger.cc, NONE, 1.1.2.1

[Richard V. <rt...@us...>]

Update of /cvsroot/playerstage/code/stage/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv2675

Added Files:
      Tag: opengl
	model_fiducial.cc model_ranger.cc 
Log Message:
feshening up

--- NEW FILE: model_ranger.cc ---
///////////////////////////////////////////////////////////////////////////
//
// File: model_ranger.cc
// Author: Richard Vaughan
// Date: 10 June 2004
//
// CVS info:
//  $Source: /cvsroot/playerstage/code/stage/src/Attic/model_ranger.cc,v $
//  $Author: rtv $
//  $Revision: 1.1.2.1 $
//
///////////////////////////////////////////////////////////////////////////

/**
@ingroup model
@defgroup model_ranger Ranger model 
The ranger model simulates an array of sonar or infra-red (IR) range sensors.

<h2>Worldfile properties</h2>

@par Summary and default values

@verbatim
ranger
(
  # ranger properties
  scount 16
  spose[0] [? ? ?]
  spose[1] [? ? ?]
  spose[2] [? ? ?]
  spose[3] [? ? ?]
  spose[4] [? ? ?]
  spose[5] [? ? ?]
  spose[6] [? ? ?]
  spose[7] [? ? ?]
  spose[8] [? ? ?]
  spose[9] [? ? ?]
  spose[10] [? ? ?]
  spose[11] [? ? ?]
  spose[12] [? ? ?]
  spose[13] [? ? ?]
  spose[14] [? ? ?]
  spose[15] [? ? ?]
   
  ssize [0.01 0.03]
  sview [0.0 5.0 5.0]

  # model properties
  watts 2.0
)
@endverbatim

@par Notes

The ranger model allows configuration of the pose, size and view parameters of each transducer seperately (using spose[index], ssize[index] and sview[index]). However, most users will set a common size and view (using ssize and sview), and just specify individual transducer poses.

@par Details
- scount int 
  - the number of range transducers
- spose[\<transducer index\>] [float float float]
  - [x y theta] 
  - pose of the transducer relative to its parent.
- ssize [float float]
  - [x y] 
  - size in meters. Has no effect on the data, but controls how the sensor looks in the Stage window.
- ssize[\<transducer index\>] [float float]
  - per-transducer version of the ssize property. Overrides the common setting.
- sview [float float float]
   - [range_min range_max fov] 
   - minimum range and maximum range in meters, field of view angle in degrees. Currently fov has no effect on the sensor model, other than being shown in the confgiuration graphic for the ranger device.
- sview[\<transducer index\>] [float float float]
  - per-transducer version of the sview property. Overrides the common setting.

*/

#define DEBUG

#include <math.h>
#include "model.hh"
#include "gui.h"
#include <GL/gl.h>

#define STG_RANGER_WATTS 2.0 // ranger power consumption

StgModelRanger::StgModelRanger( stg_world_t* world, 
				StgModel* parent,
				stg_id_t id,
				CWorldFile* wf )
  : StgModel( world, parent, id, wf )
{
  PRINT_DEBUG2( "Constructing StgModelRanger %d (%s)\n", 
		id, wf->GetEntityType( id ) );
  

  
  
  // Set up sensible defaults
  
  stg_color_t col = stg_lookup_color( STG_RANGER_CONFIG_COLOR );
  this->SetColor( col );
  
  // remove the polygon: ranger has no body
  this->ClearBlocks();

  //stg_geom_t geom;
  //memset( &geom, 0, sizeof(geom)); // no size
  //this->SetGeom( &geom );
  
  sensor_count = 16;
  sensors= g_new0( stg_ranger_sensor_t, sensor_count );

  double offset = MIN(geom.size.x, geom.size.y) / 2.0;

  // create default ranger config
  int c;
  for( c=0; c<sensor_count; c++ )
    {
      sensors[c].pose.a = (2.0*M_PI)/sensor_count * c;
      sensors[c].pose.x = offset * cos( sensors[c].pose.a );
      sensors[c].pose.y = offset * sin( sensors[c].pose.a );
      
      sensors[c].size.x = 0.01; // a small device
      sensors[c].size.y = 0.04;
      
      sensors[c].bounds_range.min = 0;
      sensors[c].bounds_range.max = 5.0;
      sensors[c].fov = (2.0*M_PI)/(sensor_count+1);
      sensors[c].ray_count = 3;
    }
}

StgModelRanger::~StgModelRanger()
{
}

void StgModelRanger::Startup( void )
{
  StgModel::Startup();
  
  PRINT_DEBUG( "ranger startup" );
  
  this->SetWatts( STG_RANGER_WATTS );
}


void StgModelRanger::Shutdown( void )
{
  PRINT_DEBUG( "ranger shutdown" );

  this->SetWatts( 0 );
  //this->SetData( NULL, 0 ); // this will unrender data too.

  StgModel::Shutdown();
}

void StgModelRanger::Load( void )
{
  StgModel::Load();
  
  if( wf->PropertyExists(id, "scount" ) )
    {
      PRINT_DEBUG( "Loading ranger array" );
      puts( "Loading ranger array" );

      // Load the geometry of a ranger array
      sensor_count = wf->ReadInt( id, "scount", 0);
      assert( sensor_count > 0 );
      
      char key[256];
      sensors = g_renew( stg_ranger_sensor_t, sensors, sensor_count );
      
      stg_size_t common_size;
      common_size.x = wf->ReadTupleLength( id, "ssize", 0, sensors[0].size.x );
      common_size.y = wf->ReadTupleLength( id, "ssize", 1, sensors[0].size.y );
      
      double common_min = wf->ReadTupleLength( id, "sview", 0, sensors[0].bounds_range.min );
      double common_max = wf->ReadTupleLength( id, "sview", 1, sensors[0].bounds_range.max );
      double common_fov = wf->ReadTupleAngle( id, "sview", 2, sensors[0].fov );

      int common_ray_count = wf->ReadInt( id, "sraycount", sensors[0].ray_count );

      // set all transducers with the common settings
      int i;
      for(i = 0; i < sensor_count; i++)
	{
	  sensors[i].size.x = common_size.x;
	  sensors[i].size.y = common_size.y;
	  sensors[i].bounds_range.min = common_min;
	  sensors[i].bounds_range.max = common_max;
	  sensors[i].fov = common_fov;
	  sensors[i].ray_count = common_ray_count;	  
	}

      // TODO - do this properly, without the hard-coded defaults
      // allow individual configuration of transducers
      for(i = 0; i < sensor_count; i++)
	{
	  snprintf(key, sizeof(key), "spose[%d]", i);
	  sensors[i].pose.x = wf->ReadTupleLength( id, key, 0, 0);
	  sensors[i].pose.y = wf->ReadTupleLength( id, key, 1, 0);
	  sensors[i].pose.a = wf->ReadTupleAngle( id, key, 2, 0);
	  
/* 	  snprintf(key, sizeof(key), "ssize[%d]", i); */
/* 	  sensors[i].size.x = wf->ReadTuplelength(mod->id, key, 0, 0.01); */
/* 	  sensors[i].size.y = wf->ReadTuplelength(mod->id, key, 1, 0.05); */
	  
/* 	  snprintf(key, sizeof(key), "sview[%d]", i); */
/* 	  sensors[i].bounds_range.min = */
/* 	    wf->ReadTuplelength(mod->id, key, 0, 0); */
/* 	  sensors[i].bounds_range.max =   // set up sensible defaults */

/* 	    wf->ReadTuplelength(mod->id, key, 1, 5.0); */
/* 	  sensors[i].fov */
/* 	    = DTOR(wf->ReadTupleangle(mod->id, key, 2, 5.0 )); */


/* 	  sensors[i].ray_count = common_ray_count; */

	}
      
      PRINT_DEBUG1( "loaded %d ranger configs", sensor_count );	  
    }
}

int ranger_raytrace_match( StgModel* mod, StgModel* hitmod )
{
  // Ignore myself, my children, and my ancestors.
  return( hitmod->RangerReturn() );//&& !stg_model_is_related(mod,hitmod) );
}	

void StgModelRanger::Update( void )
{     
  StgModel::Update();
  
  if( this->subs < 1 )
    return;
  
  if( (sensors == NULL) || (sensor_count < 1 ))
    return;

  PRINT_DEBUG2( "[%d] updating ranger %s", (int)world->sim_time_ms, token );
  
  // raytrace new range data for all sensors
  for( unsigned int t=0; t<sensor_count; t++ )
    {
      // get the sensor's pose in global coords
      stg_pose_t pz;
      memcpy( &pz, &sensors[t].pose, sizeof(pz) ); 
      this->LocalToGlobal( &pz );
      
      double range = sensors[t].bounds_range.max;
      
      int r;
      for( r=0; r<sensors[t].ray_count; r++ )
	{	  
	  double angle_per_ray = sensors[t].fov / sensors[t].ray_count;
	  double ray_angle = -sensors[t].fov/2.0 + angle_per_ray * r + angle_per_ray/2.0;
	  
	  stg_meters_t hitrange, hitx, hity;
	  
	  // if the ray hits anything, 'range' will be changed
	  if( stg_first_model_on_ray( pz.x, pz.y, pz.z + 0.01, // TODO: UNHACK 
				      pz.a + ray_angle, 
				      sensors[t].bounds_range.max, 
				      world->matrix, 
				      PointToBearingRange, 
				      ranger_raytrace_match,
				      this,
				      &hitrange, &hitx, &hity ) )
	    {
	      if( hitrange < range )
		range = hitrange;
	    } 
	}
      // low-threshold the range
      if( range < sensors[t].bounds_range.min )
	range = sensors[t].bounds_range.min;	
      
      sensors[t].range = range;
      //sensors[t].error = TODO;
    }   
  
  // data graphics will be updated before being drawn onscreen again
  data_dirty = true;
}

/*
int ranger_noise_test( stg_ranger_sample_t* data, size_t count,  )
{
  int s;
  for( s=0; s<count; s++ )
    {
      // add 10mm random error
      ranges[s].range *= 0.1 * drand48();
    }
}
*/

void StgModelRanger::Print( char* prefix )
{
  StgModel::Print( prefix );
    
  printf( "\tRanges[ " );
  
  for( unsigned int i=0; i<sensor_count; i++ )
    printf( "%.2f ", sensors[i].range );
  puts( " ]" );
}


void StgModelRanger::DListData( void )
{
  // recreate the display list for this data
  glNewList( this->dl_data, GL_COMPILE );
  
  if( sensors && sensor_count ) 
    {
      // now get on with rendering the ranger data 
      glPolygonMode( GL_FRONT_AND_BACK, 
		     world->win->show_alpha ? GL_FILL : GL_LINES );
      push_color_rgba( 1, 0, 0, 0.1 ); // transparent pale red 
      glDepthMask( GL_FALSE ); 
      
      // draw the range  beams 
      for( unsigned int s=0; s<sensor_count; s++ ) 
	{ 
	  if( sensors[s].range > 0.0 ) 
	    { 
	      stg_ranger_sensor_t* rngr = &sensors[s]; 
	      
	      // sensor FOV 
	      double sidelen = sensors[s].range; 
	      double da = rngr->fov/2.0; 
	      
	      double x1= rngr->pose.x + sidelen*cos(rngr->pose.a - da ); 
	      double y1= rngr->pose.y + sidelen*sin(rngr->pose.a - da ); 
	      double x2= rngr->pose.x + sidelen*cos(rngr->pose.a + da ); 
	      double y2= rngr->pose.y + sidelen*sin(rngr->pose.a + da ); 
	      
	      glBegin( GL_POLYGON); 		
	        glVertex3f( rngr->pose.x, rngr->pose.y, 0 ); 
	        glVertex3f( x1, y1, 0 ); 
	        glVertex3f( x2, y2, 0 ); 
	      glEnd();	   	      
	    } 
	} 

      // draw the hitpoints
//       push_color_rgba( 1, 0, 0, 1.0 );
//       glPointSize( 3.0 );
//       glBegin(GL_POINTS); 		
      
//       for( unsigned int s=0; s<sensor_count; s++ ) 
// 	{ 
// 	  if( sensors[s].range > 0.0 ) 
// 	    { 
// 	      stg_ranger_sensor_t* rngr = &sensors[s]; 
// 	      double sidelen = sensors[s].range; 
// 	      double x1= rngr->pose.x + sidelen*cos( rngr->pose.a ); 
// 	      double y1= rngr->pose.y + sidelen*sin( rngr->pose.a ); 
	      
// 	      glVertex3f( x1, y1, 0 ); 		
// 	    } 
// 	} 

//       glEnd();	   
//       pop_color();
      
      
      // restore state 
      glDepthMask( GL_TRUE ); 
      pop_color(); 
    }
  
  glEndList();
}
   
   

--- NEW FILE: model_fiducial.cc ---
///////////////////////////////////////////////////////////////////////////
//
// File: model_fiducial.c
// Author: Richard Vaughan
// Date: 10 June 2004
//
// CVS info:
//  $Source: /cvsroot/playerstage/code/stage/src/Attic/model_fiducial.cc,v $
//  $Author: rtv $
//  $Revision: 1.2.2.1 $
//
///////////////////////////////////////////////////////////////////////////

//#define DEBUG 

#include <assert.h>
#include <math.h>
#include "stage_internal.h"
#include "gui.h"

#define STG_FIDUCIALS_MAX 64
#define STG_DEFAULT_FIDUCIAL_RANGEMIN 0
#define STG_DEFAULT_FIDUCIAL_RANGEMAXID 5
#define STG_DEFAULT_FIDUCIAL_RANGEMAXANON 8
#define STG_DEFAULT_FIDUCIAL_FOV DTOR(180)

const double STG_FIDUCIAL_WATTS = 10.0;

/** 
@ingroup model
@defgroup model_fiducial Fiducial detector model

The fiducial model simulates a fiducial-detecting device.

<h2>Worldfile properties</h2>

@par Summary and default values

@verbatim
fiducialfinder
(
  # fiducialfinder properties
  range_min 0.0
  range_max 8.0
  range_max_id 5.0
  fov 180.0

  # model properties
  size [0 0]
)
@endverbatim

@par Details
- range_min float
  - the minimum range reported by the sensor, in meters. The sensor will detect objects closer than this, but report their range as the minimum.
- range_max float
  - the maximum range at which the sensor can detect a fiducial, in meters. The sensor may not be able to uinquely identify the fiducial, depending on the value of range_max_id.
- range_max_id float
  - the maximum range at which the sensor can detect the ID of a fiducial, in meters.
- fov float
  - the angular field of view of the scanner, in degrees. 

*/

StgModelFiducial::StgModelFiducial( stg_world_t* world, 
			      StgModel* parent,
			      stg_id_t id,
			      CWorldFile* wf )
  : StgModel( world, parent, id, wf )
{
  PRINT_DEBUG2( "Constructing StgModelFiducial %d (%s)\n", 
		id, wf->GetEntityType( id ) );
  
  // sensible fiducial defaults 
  this->update_interval_ms = 200; // common for a SICK LMS200

  stg_geom_t geom; 
  memset( &geom, 0, sizeof(geom));
  geom.size.x = STG_DEFAULT_FIDUCIAL_SIZEX;
  geom.size.y = STG_DEFAULT_FIDUCIAL_SIZEY;
  geom.size.z = STG_DEFAULT_FIDUCIAL_SIZEZ;
  this->SetGeom( &geom );

  // set default color
  this->SetColor( stg_lookup_color(STG_FIDUCIAL_GEOM_COLOR));

  // set up a fiducial-specific config structure
  stg_fiducial_config_t lconf;
  memset(&lconf,0,sizeof(lconf));  
  lconf.range_min   = STG_DEFAULT_FIDUCIAL_MINRANGE;
  lconf.range_max   = STG_DEFAULT_FIDUCIAL_MAXRANGE;
  lconf.fov         = STG_DEFAULT_FIDUCIAL_FOV;
  lconf.samples     = STG_DEFAULT_FIDUCIAL_SAMPLES;  
  lconf.resolution = 1;
  this->SetCfg( &lconf, sizeof(lconf) );

  this->SetData( NULL, 0 );
}

  // no body
  stg_geom_t geom;
  memset( &geom, 0, sizeof(geom));
  stg_model_set_geom( mod, &geom );    
  stg_model_set_polygons( mod, NULL, 0 );

  // start with no data
  stg_model_set_data( mod, NULL, 0 );

  // default parameters
  stg_fiducial_config_t cfg; 
  cfg.min_range = STG_DEFAULT_FIDUCIAL_RANGEMIN;
  cfg.max_range_anon = STG_DEFAULT_FIDUCIAL_RANGEMAXANON;
  cfg.max_range_id = STG_DEFAULT_FIDUCIAL_RANGEMAXID;
  cfg.fov = STG_DEFAULT_FIDUCIAL_FOV;  
  stg_model_set_cfg( mod, &cfg, sizeof(cfg) );
  
  gui_fiducial_init( mod );
 
  return 0;
}

typedef struct 
{
  stg_model_t* mod;
  stg_pose_t pose;
  stg_fiducial_config_t cfg;
  GArray* fiducials;
} model_fiducial_buffer_t;


int fiducial_raytrace_match( stg_model_t* mod, stg_model_t* hitmod )
{
  // Ignore myself, my children, and my ancestors.
  return( !stg_model_is_related(mod,hitmod) );
}	


void model_fiducial_check_neighbor( gpointer key, 
				    stg_model_t* him,  
				    model_fiducial_buffer_t* mfb )
{
  PRINT_DEBUG2( "Model %s inspected model %s", 
		mfb->mod->token, him->token );
  
  // dont' compare a model with itself
  if( him == mfb->mod )
    {
      PRINT_DEBUG1( "  but model %s is itself", him->token);
      return;
    }

  // and don't consider models with invalid returns  
  if( him->fiducial_return == 0 )
    {
      PRINT_DEBUG1( "  but model %s has a zero fiducial ID", him->token);
      return;
    }

  // check to see if this neighbor has the right fiducial key
  if( him->fiducial_key != mfb->mod->fiducial_key )
    {
      PRINT_DEBUG1( "  but model %s doesn't match the fiducial key", him->token);
      return;
    }

  // are we within range?
  stg_pose_t hispose;
  stg_model_get_global_pose( him, &hispose );  
  double dx = hispose.x - mfb->pose.x;
  double dy = hispose.y - mfb->pose.y;  
  double range = hypot( dy, dx );
  if( range > mfb->cfg.max_range_anon )
    {
      PRINT_DEBUG1( "  but model %s is outside my range", him->token);
      return;
    }
  
  // is he in my field of view?
  double hisbearing = atan2( dy, dx );
  double dif = mfb->pose.a - hisbearing;

  if( fabs(NORMALIZE(dif)) > mfb->cfg.fov/2.0 )
    {
      PRINT_DEBUG1( "  but model %s is outside my FOV", him->token);
      return;
    }
   
  // now check if we have line-of-sight
  itl_t *itl = itl_create( mfb->pose.x, mfb->pose.y,
			   hispose.x, hispose.y, 
			   him->world->matrix, PointToPoint );
  
  stg_model_t* hitmod = 
    itl_first_matching( itl, fiducial_raytrace_match, mfb->mod );
  
  itl_destroy( itl );

  if( hitmod )
    PRINT_DEBUG2( "I saw %s with fid %d",
		  hitmod->token, hitmod->fiducial_return );
  
  // if it was him, we can see him
  if( hitmod == him )
    {
      stg_geom_t hisgeom;
      stg_model_get_geom(him,&hisgeom);

      // record where we saw him and what he looked like
      stg_fiducial_t fid;      
      fid.range = range;
      fid.bearing = NORMALIZE( hisbearing - mfb->pose.a);
      fid.geom.x = hisgeom.size.x;
      fid.geom.y = hisgeom.size.y;
      fid.geom.a = NORMALIZE( hispose.a - mfb->pose.a);

      // store the global pose of the fiducial (mainly for the GUI)
      memcpy( &fid.pose, &hispose, sizeof(fid.pose));
      
      // if he's within ID range, get his fiducial.return value, else
      // we see value 0
      fid.id = range < mfb->cfg.max_range_id ? hitmod->fiducial_return : 0;

      PRINT_DEBUG2( "adding %s's value %d to my list of fiducials",
      	    him->token, him->fiducial_return );

      g_array_append_val( mfb->fiducials, fid );
    }
}


///////////////////////////////////////////////////////////////////////////
// Update the beacon data
//
int fiducial_update( stg_model_t* mod, void* unused )
{
  PRINT_DEBUG( "fiducial update" );

  if( mod->subs < 1 )
    return 0;

  model_fiducial_buffer_t mfb;
  memset( &mfb, 0, sizeof(mfb) );
  
  mfb.mod = mod;

  stg_fiducial_config_t* cfg = (stg_fiducial_config_t*)mod->cfg;
  assert(cfg);

  memcpy( &mfb.cfg, cfg, sizeof(mfb.cfg));
  
  mfb.fiducials = g_array_new( FALSE, TRUE, sizeof(stg_fiducial_t) );
  stg_model_get_global_pose( mod, &mfb.pose );
  
  // for all the objects in the world
  g_hash_table_foreach( mod->world->models, 
			(GHFunc)(model_fiducial_check_neighbor), &mfb );

  PRINT_DEBUG2( "model %s saw %d fiducials", mod->token, mfb.fiducials->len );
  
  stg_model_set_data( mod, mfb.fiducials->data, 
		      mfb.fiducials->len * sizeof(stg_fiducial_t) );
  
  g_array_free( mfb.fiducials, TRUE );

  return 0;
}

void StgModelFiducial::Load( void )
{  
  if( wf->PropertyExists( mod->id, "range_min" ) ||
      wf_property_exists( mod->id, "range_max" ) ||
      wf_property_exists( mod->id, "range_max_id") ||
      wf_property_exists( mod->id, "fov" ) )
    {
      stg_fiducial_config_t* now = (stg_fiducial_config_t*)mod->cfg; 
      
      stg_fiducial_config_t cfg;
      memset( &cfg, 0, sizeof(cfg) );
      
      cfg.fov = 
	wf_read_angle(mod->id, "fov", now->fov );
      
      cfg.min_range = 
	wf_read_length(mod->id, "range_min", now->min_range );
      
      cfg.max_range_anon = 
	wf_read_length(mod->id, "range_max", now->max_range_anon );
      
      cfg.max_range_id = 
	wf_read_length(mod->id, "range_max_id", now->max_range_id );

      stg_model_set_cfg( mod, &cfg, sizeof(cfg));
    }
  return 0;
}