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;
}
-------------------------------------------------------------------------
This SF.net email is sponsored by: Splunk Inc.
Still grepping through log files to find problems? Stop.
Now Search log events and configuration files using AJAX and a browser.
Download your FREE copy of Splunk now >> http://get.splunk.com/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
