Revision: 7766
http://playerstage.svn.sourceforge.net/playerstage/?rev=7766&view=rev
Author: rtv
Date: 2009-06-03 06:26:12 +0000 (Wed, 03 Jun 2009)
Log Message:
-----------
cleaning up the class structure - some API changes will need to be tracked by
RAPI
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/examples/ctrl/lasernoise.cc
code/stage/trunk/examples/ctrl/wander.cc
code/stage/trunk/libstage/model_blobfinder.cc
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/model_ranger.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstageplugin/p_blobfinder.cc
code/stage/trunk/libstageplugin/p_laser.cc
code/stage/trunk/libstageplugin/p_sonar.cc
code/stage/trunk/webstage/webstage.cc
code/stage/trunk/worlds/benchmark/expand.cc
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2009-06-03 02:29:12 UTC (rev
7765)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2009-06-03 06:26:12 UTC (rev
7766)
@@ -215,7 +215,7 @@
// Get the data
uint32_t sample_count=0;
- stg_laser_sample_t* scan = laser->GetSamples( &sample_count );
+ ModelLaser::Sample* scan = laser->GetSamples( &sample_count );
for (uint32_t i = 0; i < sample_count; i++)
{
@@ -453,7 +453,7 @@
static int BlobFinderUpdate( ModelBlobfinder* blobmod, Robot* robot )
{
unsigned int blob_count = 0;
- stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &blob_count );
+ ModelBlobfinder::Blob* blobs = blobmod->GetBlobs( &blob_count );
if( blobs && (blob_count>0) )
{
Modified: code/stage/trunk/examples/ctrl/lasernoise.cc
===================================================================
--- code/stage/trunk/examples/ctrl/lasernoise.cc 2009-06-03 02:29:12 UTC
(rev 7765)
+++ code/stage/trunk/examples/ctrl/lasernoise.cc 2009-06-03 06:26:12 UTC
(rev 7766)
@@ -25,7 +25,7 @@
{
// get the data
uint32_t sample_count=0;
- stg_laser_sample_t* scan = mod->GetSamples( &sample_count );
+ ModelLaser::Sample* scan = mod->GetSamples( &sample_count );
if( scan )
for( unsigned int i=0; i<sample_count; i++ )
Modified: code/stage/trunk/examples/ctrl/wander.cc
===================================================================
--- code/stage/trunk/examples/ctrl/wander.cc 2009-06-03 02:29:12 UTC (rev
7765)
+++ code/stage/trunk/examples/ctrl/wander.cc 2009-06-03 06:26:12 UTC (rev
7766)
@@ -41,7 +41,7 @@
{
// get the data
uint32_t sample_count=0;
- stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count );
+ ModelLaser::Sample* scan = robot->laser->GetSamples( &sample_count );
if( ! scan )
return 0;
Modified: code/stage/trunk/libstage/model_blobfinder.cc
===================================================================
--- code/stage/trunk/libstage/model_blobfinder.cc 2009-06-03 02:29:12 UTC
(rev 7765)
+++ code/stage/trunk/libstage/model_blobfinder.cc 2009-06-03 06:26:12 UTC
(rev 7766)
@@ -81,36 +81,26 @@
*/
-ModelBlobfinder::ModelBlobfinder( World* world,
-
Model* parent )
- : Model( world, parent, MODEL_TYPE_BLOBFINDER )
+ ModelBlobfinder::ModelBlobfinder( World* world,
+
Model* parent )
+ : Model( world, parent, MODEL_TYPE_BLOBFINDER ),
+ blobs(),
+
colors(),
+ fov(
DEFAULT_BLOBFINDERFOV ),
+ pan(
DEFAULT_BLOBFINDERPAN ),
+ range(
DEFAULT_BLOBFINDERRANGE ),
+
scan_height( DEFAULT_BLOBFINDERSCANHEIGHT ),
+
scan_width( DEFAULT_BLOBFINDERSCANWIDTH )
{
PRINT_DEBUG2( "Constructing ModelBlobfinder %d (%s)\n",
- id, typestr );
-
- scan_width = DEFAULT_BLOBFINDERSCANWIDTH;
- scan_height = DEFAULT_BLOBFINDERSCANHEIGHT;
- fov = DEFAULT_BLOBFINDERFOV;
- range = DEFAULT_BLOBFINDERRANGE;
-
+ id, typestr );
ClearBlocks();
-
- blobs = g_array_new( false, true, sizeof(stg_blobfinder_blob_t));
-
- // leave the color filter array empty initally - this tracks all colors
- colors = g_array_new( false, true, sizeof(stg_color_t) );
-
RegisterOption( &showBlobData );
}
ModelBlobfinder::~ModelBlobfinder( void )
{
- if( blobs )
- g_array_free( blobs, true );
-
- if( colors )
- g_array_free( colors, true );
}
static bool blob_match( Model* candidate,
@@ -128,22 +118,26 @@
void ModelBlobfinder::ModelBlobfinder::AddColor( stg_color_t col )
{
- g_array_append_val( colors, col );
+ colors.push_back( col );
}
/** Stop tracking blobs with this color */
void ModelBlobfinder::RemoveColor( stg_color_t col )
{
- for( unsigned int i=0; i<colors->len; i++ )
- if( col == g_array_index( colors, stg_color_t, i ) )
- g_array_remove_index_fast( colors, i );
+ for( std::vector<stg_color_t>::iterator it = colors.begin();
+ it != colors.end();
+ ++it )
+ {
+ if( (*it) == col )
+ it = colors.erase(it);
+ }
}
/** Stop tracking all colors. Call this to clear the defaults, then
add colors individually with AddColor(); */
void ModelBlobfinder::RemoveAllColors()
{
- g_array_set_size( colors, 0 );
+ colors.clear();
}
void ModelBlobfinder::Load( void )
@@ -191,7 +185,7 @@
// now the colors and ranges are filled in - time to do blob detection
double yRadsPerPixel = fov / scan_height;
- g_array_set_size( blobs, 0 );
+ blobs.clear();
// scan through the samples looking for color blobs
for(unsigned int s=0; s < scan_width; s++ )
@@ -220,12 +214,12 @@
unsigned int left = s - 1;
//if we have color filters in place, check to see if we're
looking for this color
- if( colors->len )
+ if( colors.size() )
{
bool found = false;
- for( unsigned int c=0; c<colors->len; c++ )
- if( ColorMatchIgnoreAlpha( blobcol,
g_array_index( colors, stg_color_t, c )))
+ for( unsigned int c=0; c<colors.size(); c++ )
+ if( ColorMatchIgnoreAlpha( blobcol, colors[c]))
{
found = true;
break;
@@ -253,8 +247,7 @@
blobbottom = MIN( blobbottom, (int)scan_height );
// fill in an array entry for this blob
- stg_blobfinder_blob_t blob;
- memset( &blob, 0, sizeof(blob) );
+ Blob blob;
blob.color = blobcol;
blob.left = scan_width - left - 1;
blob.top = blobtop;
@@ -266,12 +259,11 @@
// mod, blob.color, blob.xpos, blob.ypos );
// add the blob to our stash
- g_array_append_val( blobs, blob );
+ //g_array_append_val( blobs, blob );
+ blobs.push_back( blob );
}
delete [] samples;
-
- //data_dirty = true;
}
@@ -294,8 +286,7 @@
SetWatts( 0 );
// clear the data - this will unrender it too
- if( blobs )
- g_array_set_size( blobs, 0 );
+ blobs.clear();
Model::Shutdown();
}
@@ -362,10 +353,11 @@
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
// draw the blobs on the screen
- for( unsigned int s=0; s<blobs->len; s++ )
+ for( unsigned int s=0; s<blobs.size(); s++ )
{
- stg_blobfinder_blob_t* b =
- &g_array_index( blobs, stg_blobfinder_blob_t, s);
+ Blob* b = &blobs[s];
+ //stg_blobfinder_blob_t* b =
+ //&g_array_index( blobs, stg_blobfinder_blob_t, s);
PushColor( b->color );
glRectf( b->left, b->top, b->right, b->bottom );
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2009-06-03 02:29:12 UTC (rev
7765)
+++ code/stage/trunk/libstage/model_laser.cc 2009-06-03 06:26:12 UTC (rev
7766)
@@ -87,18 +87,20 @@
void ModelLaser::Vis::Visualize( Model* mod, Camera* cam )
{
ModelLaser* laser = dynamic_cast<ModelLaser*>(mod);
- unsigned int sample_count = 0;
- stg_laser_sample_t* samples = laser->GetSamples( &sample_count );
+
+ const std::vector<Sample>& samples( laser->GetSamples() );
+ size_t sample_count = samples.size();
+
glPushMatrix();
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
glTranslatef( 0,0, laser->GetGeom().size.z/2.0 ); // shoot the laser beam
out at the right height
// pack the laser hit points into a vertex array for fast rendering
- static float* pts = NULL;
- pts = (float*)g_realloc( pts, 2 * (sample_count+1) * sizeof(float));
-
+ static std::vector<GLfloat> pts;
+ pts.resize( 2 * (sample_count+1) );
+
pts[0] = 0.0;
pts[1] = 0.0;
@@ -121,7 +123,7 @@
}
}
- glVertexPointer( 2, GL_FLOAT, 0, pts );
+ glVertexPointer( 2, GL_FLOAT, 0, &pts[0] );
laser->PopColor();
@@ -186,20 +188,17 @@
Model* parent )
: Model( world, parent, MODEL_TYPE_LASER ),
vis( world ),
- data_dl(0),
- data_dirty( true ),
sample_count( DEFAULT_SAMPLES ),
samples(),
+ rays(),
range_max( DEFAULT_MAXRANGE ),
fov( DEFAULT_FOV ),
- resolution( DEFAULT_RESOLUTION ),
- rays()
+ resolution( DEFAULT_RESOLUTION )
{
PRINT_DEBUG2( "Constructing ModelLaser %d (%s)\n",
id, typestr );
-
// Model data members
interval = DEFAULT_INTERVAL_MS * (int)thousand;
@@ -246,9 +245,9 @@
SampleConfig();
}
-stg_laser_cfg_t ModelLaser::GetConfig()
+ModelLaser::Config ModelLaser::GetConfig()
{
- stg_laser_cfg_t cfg;
+ Config cfg;
cfg.sample_count = sample_count;
cfg.range_bounds.max = range_max;
cfg.fov = fov;
@@ -257,14 +256,14 @@
return cfg;
}
-void ModelLaser::SetConfig( stg_laser_cfg_t cfg )
+void ModelLaser::SetConfig( Config& cfg )
{
range_max = cfg.range_bounds.max;
fov = cfg.fov;
resolution = cfg.resolution;
interval = cfg.interval;
sample_count = cfg.sample_count;
-
+
SampleConfig();
}
@@ -310,9 +309,9 @@
// set up the ray origins in global coords
for( unsigned int t=0; t<sample_count; t += resolution )
{
- rays[t].origin = rayorg;
+ rays[t].origin = rayorg;
rayorg.a += sample_incr;
- }
+ }
// do the raytracing of all rays in one go (allows parallel implementation)
world->Raytrace( rays );
@@ -343,9 +342,7 @@
break;
// copy the rightmost sample data into this
point
- memcpy( &samples[t-g],
- &samples[t-resolution],
- sizeof(stg_laser_sample_t));
+ samples[t-g] = samples[t-resolution];
double left = samples[t].range;
double right = samples[t-resolution].range;
@@ -355,8 +352,6 @@
}
}
- data_dirty = true;
-
Model::Update();
}
@@ -389,19 +384,19 @@
printf( "\tReflectance[ " );
for( unsigned int i=0; i<sample_count; i++ )
- printf( "%.2f ", samples[i].reflectance );
+ printf( "%.2f ", samples[i].reflectance );
puts( " ]" );
}
-stg_laser_sample_t* ModelLaser::GetSamples( uint32_t* count )
+ModelLaser::Sample* ModelLaser::GetSamples( uint32_t* count )
{
// get a C style array from our vector
if( count ) *count = sample_count;
return &samples[0];
}
-const std::vector<stg_laser_sample_t>& ModelLaser::GetSamples()
+const std::vector<ModelLaser::Sample>& ModelLaser::GetSamples()
{
return samples;
}
Modified: code/stage/trunk/libstage/model_ranger.cc
===================================================================
--- code/stage/trunk/libstage/model_ranger.cc 2009-06-03 02:29:12 UTC (rev
7765)
+++ code/stage/trunk/libstage/model_ranger.cc 2009-06-03 06:26:12 UTC (rev
7766)
@@ -101,7 +101,6 @@
Option ModelRanger::showRangerData( "Ranger ranges", "show_ranger", "", true,
NULL );
Option ModelRanger::showRangerTransducers( "Ranger transducers",
"show_ranger_transducers", "", false, NULL );
-
ModelRanger::ModelRanger( World* world,
Model* parent )
: Model( world, parent, MODEL_TYPE_RANGER )
@@ -121,23 +120,20 @@
this->ClearBlocks();
Geom geom;
- memset( &geom, 0, sizeof(geom)); // no size
geom.size.x = RANGER_SIZEX;
geom.size.y = RANGER_SIZEY;
geom.size.z = RANGER_SIZEZ;
this->SetGeom( geom );
- samples = NULL;
- sensor_count = RANGER_SENSORCOUNT;
- sensors = new stg_ranger_sensor_t[sensor_count];
-
// spread the transducers around the ranger's body
double offset = MIN(geom.size.x, geom.size.y) / 2.0;
-
+
+ sensors.resize( RANGER_SENSORCOUNT );
+
// create default ranger config
- for( unsigned int c=0; c<sensor_count; c++ )
+ for( unsigned int c=0; c<sensors.size(); c++ )
{
- sensors[c].pose.a = (2.0*M_PI)/sensor_count * c;
+ sensors[c].pose.a = (2.0*M_PI)/sensors.size() * c;
sensors[c].pose.x = offset * cos( sensors[c].pose.a );
sensors[c].pose.y = offset * sin( sensors[c].pose.a );
sensors[c].pose.z = geom.size.z / 2.0; // half way up
@@ -148,8 +144,10 @@
sensors[c].bounds_range.min = RANGER_RANGEMIN;
sensors[c].bounds_range.max = RANGER_RANGEMAX;;
- sensors[c].fov = (2.0*M_PI)/(sensor_count+1);
+ sensors[c].fov = (2.0*M_PI)/(sensors.size()+1);
sensors[c].ray_count = RANGER_RAYCOUNT;
+
+ sensors[c].range = 0; // invalid range
}
RegisterOption( &showRangerData );
@@ -158,8 +156,6 @@
ModelRanger::~ModelRanger()
{
- if( sensors )
- delete[] sensors;
}
void ModelRanger::Startup( void )
@@ -168,7 +164,7 @@
PRINT_DEBUG( "ranger startup" );
- this->SetWatts( RANGER_WATTSPERSENSOR * sensor_count );
+ this->SetWatts( RANGER_WATTSPERSENSOR * sensors.size() );
}
@@ -178,12 +174,6 @@
this->SetWatts( 0 );
- if( this->samples )
- {
- delete[] samples;
- samples = NULL;
- }
-
Model::Shutdown();
}
@@ -196,13 +186,12 @@
PRINT_DEBUG( "Loading ranger array" );
// Load the geometry of a ranger array
- sensor_count = wf->ReadInt( wf_entity, "scount", 0);
+ int sensor_count = wf->ReadInt( wf_entity, "scount", 0);
assert( sensor_count > 0 );
char key[256];
-
- if( sensors ) delete [] sensors;
- sensors = new stg_ranger_sensor_t[sensor_count];
+
+ sensors.resize( sensor_count );
Size common_size;
common_size.x = wf->ReadTupleLength( wf_entity, "ssize", 0, RANGER_SIZEX
);
@@ -213,52 +202,53 @@
double common_fov = wf->ReadTupleAngle( wf_entity, "sview", 2, M_PI /
(double)sensor_count );
int common_ray_count = wf->ReadInt( wf_entity, "sraycount",
sensors[0].ray_count );
-
+
// set all transducers with the common settings
- for( unsigned int 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;
- }
+ for( unsigned int i = 0; i < sensors.size(); 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;
+ sensors[i].range = 0.0;
+ }
// TODO - do this properly, without the hard-coded defaults
// allow individual configuration of transducers
- for( unsigned int i = 0; i < sensor_count; i++)
- {
- snprintf(key, sizeof(key), "spose[%d]", i);
- sensors[i].pose.x = wf->ReadTupleLength( wf_entity, key, 0,
sensors[i].pose.x );
- sensors[i].pose.y = wf->ReadTupleLength( wf_entity, key, 1,
sensors[i].pose.y );
- sensors[i].pose.z = 0.0;
- sensors[i].pose.a = wf->ReadTupleAngle( wf_entity, key, 2,
sensors[i].pose.a );
-
- snprintf(key, sizeof(key), "spose3[%d]", i);
- sensors[i].pose.x = wf->ReadTupleLength( wf_entity, key, 0,
sensors[i].pose.x );
- sensors[i].pose.y = wf->ReadTupleLength( wf_entity, key, 1,
sensors[i].pose.y );
- sensors[i].pose.z = wf->ReadTupleLength( wf_entity, key, 2,
sensors[i].pose.z );
- sensors[i].pose.a = wf->ReadTupleAngle( wf_entity, key, 3,
sensors[i].pose.a );
-
- /* 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; */
-
- }
-
+ for( unsigned int i = 0; i < sensors.size(); i++)
+ {
+ snprintf(key, sizeof(key), "spose[%d]",
i);
+ sensors[i].pose.x =
wf->ReadTupleLength( wf_entity, key, 0, sensors[i].pose.x );
+ sensors[i].pose.y =
wf->ReadTupleLength( wf_entity, key, 1, sensors[i].pose.y );
+ sensors[i].pose.z = 0.0;
+ sensors[i].pose.a = wf->ReadTupleAngle(
wf_entity, key, 2, sensors[i].pose.a );
+
+ snprintf(key, sizeof(key),
"spose3[%d]", i);
+ sensors[i].pose.x =
wf->ReadTupleLength( wf_entity, key, 0, sensors[i].pose.x );
+ sensors[i].pose.y =
wf->ReadTupleLength( wf_entity, key, 1, sensors[i].pose.y );
+ sensors[i].pose.z =
wf->ReadTupleLength( wf_entity, key, 2, sensors[i].pose.z );
+ sensors[i].pose.a = wf->ReadTupleAngle(
wf_entity, key, 3, sensors[i].pose.a );
+
+ /* 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", (int)sensor_count );
}
}
@@ -270,36 +260,34 @@
// block->Model()->Token(), block->Model(), block->Model()->LaserReturn()
);
// Ignore myself, my children, and my ancestors.
- return( candidate->vis.ranger_return &&
- !candidate->IsRelated( finder ) );
+ return( candidate->vis.ranger_return && !candidate->IsRelated( finder ) );
}
void ModelRanger::Update( void )
{
Model::Update();
- if( (sensors == NULL) || (sensor_count < 1 ))
- return;
+ if( sensors.size() < 1 )
+ return;
- if( samples == NULL )
- samples = new stg_meters_t[sensor_count];
- assert( samples );
-
//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++ )
+ for( std::vector<Sensor>::iterator it = sensors.begin();
+ it != sensors.end();
+ ++it )
{
-
+ Sensor& s = *it;
+
// TODO - reinstate multi-ray rangers
//for( int r=0; r<sensors[t].ray_count; r++ )
//{
- stg_raytrace_result_t ray = Raytrace( sensors[t].pose,
- sensors[t].bounds_range.max,
- ranger_match,
- NULL );
-
- samples[t] = MAX( ray.range, sensors[t].bounds_range.min );
+ stg_raytrace_result_t ray = Raytrace( s.pose,
+
s.bounds_range.max,
+
ranger_match,
+
NULL );
+
+ s.range = MAX( ray.range, s.bounds_range.min );
//sensors[t].error = TODO;
}
}
@@ -323,78 +311,77 @@
printf( "\tRanges[ " );
- for( unsigned int i=0; i<sensor_count; i++ )
- printf( "%.2f ", samples[i] );
+ for( unsigned int i=0; i<sensors.size(); i++ )
+ printf( "%.2f ", sensors[i].range );
puts( " ]" );
}
void ModelRanger::DataVisualize( Camera* cam )
{
- if( ! (samples && sensors && sensor_count) )
+ if( sensors.size() < 1 )
return;
-
+
if( showRangerTransducers )
{
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
PushColor( 0,0,0,1 );
-
- for( unsigned int s=0; s<sensor_count; s++ )
- {
- if( samples[s] > 0.0 )
- {
- stg_ranger_sensor_t* rngr = &sensors[s];
+
+ for( unsigned int s=0; s<sensors.size(); s++ )
+ {
+ Sensor& rngr = sensors[s];
- glPointSize( 4 );
- glBegin( GL_POINTS );
- glVertex3f( rngr->pose.x, rngr->pose.y, rngr->pose.z );
- glEnd();
- }
- }
+ if( rngr.range > 0.0 )
+ {
+
+ glPointSize( 4 );
+ glBegin( GL_POINTS );
+ glVertex3f(
rngr.pose.x, rngr.pose.y, rngr.pose.z );
+ glEnd();
+ }
+ }
PopColor();
}
-
+
if ( !showRangerData )
return;
- // if all models have the same number of sensors, this is fast as
- // it will probably not use a system call
- static float* pts = NULL;
- size_t memsize = 9 * sensor_count * sizeof(float);
- pts = (float*)g_realloc( pts, memsize );
- bzero( pts, memsize );
-
+ // if sequential models have the same number of sensors, this is
+ // fast as it will probably not use a system call
+ static std::vector<GLfloat> pts;
+ pts.resize( 9 * sensors.size() );
+
// calculate a triangle for each non-zero sensor range
- for( unsigned int s=0; s<sensor_count; s++ )
+ for( unsigned int s=0; s<sensors.size(); s++ )
{
- if( samples[s] > 0.0 )
- {
- stg_ranger_sensor_t* rngr = &sensors[s];
-
- // sensor FOV
- double sidelen = samples[s];
- double da = rngr->fov/2.0;
-
- unsigned int index = s*9;
- pts[index+0] = rngr->pose.x;
- pts[index+1] = rngr->pose.y;
- pts[index+2] = rngr->pose.z;
-
- pts[index+3] = rngr->pose.x + sidelen*cos(rngr->pose.a - da );
- pts[index+4] = rngr->pose.y + sidelen*sin(rngr->pose.a - da );
- pts[index+5] = rngr->pose.z;
-
- pts[index+6] = rngr->pose.x + sidelen*cos(rngr->pose.a + da );
- pts[index+7] = rngr->pose.y + sidelen*sin(rngr->pose.a + da );
- pts[index+8] = rngr->pose.z;
- }
+ Sensor& rngr = sensors[s];
+
+ if( rngr.range > 0.0 )
+ {
+ // sensor FOV
+ double sidelen = rngr.range;
+ double da = rngr.fov/2.0;
+
+ unsigned int index = s*9;
+ pts[index+0] = rngr.pose.x;
+ pts[index+1] = rngr.pose.y;
+ pts[index+2] = rngr.pose.z;
+
+ pts[index+3] = rngr.pose.x +
sidelen*cos(rngr.pose.a - da );
+ pts[index+4] = rngr.pose.y +
sidelen*sin(rngr.pose.a - da );
+ pts[index+5] = rngr.pose.z;
+
+ pts[index+6] = rngr.pose.x +
sidelen*cos(rngr.pose.a + da );
+ pts[index+7] = rngr.pose.y +
sidelen*sin(rngr.pose.a + da );
+ pts[index+8] = rngr.pose.z;
+ }
}
// draw the filled triangles in transparent pale green
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
glDepthMask( GL_FALSE );
PushColor( 0, 1, 0, 0.1 );
- glVertexPointer( 3, GL_FLOAT, 0, pts );
- glDrawArrays( GL_TRIANGLES, 0, 3 * sensor_count );
+ glVertexPointer( 3, GL_FLOAT, 0, &pts[0] );
+ glDrawArrays( GL_TRIANGLES, 0, 3 * sensors.size() );
// restore state
glDepthMask( GL_TRUE );
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-06-03 02:29:12 UTC (rev 7765)
+++ code/stage/trunk/libstage/stage.hh 2009-06-03 06:26:12 UTC (rev 7766)
@@ -847,27 +847,18 @@
};
typedef RaytraceResult stg_raytrace_result_t;
-
+
class Ray
{
public:
- //SuperRegion& sup;
- //Regiion& reg;
- //Cell& cell;
- //stg_point_int_t glob;
- //stg_point_int_t origin;
- //stg_point_int_t dest;
-
- Model* mod;
- Pose origin;
- stg_meters_t range;
- stg_ray_test_func_t func;
- void* arg;
- bool ztest;
-
- RaytraceResult result;
+ Model* mod;
+ Pose origin;
+ stg_meters_t range;
+ stg_ray_test_func_t func;
+ void* arg;
+ bool ztest;
+ RaytraceResult result;
};
-
const uint32_t INTERVAL_LOG_LEN = 32;
@@ -2402,21 +2393,23 @@
// BLOBFINDER MODEL --------------------------------------------------------
- /** blobfinder data packet
- */
- typedef struct
- {
- stg_color_t color;
- uint32_t left, top, right, bottom;
- stg_meters_t range;
- } stg_blobfinder_blob_t;
/// %ModelBlobfinder class
class ModelBlobfinder : public Model
{
+ public:
+ /** Sample data */
+ class Blob
+ {
+ public:
+ stg_color_t color;
+ uint32_t left, top, right, bottom;
+ stg_meters_t range;
+ };
+
private:
- GArray* colors;
- GArray* blobs;
+ std::vector<Blob> blobs;
+ std::vector<stg_color_t> colors;
// predicate for ray tracing
static bool BlockMatcher( Block* testblock, Model* finder );
@@ -2426,11 +2419,11 @@
virtual void DataVisualize( Camera* cam );
public:
- unsigned int scan_width;
- unsigned int scan_height;
- stg_meters_t range;
stg_radians_t fov;
stg_radians_t pan;
+ stg_meters_t range;
+ unsigned int scan_height;
+ unsigned int scan_width;
static const char* typestr;
@@ -2444,13 +2437,13 @@
virtual void Shutdown();
virtual void Update();
virtual void Load();
+
+ Blob* GetBlobs( unsigned int* count )
+ {
+ if( count ) *count = blobs.size();
+ return &blobs[0];
+ }
- stg_blobfinder_blob_t* GetBlobs( unsigned int* count )
- {
- if( count ) *count = blobs->len;
- return (stg_blobfinder_blob_t*)blobs->data;
- }
-
/** Start finding blobs with this color.*/
void AddColor( stg_color_t col );
@@ -2463,32 +2456,31 @@
};
-
-
// LASER MODEL --------------------------------------------------------
- // TODO - move these into the class definition, like the gripper
- /** laser sample packet
- */
- typedef struct
- {
- stg_meters_t range; ///< range to laser hit in meters
- double reflectance; ///< intensity of the reflection 0.0 to 1.0
- } stg_laser_sample_t;
-
- typedef struct
- {
- uint32_t sample_count;
- uint32_t resolution;
- Bounds range_bounds;
- stg_radians_t fov;
- stg_usec_t interval;
- } stg_laser_cfg_t;
-
-
/// %ModelLaser class
class ModelLaser : public Model
{
+ public:
+ /** Laser range data */
+ class Sample
+ {
+ public:
+ stg_meters_t range; ///< range to laser hit in meters
+ double reflectance; ///< intensity of the reflection
0.0 to 1.0
+ };
+
+ /** Convenience object for setting parameters using
SetConfig/GetConfig */
+ class Config
+ {
+ public:
+ uint32_t sample_count; ///< Number of range samples
+ uint32_t resolution; ///< interpolation
+ Bounds range_bounds; ///< min and max ranges
+ stg_radians_t fov; ///< Field of view, centered about
the pose angle
+ stg_usec_t interval; ///< Time interval between
updates (TODO: is this used?)
+ };
+
private:
class Vis : public Visualizer
@@ -2504,23 +2496,15 @@
virtual ~Vis( void ){}
virtual void Visualize( Model* mod, Camera* cam );
} vis;
-
-
- /** OpenGL displaylist for laser data */
- int data_dl;
- bool data_dirty;
-
- //stg_laser_sample_t* samples;
- uint32_t sample_count;
-
- std::vector<stg_laser_sample_t> samples;
-
- stg_meters_t range_max;
- stg_radians_t fov;
- uint32_t resolution;
-
- std::vector<Ray> rays;
-
+
+ unsigned int sample_count;
+ std::vector<Sample> samples;
+ std::vector<Ray> rays;
+
+ stg_meters_t range_max;
+ stg_radians_t fov;
+ uint32_t resolution;
+
// set up data buffers after the config changes
void SampleConfig();
@@ -2539,20 +2523,17 @@
virtual void Load();
virtual void Print( char* prefix );
- uint32_t GetSampleCount(){ return sample_count; }
-
/** returns an array of samples */
- stg_laser_sample_t* GetSamples( uint32_t* count );
+ Sample* GetSamples( uint32_t* count );
- /** returns a const reference to a vector of samples */
- const std::vector<stg_laser_sample_t>& GetSamples();
-
- // Get the user-tweakable configuration of the laser
- stg_laser_cfg_t GetConfig( );
-
- // Set the user-tweakable configuration of the laser
- void SetConfig( stg_laser_cfg_t cfg );
-
+ /** returns a const reference to a vector of samples */
+ const std::vector<Sample>& GetSamples();
+
+ /** Get the user-tweakable configuration of the laser */
+ Config GetConfig( );
+
+ /** Set the user-tweakable configuration of the laser */
+ void SetConfig( Config& cfg );
};
// \todo GRIPPER MODEL
--------------------------------------------------------
@@ -2739,18 +2720,21 @@
// RANGER MODEL --------------------------------------------------------
- typedef struct
- {
- Pose pose;
- Size size;
- Bounds bounds_range;
- stg_radians_t fov;
- int ray_count;
- } stg_ranger_sensor_t;
-
/// %ModelRanger class
class ModelRanger : public Model
{
+ public:
+ class Sensor
+ {
+ public:
+ Pose pose;
+ Size size;
+ Bounds bounds_range;
+ stg_radians_t fov;
+ int ray_count;
+ stg_meters_t range;
+ };
+
protected:
virtual void Startup();
@@ -2768,11 +2752,9 @@
virtual void Load();
virtual void Print( char* prefix );
-
- uint32_t sensor_count;
- stg_ranger_sensor_t* sensors;
- stg_meters_t* samples;
-
+
+ std::vector<Sensor> sensors;
+
private:
static Option showRangerData;
static Option showRangerTransducers;
@@ -2974,16 +2956,6 @@
Pose est_origin; ///< global origin of the local coordinate system
};
-
- class ModelScooper : public Model
- {
- GList* items;
-
- public:
- ModelScooper( World* world, Model* parent );
- };
-
-
}; // end namespace stg
#endif
Modified: code/stage/trunk/libstageplugin/p_blobfinder.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_blobfinder.cc 2009-06-03 02:29:12 UTC
(rev 7765)
+++ code/stage/trunk/libstageplugin/p_blobfinder.cc 2009-06-03 06:26:12 UTC
(rev 7766)
@@ -56,7 +56,7 @@
ModelBlobfinder* blobmod = (ModelBlobfinder*)this->mod;
uint32_t bcount = 0;
- stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &bcount );
+ ModelBlobfinder::Blob* blobs = blobmod->GetBlobs( &bcount );
if ( bcount > 0 )
{
Modified: code/stage/trunk/libstageplugin/p_laser.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_laser.cc 2009-06-03 02:29:12 UTC (rev
7765)
+++ code/stage/trunk/libstageplugin/p_laser.cc 2009-06-03 06:26:12 UTC (rev
7766)
@@ -53,8 +53,8 @@
void InterfaceLaser::Publish( void )
{
ModelLaser* mod = (ModelLaser*)this->mod;
- stg_laser_sample_t* samples = mod->GetSamples(NULL);
-
+ ModelLaser::Sample* samples = mod->GetSamples(NULL);
+
// don't publish anything until we have some real data
if( samples == NULL )
return;
@@ -62,7 +62,7 @@
player_laser_data_t pdata;
memset( &pdata, 0, sizeof(pdata) );
- stg_laser_cfg_t cfg = mod->GetConfig();
+ ModelLaser::Config cfg = mod->GetConfig();
pdata.min_angle = -cfg.fov/2.0;
pdata.max_angle = +cfg.fov/2.0;
pdata.resolution = cfg.fov / cfg.sample_count;
@@ -109,7 +109,7 @@
// TODO
// int intensity = plc->intensity;
- stg_laser_cfg_t cfg = mod->GetConfig();
+ ModelLaser::Config cfg = mod->GetConfig();
PRINT_DEBUG3( "laser config was: resolution %d, fov %.6f, interval
%d\n",
cfg.resolution, cfg.fov, cfg.interval );
@@ -149,7 +149,7 @@
{
if( hdr->size == 0 )
{
- stg_laser_cfg_t cfg = mod->GetConfig();
+ ModelLaser::Config cfg = mod->GetConfig();
player_laser_config_t plc;
memset(&plc,0,sizeof(plc));
Modified: code/stage/trunk/libstageplugin/p_sonar.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_sonar.cc 2009-06-03 02:29:12 UTC (rev
7765)
+++ code/stage/trunk/libstageplugin/p_sonar.cc 2009-06-03 06:26:12 UTC (rev
7766)
@@ -56,36 +56,29 @@
void InterfaceSonar::Publish( void )
{
ModelRanger* mod = (ModelRanger*)this->mod;
-
- if( mod->samples == NULL )
- return;
-
+
player_sonar_data_t sonar;
memset( &sonar, 0, sizeof(sonar) );
- size_t sensor_count = mod->sensor_count;
+ size_t count = mod->sensors.size();
- if( sensor_count > 0 )
+ if( count > 0 )
{
- // limit the number of samples to Player's maximum
- //if( sensor_count > PLAYER_SONAR_MAX_SAMPLES )
- //sensor_count = PLAYER_SONAR_MAX_SAMPLES;
-
//if( son->power_on ) // set with a sonar config
{
- sonar.ranges_count = sensor_count;
- sonar.ranges = new float[sensor_count];
-
- for( unsigned int i=0; i<sensor_count; i++ )
- sonar.ranges[i] = mod->samples[i];
+ sonar.ranges_count = count;
+ sonar.ranges = new float[count];
+
+ for( unsigned int i=0; i<count; i++ )
+ sonar.ranges[i] = mod->sensors[i].range;
}
}
this->driver->Publish( this->addr,
- PLAYER_MSGTYPE_DATA,
- PLAYER_SONAR_DATA_RANGES,
- &sonar, sizeof(sonar), NULL);
-
+
PLAYER_MSGTYPE_DATA,
+
PLAYER_SONAR_DATA_RANGES,
+
&sonar, sizeof(sonar), NULL);
+
if( sonar.ranges )
delete[] sonar.ranges;
}
@@ -102,35 +95,31 @@
{
ModelRanger* mod = (ModelRanger*)this->mod;
- size_t rcount = mod->sensor_count;
+ size_t count = mod->sensors.size();
- // limit the number of samples to Player's maximum
- //if( rcount > PLAYER_SONAR_MAX_SAMPLES )
- //rcount = PLAYER_SONAR_MAX_SAMPLES;
-
// convert the ranger data into Player-format sonar poses
player_sonar_geom_t pgeom;
memset( &pgeom, 0, sizeof(pgeom) );
- pgeom.poses_count = rcount;
- pgeom.poses = new player_pose3d_t[rcount];
+ pgeom.poses_count = count;
+ pgeom.poses = new player_pose3d_t[count];
- for( unsigned int i=0; i<rcount; i++ )
- {
- // fill in the geometry data formatted player-like
- pgeom.poses[i].px = mod->sensors[i].pose.x;
- pgeom.poses[i].py = mod->sensors[i].pose.y;
- pgeom.poses[i].pz = 0;
- pgeom.poses[i].ppitch = 0;
- pgeom.poses[i].proll = 0;
- pgeom.poses[i].pyaw = mod->sensors[i].pose.a;
- }
+ for( unsigned int i=0; i<count; i++ )
+ {
+ // fill in the geometry data formatted
player-like
+ pgeom.poses[i].px =
mod->sensors[i].pose.x;
+ pgeom.poses[i].py =
mod->sensors[i].pose.y;
+ pgeom.poses[i].pz = 0;
+ pgeom.poses[i].ppitch = 0;
+ pgeom.poses[i].proll = 0;
+ pgeom.poses[i].pyaw =
mod->sensors[i].pose.a;
+ }
this->driver->Publish( this->addr, resp_queue,
- PLAYER_MSGTYPE_RESP_ACK,
- PLAYER_SONAR_REQ_GET_GEOM,
- (void*)&pgeom, sizeof(pgeom), NULL );
-
+
PLAYER_MSGTYPE_RESP_ACK,
+
PLAYER_SONAR_REQ_GET_GEOM,
+
(void*)&pgeom, sizeof(pgeom), NULL );
+
delete[] pgeom.poses;
return 0; // ok
}
@@ -138,7 +127,7 @@
{
// Don't know how to handle this message.
PRINT_WARN2( "stg_sonar doesn't support msg with type/subtype %d/%d",
- hdr->type, hdr->subtype);
+
hdr->type, hdr->subtype);
return(-1);
}
}
Modified: code/stage/trunk/webstage/webstage.cc
===================================================================
--- code/stage/trunk/webstage/webstage.cc 2009-06-03 02:29:12 UTC (rev
7765)
+++ code/stage/trunk/webstage/webstage.cc 2009-06-03 06:26:12 UTC (rev
7766)
@@ -184,17 +184,17 @@
Model* mod = world->GetModel( name.c_str() );
if( mod )
{
- ModelLaser* laser =
(ModelLaser*)mod->GetModel("laser:0");
-
- if(laser){
- uint32_t sample_count=0;
- stg_laser_sample_t* scan = laser->GetSamples(
&sample_count );
- assert(scan);
-
- stg_laser_cfg_t cfg = laser->GetConfig();
- resolution = cfg.resolution;
+ ModelLaser* laser =
(ModelLaser*)mod->GetModel("laser:0");
+
+ if(laser){
+ uint32_t sample_count=0;
+ ModelLaser::Sample* scan = laser->GetSamples(
&sample_count );
+ assert(scan);
+
+ ModelLaser::Config cfg = laser->GetConfig();
+ resolution = cfg.resolution;
fov = cfg.fov;
-
+
for(unsigned int i=0;i<sample_count;i++)
ranges.push_back(scan[i].range);
}else{
@@ -225,38 +225,33 @@
Model* mod = world->GetModel( name.c_str() );
if( mod )
- {
- ModelRanger* ranger =
(ModelRanger*)mod->GetModel("ranger:0");
-
- if(ranger){
- uint32_t count = ranger->sensor_count;
- if(ranger->samples)
- for(unsigned int
i=0;i<count;i++)
-
ranges.push_back(ranger->samples[i]);
-
//std::copy(ranger->samples,ranger->samples+ranger->sensor_count,ranges.begin());
-
- if(ranger->sensors)
- for(unsigned int
i=0;i<count;i++){
- websim::Pose pos;
- Pose rpos;
- rpos =
ranger->sensors[i].pose;
- pos.x = rpos.x;
- pos.y = rpos.y;
- pos.z = rpos.z;
- pos.a = rpos.a;
- p.push_back(pos);
- }
-
- }else{
-
- printf( "Warning: attempt to get ranger data
for unrecognized ranger model of model \"%s\"\n",
- name.c_str() );
- return false;
-
-
- }
-
- }
+ {
+ ModelRanger* ranger =
(ModelRanger*)mod->GetModel("ranger:0");
+
+ if(ranger){
+ uint32_t count = ranger->sensors.size();
+ for(unsigned int i=0;i<count;i++)
+
ranges.push_back(ranger->sensors[i].range);
+
//std::copy(ranger->samples,ranger->samples+ranger->sensor_count,ranges.begin());
+
+ for(unsigned int i=0;i<count;i++){
+ websim::Pose pos;
+ Pose rpos;
+ rpos = ranger->sensors[i].pose;
+ pos.x = rpos.x;
+ pos.y = rpos.y;
+ pos.z = rpos.z;
+ pos.a = rpos.a;
+ p.push_back(pos);
+ }
+
+ }else{
+
+ printf( "Warning: attempt to get ranger data
for unrecognized ranger model of model \"%s\"\n",
+ name.c_str() );
+ return false;
+ }
+ }
else{
printf( "Warning: attempt to get ranger data for unrecognized
model \"%s\"\n",
name.c_str() );
@@ -335,7 +330,7 @@
unsigned int n=0;
this->GetNumberOfRobots(n);
- for(int i=0;i<n;i++){
+ for(unsigned int i=0;i<n;i++){
char temp[128];
sprintf(temp,"position:%d",i);
Model *mod = world->GetModel(temp);
Modified: code/stage/trunk/worlds/benchmark/expand.cc
===================================================================
--- code/stage/trunk/worlds/benchmark/expand.cc 2009-06-03 02:29:12 UTC (rev
7765)
+++ code/stage/trunk/worlds/benchmark/expand.cc 2009-06-03 06:26:12 UTC (rev
7766)
@@ -27,7 +27,7 @@
#define SAFE_ANGLE 1 // radians
// forward declare
-int RangerUpdate( Model* mod, robot_t* robot );
+int RangerUpdate( ModelRanger* mod, robot_t* robot );
// Stage calls this when the model starts up
extern "C" int Init( Model* mod )
@@ -51,23 +51,19 @@
return 0; //ok
}
-int RangerUpdate( Model* mod, robot_t* robot )
-{
- ModelRanger* rgr = robot->ranger;
-
- if( rgr->samples == NULL )
- return 0;
-
+int RangerUpdate( ModelRanger* rgr, robot_t* robot )
+{
// compute the vector sum of the sonar ranges
double dx=0, dy=0;
-
- for( unsigned int s=0; s< rgr->sensor_count; s++ )
- {
- double srange = rgr->samples[s];
-
- dx += srange * cos( rgr->sensors[s].pose.a );
- dy += srange * sin( rgr->sensors[s].pose.a );
-
+
+ for( std::vector<ModelRanger::Sensor>::iterator it =
rgr->sensors.begin();
+ it != rgr->sensors.end();
+ ++it )
+ {
+ ModelRanger::Sensor& s = *it;
+ dx += s.range * cos( s.pose.a );
+ dy += s.range * sin( s.pose.a );
+
//printf( "sensor %d angle= %.2f\n", s, rgr->sensors[s].pose.a
);
}
@@ -85,11 +81,11 @@
//printf( "resultant %.2f turn_speed %.2f\n", resultant_angle, turn_speed );
// if the front is clear, drive forwards
- if( (rgr->samples[0] > SAFE_DIST) &&
- (rgr->samples[1] > SAFE_DIST/2.0) &&
- (rgr->samples[2] > SAFE_DIST/5.0) &&
- (rgr->samples[15] > SAFE_DIST/2.0) &&
- (rgr->samples[14] > SAFE_DIST/5.0) &&
+ if( (rgr->sensors[0].range > SAFE_DIST) &&
+ (rgr->sensors[1].range > SAFE_DIST/2.0) &&
+ (rgr->sensors[2].range > SAFE_DIST/5.0) &&
+ (rgr->sensors[15].range > SAFE_DIST/2.0) &&
+ (rgr->sensors[14].range > SAFE_DIST/5.0) &&
(fabs( resultant_angle ) < SAFE_ANGLE) )
{
forward_speed = VSPEED;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
OpenSolaris 2009.06 is a cutting edge operating system for enterprises
looking to deploy the next generation of Solaris that includes the latest
innovations from Sun and the OpenSource community. Download a copy and
enjoy capabilities such as Networking, Storage and Virtualization.
Go to: http://p.sf.net/sfu/opensolaris-get
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
