Revision: 7873
http://playerstage.svn.sourceforge.net/playerstage/?rev=7873&view=rev
Author: rtv
Date: 2009-06-23 00:16:57 +0000 (Tue, 23 Jun 2009)
Log Message:
-----------
working on blobfinder vis
Modified Paths:
--------------
code/stage/trunk/libstage/model_blobfinder.cc
code/stage/trunk/libstage/model_fiducial.cc
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/model_ranger.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/worldfile.cc
code/stage/trunk/todo.txt
code/stage/trunk/worlds/everything.world
code/stage/trunk/worlds/fasr.world
code/stage/trunk/worlds/simple.world
Modified: code/stage/trunk/libstage/model_blobfinder.cc
===================================================================
--- code/stage/trunk/libstage/model_blobfinder.cc 2009-06-22 23:28:53 UTC
(rev 7872)
+++ code/stage/trunk/libstage/model_blobfinder.cc 2009-06-23 00:16:57 UTC
(rev 7873)
@@ -29,8 +29,6 @@
static const unsigned int DEFAULT_BLOBFINDERSCANWIDTH = 80;
static const unsigned int DEFAULT_BLOBFINDERSCANHEIGHT = 60;
-//TODO make instance attempt to register an option (as customvisualizations do)
-Option ModelBlobfinder::showBlobData( "Show Blobfinder", "show_blob", "",
true, NULL );
/**
@ingroup model
@@ -81,21 +79,23 @@
*/
- ModelBlobfinder::ModelBlobfinder( World* world,
-
Model* parent )
+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 )
+ vis( world ),
+ 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 );
- ClearBlocks();
- RegisterOption( &showBlobData );
+ PRINT_DEBUG2( "Constructing ModelBlobfinder %d (%s)\n",
+ id, typestr );
+ ClearBlocks();
+
+ AddVisualizer( &this->vis, true );
}
@@ -189,30 +189,27 @@
// scan through the samples looking for color blobs
for(unsigned int s=0; s < scan_width; s++ )
- {
- if( samples[s].mod == NULL )
+ {
+ if( samples[s].mod == NULL )
continue; // we saw nothing
+
+ unsigned int right = s;
+ stg_color_t blobcol = samples[s].color;
+
+ //printf( "blob start %d color %X\n", blobleft, blobcol );
+
+ // loop until we hit the end of the blob
+ // there has to be a gap of >1 pixel to end a blob
+ // this avoids getting lots of crappy little blobs
+ while( s < scan_width && samples[s].mod &&
+ ColorMatchIgnoreAlpha( samples[s].color,
blobcol) )
+ {
+ //printf( "%u blobcol %X block %p %s color %X\n", s,
blobcol, samples[s].block, samples[s].block->Model()->Token(),
samples[s].block->Color() );
+ s++;
+ }
+
+ unsigned int left = s - 1;
- //if( samples[s].block->Color() != 0xFFFF0000 )
- //continue; // we saw nothing
-
- unsigned int right = s;
- stg_color_t blobcol = samples[s].color;
-
- //printf( "blob start %d color %X\n", blobleft, blobcol );
-
- // loop until we hit the end of the blob
- // there has to be a gap of >1 pixel to end a blob
- // this avoids getting lots of crappy little blobs
- while( s < scan_width && samples[s].mod &&
- ColorMatchIgnoreAlpha( samples[s].color,
blobcol) )
- {
- //printf( "%u blobcol %X block %p %s color %X\n", s,
blobcol, samples[s].block, samples[s].block->Model()->Token(),
samples[s].block->Color() );
- s++;
- }
-
- unsigned int left = s - 1;
-
//if we have color filters in place, check to see if we're
looking for this color
if( colors.size() )
{
@@ -291,82 +288,96 @@
Model::Shutdown();
}
-void ModelBlobfinder::DataVisualize( Camera* cam )
-{
- if ( !showBlobData )
- return;
-
- if( debug )
- {
- // draw the FOV
- GLUquadric* quadric = gluNewQuadric();
+//******************************************************************************
+// visualization
- PushColor( 0,0,0,0.2 );
+//TODO make instance attempt to register an option (as customvisualizations do)
+// Option ModelBlobfinder::showBlobData( "Show Blobfinder", "show_blob", "",
true, NULL );
- gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
- gluPartialDisk( quadric,
- 0,
- range,
- 20, // slices
- 1, // loops
- rtod( M_PI/2.0 + fov/2.0 - pan), // start angle
- rtod(-fov) ); // sweep angle
+ModelBlobfinder::Vis::Vis( World* world )
+ : Visualizer( "Blobfinder", "blobfinder_vis" )
+{
+ //world->RegisterOption( &showArea );
+ //world->RegisterOption( &showStrikes );
+ //world->RegisterOption( &showFov );
+ //world->RegisterOption( &showBeams );
+}
- gluDeleteQuadric( quadric );
- PopColor();
+void ModelBlobfinder::Vis::Visualize( Model* mod, Camera* cam )
+{
+ ModelBlobfinder* bf( dynamic_cast<ModelBlobfinder*>(mod) );
+
+ if( bf->debug )
+ {
+ // draw the FOV
+ GLUquadric* quadric = gluNewQuadric();
+
+ bf->PushColor( 0,0,0,0.2 );
+
+ gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
+ gluPartialDisk( quadric,
+ 0,
+ bf->range,
+ 20, // slices
+ 1, // loops
+ rtod( M_PI/2.0 +
bf->fov/2.0 - bf->pan), // start angle
+ rtod(-bf->fov) ); //
sweep angle
+
+ gluDeleteQuadric( quadric );
+ bf->PopColor();
}
+
+ if( bf->subs < 1 )
+ return;
+
+ glPushMatrix();
- if( subs < 1 )
- return;
-
- glPushMatrix();
-
// return to global rotation frame
- Pose gpose = GetGlobalPose();
- glRotatef( rtod(-gpose.a),0,0,1 );
-
- // place the "screen" a little away from the robot
- glTranslatef( -2.5, -1.5, 0.5 );
-
- // rotate to face screen
- float yaw, pitch;
- pitch = - cam->pitch();
- yaw = - cam->yaw();
- float robotAngle = -rtod(pose.a);
- glRotatef( robotAngle - yaw, 0,0,1 );
- glRotatef( -pitch, 1,0,0 );
-
- // convert blob pixels to meters scale - arbitrary
- glScalef( 0.025, 0.025, 1 );
-
- // draw a white screen with a black border
- PushColor( 0xFFFFFFFF );
- glRectf( 0,0, scan_width, scan_height );
- PopColor();
-
- glTranslatef(0,0,0.01 );
-
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
- PushColor( 0xFF000000 );
- glRectf( 0,0, scan_width, scan_height );
- PopColor();
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
-
- // draw the blobs on the screen
- for( unsigned int s=0; s<blobs.size(); s++ )
- {
- Blob* b = &blobs[s];
+ Pose gpose( bf->GetGlobalPose() );
+ glRotatef( rtod(-gpose.a),0,0,1 );
+
+ // place the "screen" a little away from the robot
+ glTranslatef( -2.5, -1.5, 0.5 );
+
+ // rotate to face screen
+ float yaw, pitch;
+ pitch = - cam->pitch();
+ yaw = - cam->yaw();
+ float robotAngle = -rtod(bf->pose.a);
+ glRotatef( robotAngle - yaw, 0,0,1 );
+ glRotatef( -pitch, 1,0,0 );
+
+ // convert blob pixels to meters scale - arbitrary
+ glScalef( 0.025, 0.025, 1 );
+
+ // draw a white screen with a black border
+ bf->PushColor( 0xFFFFFFFF );
+ glRectf( 0,0, bf->scan_width, bf->scan_height );
+ bf->PopColor();
+
+ glTranslatef(0,0,0.01 );
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+ bf->PushColor( 0xFF000000 );
+ glRectf( 0,0, bf->scan_width, bf->scan_height );
+ bf->PopColor();
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+
+ // draw the blobs on the screen
+ for( unsigned int s=0; s<bf->blobs.size(); s++ )
+ {
+ Blob* b = &bf->blobs[s];
//stg_blobfinder_blob_t* b =
//&g_array_index( blobs, stg_blobfinder_blob_t, s);
-
- PushColor( b->color );
+
+ bf->PushColor( b->color );
glRectf( b->left, b->top, b->right, b->bottom );
//printf( "%u l %u t%u r %u b %u\n", s, b->left, b->top,
b->right, b->bottom );
- PopColor();
- }
-
- glPopMatrix();
+ bf->PopColor();
+ }
+
+ glPopMatrix();
}
Modified: code/stage/trunk/libstage/model_fiducial.cc
===================================================================
--- code/stage/trunk/libstage/model_fiducial.cc 2009-06-22 23:28:53 UTC (rev
7872)
+++ code/stage/trunk/libstage/model_fiducial.cc 2009-06-23 00:16:57 UTC (rev
7873)
@@ -18,11 +18,11 @@
#include "worldfile.hh"
using namespace Stg;
-const stg_meters_t DEFAULT_FIDUCIAL_RANGEMIN = 0.0;
-const stg_meters_t DEFAULT_FIDUCIAL_RANGEMAXID = 5.0;
-const stg_meters_t DEFAULT_FIDUCIAL_RANGEMAXANON = 8.0;
-const stg_radians_t DEFAULT_FIDUCIAL_FOV = M_PI;
-const stg_watts_t DEFAULT_FIDUCIAL_WATTS = 10.0;
+static const stg_meters_t DEFAULT_RANGEMIN = 0.0;
+static const stg_meters_t DEFAULT_RANGEMAXID = 5.0;
+static const stg_meters_t DEFAULT_RANGEMAXANON = 8.0;
+static const stg_radians_t DEFAULT_FOV = M_PI;
+static const stg_watts_t DEFAULT_WATTS = 10.0;
//TODO make instance attempt to register an option (as customvisualizations do)
Option ModelFiducial::showFiducialData( "Fiducials", "show_fiducial", "",
true, NULL );
@@ -70,10 +70,10 @@
Model* parent )
: Model( world, parent, MODEL_TYPE_FIDUCIAL ),
fiducials(),
- max_range_anon( DEFAULT_FIDUCIAL_RANGEMAXANON ),
- max_range_id( DEFAULT_FIDUCIAL_RANGEMAXID ),
- min_range( DEFAULT_FIDUCIAL_RANGEMIN ),
- fov( DEFAULT_FIDUCIAL_FOV ),
+ max_range_anon( DEFAULT_RANGEMAXANON ),
+ max_range_id( DEFAULT_RANGEMAXID ),
+ min_range( DEFAULT_RANGEMIN ),
+ fov( DEFAULT_FOV ),
heading( 0 ),
key( 0 )
{
@@ -162,14 +162,13 @@
//printf( "bearing %.2f\n", RTOD(bearing) );
- stg_raytrace_result_t ray = Raytrace( dtheta,
-
max_range_anon,
-
fiducial_raytrace_match,
-
NULL,
-
true );
+ stg_raytrace_result_t ray( Raytrace( dtheta,
+
max_range_anon,
+
fiducial_raytrace_match,
+
NULL,
+
true ) );
- //range = ray.range;
- Model* hitmod = ray.mod;
+ Model* hitmod( ray.mod );
//printf( "ray hit %s and was seeking LOS to %s\n",
// hitmod ? hitmod->Token() : "null",
@@ -178,7 +177,7 @@
// if it was him, we can see him
if( hitmod == him )
{
- Geom hisgeom = him->GetGeom();
+ Geom hisgeom( him->GetGeom() );
// record where we saw him and what he looked like
Fiducial fid;
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2009-06-22 23:28:53 UTC (rev
7872)
+++ code/stage/trunk/libstage/model_laser.cc 2009-06-23 00:16:57 UTC (rev
7873)
@@ -86,11 +86,11 @@
void ModelLaser::Vis::Visualize( Model* mod, Camera* cam )
{
- ModelLaser* laser = dynamic_cast<ModelLaser*>(mod);
+ ModelLaser* laser( dynamic_cast<ModelLaser*>(mod) );
const std::vector<Sample>& samples( laser->GetSamples() );
- size_t sample_count = samples.size();
+ size_t sample_count( samples.size() );
glPushMatrix();
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
@@ -108,7 +108,7 @@
glDepthMask( GL_FALSE );
glPointSize( 2 );
- for( unsigned int s=0; s<sample_count; s++ )
+ for( unsigned int s(0); s<sample_count; s++ )
{
double ray_angle = (s * (laser->fov / (sample_count-1))) -
laser->fov/2.0;
pts[2*s+2] = (float)(samples[s].range * cos(ray_angle) );
@@ -148,11 +148,11 @@
if( showFov )
{
- for( unsigned int s=0; s<sample_count; s++ )
+ for( unsigned int s(0); s<sample_count; s++ )
{
- double ray_angle = (s * (laser->fov /
(sample_count-1))) - laser->fov/2.0;
+ double ray_angle((s * (laser->fov / (sample_count-1)))
- laser->fov/2.0);
pts[2*s+2] = (float)(laser->range_max * cos(ray_angle)
);
- pts[2*s+3] = (float)(laser->range_max * sin(ray_angle) );
+ pts[2*s+3] = (float)(laser->range_max * sin(ray_angle)
);
}
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
@@ -167,11 +167,11 @@
laser->PushColor( 0, 0, 1, 0.5 );
glBegin( GL_LINES );
- for( unsigned int s=0; s<sample_count; s++ )
+ for( unsigned int s(0); s<sample_count; s++ )
{
glVertex2f( 0,0 );
- double ray_angle = (s * (laser->fov /
(sample_count-1))) - laser->fov/2.0;
+ double ray_angle((s * (laser->fov / (sample_count-1)))
- laser->fov/2.0);
glVertex2f( samples[s].range * cos(ray_angle),
samples[s].range *
sin(ray_angle) );
@@ -297,46 +297,46 @@
rayorg.z += geom.size.z/2.0;
rayorg.a = bearing;
rayorg = LocalToGlobal(rayorg);
-
- // set up a ray to trace
- Ray ray( this, rayorg, range_max, laser_raytrace_match, NULL, true );
-
- // trace the ray, incrementing its heading for each sample
- for( unsigned int t=0; t<sample_count; t += resolution )
+
+ // set up a ray to trace
+ Ray ray( this, rayorg, range_max, laser_raytrace_match, NULL, true );
+
+ // trace the ray, incrementing its heading for each sample
+ for( unsigned int t(0); t<sample_count; t += resolution )
{
- stg_raytrace_result_t r( world->Raytrace( ray ) );
- samples[t].range = r.range;
-
+ stg_raytrace_result_t r( world->Raytrace( ray ) );
+ samples[t].range = r.range;
+
// if we hit a model and it reflects brightly, we set
// reflectance high, else low
if( r.mod && ( r.mod->vis.laser_return >= LaserBright ) )
- samples[t].reflectance = 1;
+ samples[t].reflectance = 1;
else
- samples[t].reflectance = 0;
-
- // point the ray to the next angle
- ray.origin.a += sample_incr;
+ samples[t].reflectance = 0;
+
+ // point the ray to the next angle
+ ray.origin.a += sample_incr;
}
-
+
// we may need to interpolate the samples we skipped
if( resolution > 1 )
{
for( unsigned int t( resolution); t<sample_count; t+=resolution )
- for( unsigned int g(1); g<resolution; g++ )
- {
- if( t >= sample_count )
- break;
-
- // copy the rightmost sample
data into this point
- samples[t-g] =
samples[t-resolution];
-
- double left = samples[t].range;
- double right =
samples[t-resolution].range;
-
- // linear range interpolation
between the left and right samples
- samples[t-g].range =
(left-g*(left-right)/resolution);
- }
- }
+ for( unsigned int g(1); g<resolution; g++ )
+ {
+ if( t >= sample_count )
+ break;
+
+ // copy the rightmost sample data into this
point
+ samples[t-g] = samples[t-resolution];
+
+ double left( samples[t].range );
+ double right( samples[t-resolution].range );
+
+ // linear range interpolation between the left
and right samples
+ samples[t-g].range =
(left-g*(left-right)/resolution);
+ }
+ }
MapFromRoot();
Modified: code/stage/trunk/libstage/model_ranger.cc
===================================================================
--- code/stage/trunk/libstage/model_ranger.cc 2009-06-22 23:28:53 UTC (rev
7872)
+++ code/stage/trunk/libstage/model_ranger.cc 2009-06-23 00:16:57 UTC (rev
7873)
@@ -98,40 +98,40 @@
static const char RANGER_CONFIG_COLOR[] = "gray90";
static const char RANGER_GEOM_COLOR[] = "orange";
+// static members
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* parent
)
: Model( world, parent, MODEL_TYPE_RANGER )
{
PRINT_DEBUG2( "Constructing ModelRanger %d (%s)\n",
- id, typestr );
-
+ id, typestr );
+
// Set up sensible defaults
-
+
// assert that Update() is reentrant for this derived model
thread_safe = true;
-
- stg_color_t col = stg_lookup_color( RANGER_CONFIG_COLOR );
- this->SetColor( col );
-
+
+ this->SetColor( stg_lookup_color( RANGER_CONFIG_COLOR ) );
+
// remove the polygon: ranger has no body
this->ClearBlocks();
-
+
Geom geom;
geom.size.x = RANGER_SIZEX;
geom.size.y = RANGER_SIZEY;
geom.size.z = RANGER_SIZEZ;
this->SetGeom( geom );
-
+
// spread the transducers around the ranger's body
double offset = MIN(geom.size.x, geom.size.y) / 2.0;
-
- sensors.resize( RANGER_SENSORCOUNT );
-
+
+ sensors.resize( RANGER_SENSORCOUNT );
+
// create default ranger config
- for( unsigned int c=0; c<sensors.size(); c++ )
+ for( unsigned int c(0); c<sensors.size(); c++ )
{
sensors[c].pose.a = (2.0*M_PI)/sensors.size() * c;
sensors[c].pose.x = offset * cos( sensors[c].pose.a );
@@ -147,7 +147,7 @@
sensors[c].fov = (2.0*M_PI)/(sensors.size()+1);
sensors[c].ray_count = RANGER_RAYCOUNT;
- sensors[c].range = 0; // invalid range
+ sensors[c].range = 0; // invalid range
}
RegisterOption( &showRangerData );
@@ -191,7 +191,7 @@
char key[256];
- sensors.resize( sensor_count );
+ sensors.resize( sensor_count );
Size common_size;
common_size.x = wf->ReadTupleLength( wf_entity, "ssize", 0, RANGER_SIZEX
);
@@ -205,60 +205,58 @@
// set all transducers with the common settings
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;
- }
+ {
+ 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 < 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), "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), "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), "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 */
+ /* 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 )); */
+ /* 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; */
+ /* sensors[i].ray_count = common_ray_count; */
- }
+ }
PRINT_DEBUG1( "loaded %d ranger configs", (int)sensor_count );
}
}
static bool ranger_match( Model* candidate,
- Model* finder, const void* dummy )
+ Model*
finder,
+ const void*
dummy )
{
- //printf( "ranger match sees %s %p %d \n",
- // block->Model()->Token(), block->Model(), block->Model()->LaserReturn()
);
-
// Ignore myself, my children, and my ancestors.
return( candidate->vis.ranger_return && !candidate->IsRelated( finder ) );
}
@@ -267,46 +265,31 @@
{
Model::Update();
- if( subs < 1 )
- return;
+ if( subs < 1 )
+ return;
- if( sensors.size() < 1 )
- return;
+ if( sensors.size() < 1 )
+ return;
- //PRINT_DEBUG2( "[%d] updating ranger %s", (int)world->sim_time_ms, token );
-
// raytrace new range data for all sensors
for( std::vector<Sensor>::iterator it( sensors.begin() );
- it != sensors.end();
- ++it )
+ it != sensors.end();
+ ++it )
{
- Sensor& s = *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( s.pose,
-
s.bounds_range.max,
-
ranger_match,
-
NULL );
+
s.bounds_range.max,
+
ranger_match,
+
NULL );
s.range = MAX( ray.range, s.bounds_range.min );
}
}
-// TODO: configurable ranger noise model
-/*
- 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 ModelRanger::Print( char* prefix )
{
Model::Print( prefix );
@@ -329,18 +312,18 @@
PushColor( 0,0,0,1 );
for( unsigned int s=0; s<sensors.size(); s++ )
- {
- Sensor& rngr = sensors[s];
+ {
+ Sensor& rngr = sensors[s];
- if( rngr.range > 0.0 )
- {
+ if( rngr.range > 0.0 )
+ {
- glPointSize( 4 );
- glBegin( GL_POINTS );
- glVertex3f(
rngr.pose.x, rngr.pose.y, rngr.pose.z );
- glEnd();
- }
+ glPointSize( 4 );
+ glBegin( GL_POINTS );
+ glVertex3f( rngr.pose.x, rngr.pose.y,
rngr.pose.z );
+ glEnd();
}
+ }
PopColor();
}
@@ -349,33 +332,33 @@
// 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() );
+ 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<sensors.size(); s++ )
{
- Sensor& rngr = sensors[s];
+ Sensor& rngr = sensors[s];
if( rngr.range > 0.0 )
- {
- // sensor FOV
- double sidelen = rngr.range;
- double da = rngr.fov/2.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;
+ 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+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;
- }
+ 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
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-06-22 23:28:53 UTC (rev 7872)
+++ code/stage/trunk/libstage/stage.hh 2009-06-23 00:16:57 UTC (rev 7873)
@@ -1194,8 +1194,8 @@
void AddToCellArray( std::vector<Cell*>* blocks );
void RemoveFromCellArray( std::vector<Cell*>* blocks );
void GenerateCandidateCells();
- GList* AppendTouchingModels( GList* list );
-
+ GList* AppendTouchingModels( GList* list );
+
/** Returns the first model that shares a bitmap cell with this
model */
Model* TestCollision();
void SwitchToTestedCells();
@@ -1275,7 +1275,6 @@
BlockGroup();
~BlockGroup();
- //std::vector<Block*>&GList* GetBlocks(){ return blocks; };
uint32_t GetCount(){ return blocks.size(); };
Size GetSize(){ return size; };
stg_point3_t GetOffset(){ return offset; };
@@ -2413,6 +2412,18 @@
stg_meters_t range;
};
+ class Vis : public Visualizer
+ {
+ private:
+ //static Option showArea;
+
+
+ public:
+ Vis( World* world );
+ virtual ~Vis( void ){}
+ virtual void Visualize( Model* mod, Camera* cam );
+ } vis;
+
private:
std::vector<Blob> blobs;
std::vector<stg_color_t> colors;
@@ -2422,7 +2433,7 @@
static Option showBlobData;
- virtual void DataVisualize( Camera* cam );
+ // virtual void DataVisualize( Camera* cam );
public:
stg_radians_t fov;
Modified: code/stage/trunk/libstage/worldfile.cc
===================================================================
--- code/stage/trunk/libstage/worldfile.cc 2009-06-22 23:28:53 UTC (rev
7872)
+++ code/stage/trunk/libstage/worldfile.cc 2009-06-23 00:16:57 UTC (rev
7873)
@@ -1768,6 +1768,8 @@
double Worldfile::ReadTupleAngle(int entity, const char *name,
int index, double value)
{
+ //puts( name );
+
CProperty* property = GetProperty(entity, name);
if (property == NULL)
return value;
Modified: code/stage/trunk/todo.txt
===================================================================
--- code/stage/trunk/todo.txt 2009-06-22 23:28:53 UTC (rev 7872)
+++ code/stage/trunk/todo.txt 2009-06-23 00:16:57 UTC (rev 7873)
@@ -1,4 +1,20 @@
+
+** Optimization **
+
+speed records:
+
+simple.world 3600 seconds in 17.35 seconds
+fasr.world 3600 seconds in 7.83 seconds (460 x real time)
+
+** 3.1.0 RELEASE *
+
+ - feature freeze around July 1
+ - scan SF for patches
+ - fix world files
+ - docs for new models
+ - push docs to SF
+
** 3.0.0 RELEASE **
BUGS
Modified: code/stage/trunk/worlds/everything.world
===================================================================
--- code/stage/trunk/worlds/everything.world 2009-06-22 23:28:53 UTC (rev
7872)
+++ code/stage/trunk/worlds/everything.world 2009-06-23 00:16:57 UTC (rev
7873)
@@ -57,21 +57,21 @@
(
ranger( alwayson 0 )
- sicklaser( pose [0.030 0 0 0 ] )
+ sicklaser( pose [0.030 0 0 0 ] alwayson 0 )
fiducial( range_max 8 range_max_id 5 )
blobfinder(
channel_count 6
channels [ "red" "blue" "green" "cyan" "yellow" "magenta" ]
- alwayson 0
+ alwayson 1
)
fiducial_return 17
gripper_return 0
localization "gps"
- localization_origin [ 0 0 0 ]
+ localization_origin [ 0 0 0 0 ]
)
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2009-06-22 23:28:53 UTC (rev 7872)
+++ code/stage/trunk/worlds/fasr.world 2009-06-23 00:16:57 UTC (rev 7873)
@@ -11,7 +11,7 @@
paused 1
# time to pause (in GUI mode) or quit (in headless mode) the simulation
-# quit_time 120
+quit_time 3600 # 1 hour of simulated time
resolution 0.02
Modified: code/stage/trunk/worlds/simple.world
===================================================================
--- code/stage/trunk/worlds/simple.world 2009-06-22 23:28:53 UTC (rev
7872)
+++ code/stage/trunk/worlds/simple.world 2009-06-23 00:16:57 UTC (rev
7873)
@@ -9,7 +9,8 @@
interval_sim 100 # simulation timestep in milliseconds
interval_real 20 # real-time interval between simulation updates in
milliseconds
-quit_time 1800
+# time to pause (in GUI mode) or quit (in headless mode (-g)) the simulation
+quit_time 3600 # 1 hour of simulated time
paused 0
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Are you an open source citizen? Join us for the Open Source Bridge conference!
Portland, OR, June 17-19. Two days of sessions, one day of unconference: $250.
Need another reason to go? 24-hour hacker lounge. Register today!
http://ad.doubleclick.net/clk;215844324;13503038;v?http://opensourcebridge.org
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
