Update of /cvsroot/playerstage/code/stage/libstage
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv10772/libstage
Modified Files:
Tag: opengl
.cvsignore Makefile.manual canvas.cc model.cc
model_fiducial.cc model_laser.cc model_ranger.cc stage.hh
stest.cc typetable.cc world.cc
Log Message:
blobfinder nearly done, enhanced raytrace interface
Index: .cvsignore
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/.cvsignore,v
retrieving revision 1.1.2.1
retrieving revision 1.1.2.2
diff -C2 -d -r1.1.2.1 -r1.1.2.2
*** .cvsignore 29 Dec 2007 05:11:36 -0000 1.1.2.1
--- .cvsignore 7 Jan 2008 05:33:17 -0000 1.1.2.2
***************
*** 1,5 ****
Makefile
Makefile.in
! stest
! ptest
.DS_Store
--- 1,4 ----
Makefile
Makefile.in
! stage
.DS_Store
Index: stest.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/stest.cc,v
retrieving revision 1.1.2.22
retrieving revision 1.1.2.23
diff -C2 -d -r1.1.2.22 -r1.1.2.23
*** stest.cc 29 Dec 2007 05:07:40 -0000 1.1.2.22
--- stest.cc 7 Jan 2008 05:33:17 -0000 1.1.2.23
***************
*** 24,27 ****
--- 24,28 ----
StgModelRanger* ranger;
StgModelFiducial* fiducial;
+ StgModelBlobfinder* blobfinder;
} robot_t;
***************
*** 75,80 ****
assert(robots[i].ranger);
robots[i].ranger->Subscribe();
}
!
// start the clock
//world.Start();
--- 76,86 ----
assert(robots[i].ranger);
robots[i].ranger->Subscribe();
+
+ robots[i].blobfinder = (StgModelBlobfinder*)
+ robots[i].position->GetUnsubscribedModelOfType( "blobfinder" );
+ assert(robots[i].blobfinder);
+ robots[i].blobfinder->Subscribe();
}
!
// start the clock
//world.Start();
***************
*** 87,115 ****
{
! StgModelRanger* rgr = robots[i].ranger;
!
! if( rgr->samples == NULL )
! continue;
!
! // 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 );
! }
!
! if( dy == 0 )
! continue;
!
! if( dx == 0 )
! continue;
! assert( dy != 0 );
! assert( dx != 0 );
!
double resultant_angle = atan2( dy, dx );
double forward_speed = 0.0;
--- 93,123 ----
{
! //continue;
! StgModelRanger* rgr = robots[i].ranger;
!
! if( rgr->samples == NULL )
! continue;
!
! // 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 );
! }
!
! if( dy == 0 )
! continue;
!
! if( dx == 0 )
! continue;
!
! assert( dy != 0 );
! assert( dx != 0 );
!
double resultant_angle = atan2( dy, dx );
double forward_speed = 0.0;
***************
*** 130,142 ****
stg_velocity_t vel;
bzero(&vel,sizeof(vel));
! vel.x = forward_speed;
vel.y = side_speed;
vel.z = 0;
vel.a = turn_speed;
! printf( "robot %s [%.2f %.2f %.2f %.2f]\n",
! robots[i].position->Token(), vel.x, vel.y, vel.z, vel.a );
!
! robots[i].position->SetSpeed( forward_speed, side_speed, turn_speed );
}
--- 138,155 ----
stg_velocity_t vel;
bzero(&vel,sizeof(vel));
! vel.x = forward_speed;
vel.y = side_speed;
vel.z = 0;
vel.a = turn_speed;
! //printf( "robot %s [%.2f %.2f %.2f %.2f]\n",
! //robots[i].position->Token(), vel.x, vel.y, vel.z, vel.a );
!
! uint32_t bcount=0;
! stg_blobfinder_blob_t* blobs = robots[i].blobfinder->GetBlobs( &bcount
);
!
! printf( "robot %s sees %u blobs\n", robots[i].blobfinder->Token(),
bcount );
!
! //robots[i].position->SetSpeed( forward_speed, side_speed, turn_speed
);
}
Index: model_ranger.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/model_ranger.cc,v
retrieving revision 1.1.2.9
retrieving revision 1.1.2.10
diff -C2 -d -r1.1.2.9 -r1.1.2.10
*** model_ranger.cc 29 Dec 2007 05:07:40 -0000 1.1.2.9
--- model_ranger.cc 7 Jan 2008 05:33:17 -0000 1.1.2.10
***************
*** 240,250 ****
}
! bool ranger_raytrace_match( StgBlock* block, StgModel* finder )
{
! // printf( "ranger match sees %s %p %d zmin %.2f zmax %.2f global_zmin
%.2f global_zmax %.2f\n",
! // block->mod->Token(), block->mod, block->mod->LaserReturn(),
! // block->zmin, block->zmax,
! // block->zmin, block->zmax );
!
// Ignore myself, my children, and my ancestors.
return( block->Model()->RangerReturn() && !block->Model()->IsRelated(
finder ) );
--- 240,248 ----
}
! static bool ranger_match( StgBlock* block, StgModel* 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( block->Model()->RangerReturn() && !block->Model()->IsRelated(
finder ) );
***************
*** 254,261 ****
{
StgModel::Update();
!
! if( this->subs < 1 )
! return;
!
if( (sensors == NULL) || (sensor_count < 1 ))
return;
--- 252,256 ----
{
StgModel::Update();
!
if( (sensors == NULL) || (sensor_count < 1 ))
return;
***************
*** 270,290 ****
for( unsigned int t=0; t<sensor_count; t++ )
{
- double range = 0;
// TODO - reinstate multi-ray rangers
//for( int r=0; r<sensors[t].ray_count; r++ )
//{
! range = Raytrace( &sensors[t].pose,
! sensors[t].bounds_range.max,
! (stg_block_match_func_t)ranger_raytrace_match,
! this,
! NULL );
! //}
!
! // low-threshold the range
! if( range < sensors[t].bounds_range.min )
! range = sensors[t].bounds_range.min;
! samples[t] = range;
//sensors[t].error = TODO;
}
--- 265,280 ----
for( unsigned int t=0; t<sensor_count; t++ )
{
// TODO - reinstate multi-ray rangers
//for( int r=0; r<sensors[t].ray_count; r++ )
//{
! stg_raytrace_sample_t ray;
! Raytrace( sensors[t].pose,
! sensors[t].bounds_range.max,
! ranger_match,
! NULL,
! &ray );
! samples[t] = MAX( ray.range, sensors[t].bounds_range.min );
//sensors[t].error = TODO;
}
Index: model_laser.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/model_laser.cc,v
retrieving revision 1.1.2.16
retrieving revision 1.1.2.17
diff -C2 -d -r1.1.2.16 -r1.1.2.17
*** model_laser.cc 29 Dec 2007 05:07:40 -0000 1.1.2.16
--- model_laser.cc 7 Jan 2008 05:33:17 -0000 1.1.2.17
***************
*** 149,153 ****
bool laser_raytrace_match( StgBlock* testblock,
! StgModel* finder )
{
// Ignore the model that's looking and things that are invisible to
--- 149,154 ----
bool laser_raytrace_match( StgBlock* testblock,
! StgModel* finder,
! const void* dummy )
{
// Ignore the model that's looking and things that are invisible to
***************
*** 174,218 ****
double sample_incr = fov / (double)(sample_count-1);
- // if( debug )
- // {
- // glNewList( this->dl_debug_laser, GL_COMPILE );
- // glPushMatrix();
-
- // // go into global coords
- // stg_pose_t gpose;
- // GetGlobalPose( &gpose );
- // //gl_coord_shift( -gpose.x, -gpose.y, 0, -gpose.a );
- // glRotatef( RTOD(-gpose.a), 0,0,1 );
- // glTranslatef( -gpose.x, -gpose.y, 0 );
- // }
-
for( unsigned int t=0; t<sample_count; t += resolution )
{
! StgModel* hitmod = NULL;
! //printf( " [%d] %.2f\n", (int)t, samples[t].range );
! //printf( " [%d] ", (int)t );
!
! stg_pose_t pose;
! bzero( &pose, sizeof(pose) );
! pose.z =
!
! samples[t].range = Raytrace( bearing,
! range_max,
!
(stg_block_match_func_t)laser_raytrace_match,
! (const void*)this,
! &hitmod );
! // if we hit a model and it reflects brightly, we set
! // reflectance high, else low
! samples[t].reflectance =
! (hitmod && (hitmod->LaserReturn() >= LaserBright)) ? 1.0 : 0.0;
!
// todo - lower bound on range
bearing += sample_incr;
}
- //puts("");
-
// we may need to interpolate the samples we skipped
if( resolution > 1 )
--- 175,200 ----
double sample_incr = fov / (double)(sample_count-1);
for( unsigned int t=0; t<sample_count; t += resolution )
{
! stg_raytrace_sample_t sample;
! Raytrace( bearing,
! range_max,
! laser_raytrace_match,
! NULL,
! &sample );
! samples[t].range = sample.range;
! // if we hit a model and it reflects brightly, we set
! // reflectance high, else low
! if( sample.block && ( sample.block->Model()->LaserReturn() >=
LaserBright ) )
! samples[t].reflectance = 1;
! else
! samples[t].reflectance = 0;
!
// todo - lower bound on range
bearing += sample_incr;
}
// we may need to interpolate the samples we skipped
if( resolution > 1 )
***************
*** 237,246 ****
}
- // if( debug )
- // {
- // glPopMatrix();
- // glEndList();
- // }
-
data_dirty = true;
}
--- 219,222 ----
Index: model.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/model.cc,v
retrieving revision 1.1.2.20
retrieving revision 1.1.2.21
diff -C2 -d -r1.1.2.20 -r1.1.2.21
*** model.cc 29 Dec 2007 05:07:40 -0000 1.1.2.20
--- model.cc 7 Jan 2008 05:33:17 -0000 1.1.2.21
***************
*** 3,7 ****
*/
! /** @defgroup model Model
The basic model simulates an object with basic properties; position,
--- 3,7 ----
*/
! /** @ingroup StgModel
The basic model simulates an object with basic properties; position,
***************
*** 281,320 ****
}
! stg_meters_t StgModel::Raytrace( stg_radians_t angle,
! stg_meters_t range,
! stg_block_match_func_t func,
! const void* arg,
! StgModel** hitmod )
!
{
! stg_pose_t gpose;
! GetGlobalPose( &gpose );
! gpose.a += angle;
!
! return world->Raytrace( this,
! &gpose,
! range,
! func,
! arg,
! hitmod );
}
! stg_meters_t StgModel::Raytrace( stg_pose_t* pz,
! stg_meters_t range,
! stg_block_match_func_t func,
! const void* arg,
! StgModel** hitmod )
{
! stg_pose_t gpose;
! memcpy( &gpose, pz, sizeof(stg_pose_t) );
! LocalToGlobal( &gpose );
!
! return world->Raytrace( this,
! &gpose,
! range,
! func,
! arg,
! hitmod );
}
--- 281,344 ----
}
! void StgModel::Raytrace( stg_pose_t pose,
! stg_meters_t range,
! stg_block_match_func_t func,
! const void* arg,
! stg_raytrace_sample_t* sample )
{
! LocalToGlobal( &pose );
! world->Raytrace( pose,
! range,
! func,
! this,
! arg,
! sample );
}
! void StgModel::Raytrace( stg_radians_t bearing,
! stg_meters_t range,
! stg_block_match_func_t func,
! const void* arg,
! stg_raytrace_sample_t* sample )
! {
! stg_pose_t pose;
! bzero(&pose,sizeof(pose));
! pose.a = bearing;
!
! Raytrace( pose, range, func, arg, sample );
! }
+ void StgModel::Raytrace( stg_pose_t pose,
+ stg_meters_t range,
+ stg_radians_t fov,
+ stg_block_match_func_t func,
+ const void* arg,
+ stg_raytrace_sample_t* samples,
+ uint32_t sample_count )
{
! LocalToGlobal( &pose );
! world->Raytrace( pose,
! range,
! fov,
! func,
! this,
! arg,
! samples,
! sample_count );
! }
!
! void StgModel::Raytrace( stg_radians_t bearing,
! stg_meters_t range,
! stg_radians_t fov,
! stg_block_match_func_t func,
! const void* arg,
! stg_raytrace_sample_t* samples,
! uint32_t sample_count )
! {
! stg_pose_t pose;
! bzero(&pose,sizeof(pose));
! pose.a = bearing;
!
! Raytrace( pose, range, fov, func, arg, samples, sample_count );
}
***************
*** 1267,1282 ****
stg_pose_t raypose;
stg_pose_sum( &raypose, posedelta, &edgepose );
!
// raytrace in local coordinates
! Raytrace( &raypose,
range,
(stg_block_match_func_t)collision_match,
! (const void*)this,
! &hitmod );
!
! // if( hitmod )
! // {
! // if( hitx ) *hitx = itl->x; // report them
! // if( hity ) *hity = itl->y;
}
}
--- 1291,1305 ----
stg_pose_t raypose;
stg_pose_sum( &raypose, posedelta, &edgepose );
!
// raytrace in local coordinates
! stg_raytrace_sample_t sample;
! Raytrace( raypose,
range,
(stg_block_match_func_t)collision_match,
! NULL,
! &sample );
!
! if( sample.block )
! hitmod = sample.block->Model();
}
}
Index: world.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/world.cc,v
retrieving revision 1.1.2.20
retrieving revision 1.1.2.21
diff -C2 -d -r1.1.2.20 -r1.1.2.21
*** world.cc 29 Dec 2007 05:07:40 -0000 1.1.2.20
--- world.cc 7 Jan 2008 05:33:17 -0000 1.1.2.21
***************
*** 465,478 ****
}
! stg_meters_t StgWorld::Raytrace( StgModel* finder,
! stg_pose_t* pose,
! stg_meters_t max_range,
! stg_block_match_func_t func,
! const void* arg,
! StgModel** hit_model )
{
// find the global integer bitmap address of the ray
! int32_t x = (int32_t)((pose->x+width/2.0)*ppm);
! int32_t y = (int32_t)((pose->y+height/2.0)*ppm);
int32_t z = 0;
--- 465,521 ----
}
! // raytrace wrapper that returns the model hit, rather than the block
! // stg_meters_t StgWorld::Raytrace( StgModel* finder,
! // stg_pose_t* pose,
! // stg_meters_t max_range,
! // stg_block_match_func_t func,
! // const void* arg,
! // StgModel** hit_model )
! // {
! // StgBlock* block = NULL;
! // stg_meters_t m = RayTrace( finder, pose, max_range, func, arg, &block );
!
! // if( block && hit_model )
! // *hit_model = block->Model();
!
! // return range;
! // }
!
!
! void StgWorld::Raytrace( stg_pose_t pose, // global pose
! stg_meters_t range,
! stg_radians_t fov,
! stg_block_match_func_t func,
! StgModel* model,
! const void* arg,
! stg_raytrace_sample_t* samples, // preallocated
storage for samples
! uint32_t sample_count ) // number of samples
! {
! pose.a -= fov/2.0; // direction of first ray
! stg_radians_t angle_incr = fov/(double)sample_count;
!
! for( uint32_t s=0; s < sample_count; s++ )
! {
! Raytrace( pose, range, func, model, arg, &samples[s] );
! pose.a += angle_incr;
! }
! }
!
!
! void StgWorld::Raytrace( stg_pose_t pose, // global pose
! stg_meters_t range,
! stg_block_match_func_t func,
! StgModel* mod,
! const void* arg,
! stg_raytrace_sample_t* sample )
{
+ // initialize the sample
+ memcpy( &sample->pose, &pose, sizeof(stg_pose_t)); // pose stays fixed
+ sample->range = range; // we might change this below
+ sample->block = NULL; // we might change this below
+
// find the global integer bitmap address of the ray
! int32_t x = (int32_t)((pose.x+width/2.0)*ppm);
! int32_t y = (int32_t)((pose.y+height/2.0)*ppm);
int32_t z = 0;
***************
*** 481,493 ****
// and the x and y offsets of the ray
! int32_t dx = (int32_t)(ppm*max_range * cos(pose->a));
! int32_t dy = (int32_t)(ppm*max_range * sin(pose->a));
int32_t dz = 0;
! if( finder->debug )
! RecordRay( pose->x,
! pose->y,
! pose->x + max_range * cos(pose->a),
! pose->y + max_range * sin(pose->a) );
// fast integer line 3d algorithm adapted from Cohen's code from
--- 524,536 ----
// and the x and y offsets of the ray
! int32_t dx = (int32_t)(ppm*range * cos(pose.a));
! int32_t dy = (int32_t)(ppm*range * sin(pose.a));
int32_t dz = 0;
! // if( finder->debug )
! // RecordRay( pose.x,
! // pose.y,
! // pose.x + range.max * cos(pose.a),
! // pose.y + range.max * sin(pose.a) );
// fast integer line 3d algorithm adapted from Cohen's code from
***************
*** 538,551 ****
// if this block does not belong to the searching model and it
// matches the predicate and it's in the right z range
! if( block && (block->Model() != finder) &&
! block->IntersectGlobalZ( pose->z ) &&
! (*func)( block, arg ) )
{
// a hit!
! if( hit_model )
! *hit_model = block->Model();
!
! // how far away was that strike?
! return hypot( (x-xstart)/ppm, (y-ystart)/ppm );
}
}
--- 581,594 ----
// if this block does not belong to the searching model and it
// matches the predicate and it's in the right z range
! if( //block && (block->Model() != finder) &&
! block->IntersectGlobalZ( pose.z ) &&
! (*func)( block, mod, arg ) )
{
// a hit!
! //if( hit_model )
! //*hit_model = block->Model();
! sample->block = block;
! sample->range = hypot( (x-xstart)/ppm, (y-ystart)/ppm );
! return;
}
}
***************
*** 573,589 ****
}
}
-
- // hit nothing, so return max range
- return max_range;
- }
! void stg_model_save_cb( gpointer key, gpointer data, gpointer user )
! {
! ((StgModel*)data)->Save();
}
! void stg_model_reload_cb( gpointer key, gpointer data, gpointer user )
{
! ((StgModel*)data)->Load();
}
--- 616,627 ----
}
}
! // hit nothing
! return;
}
! static void _save_cb( gpointer key, gpointer data, gpointer user )
{
! ((StgModel*)data)->Save();
}
***************
*** 593,601 ****
//g_hash_table_foreach( this->models_by_id, stg_model_save_cb, NULL );
! ForEachModel( stg_model_save_cb, NULL );
this->wf->Save(NULL);
}
// reload the current worldfile
void StgWorld::Reload( void )
--- 631,644 ----
//g_hash_table_foreach( this->models_by_id, stg_model_save_cb, NULL );
! ForEachModel( _save_cb, NULL );
this->wf->Save(NULL);
}
+ static void _reload_cb( gpointer key, gpointer data, gpointer user )
+ {
+ ((StgModel*)data)->Load();
+ }
+
// reload the current worldfile
void StgWorld::Reload( void )
***************
*** 607,611 ****
//g_hash_table_foreach( this->models_by_id, stg_model_reload_cb, NULL );
! ForEachModel( stg_model_reload_cb, NULL );
}
--- 650,654 ----
//g_hash_table_foreach( this->models_by_id, stg_model_reload_cb, NULL );
! ForEachModel( _reload_cb, NULL );
}
Index: typetable.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/typetable.cc,v
retrieving revision 1.1.2.6
retrieving revision 1.1.2.7
diff -C2 -d -r1.1.2.6 -r1.1.2.7
*** typetable.cc 24 Dec 2007 10:50:45 -0000 1.1.2.6
--- typetable.cc 7 Jan 2008 05:33:17 -0000 1.1.2.7
***************
*** 6,14 ****
stg_typetable_entry_t typetable[] = {
! { "model", StgModel::Create },
! { "laser", StgModelLaser::Create },
! { "position", StgModelPosition::Create },
! { "ranger", StgModelRanger::Create },
! { "fiducial", StgModelFiducial::Create },
{ NULL, NULL } // this must be the last entry
};
--- 6,15 ----
stg_typetable_entry_t typetable[] = {
! { "model", StgModel::Create },
! { "laser", StgModelLaser::Create },
! { "position", StgModelPosition::Create },
! { "ranger", StgModelRanger::Create },
! { "fiducial", StgModelFiducial::Create },
! { "blobfinder", StgModelBlobfinder::Create },
{ NULL, NULL } // this must be the last entry
};
Index: model_fiducial.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/model_fiducial.cc,v
retrieving revision 1.1.2.3
retrieving revision 1.1.2.4
diff -C2 -d -r1.1.2.3 -r1.1.2.4
*** model_fiducial.cc 29 Dec 2007 05:07:40 -0000 1.1.2.3
--- model_fiducial.cc 7 Jan 2008 05:33:17 -0000 1.1.2.4
***************
*** 95,99 ****
}
! bool fiducial_raytrace_match( StgBlock* testblock, StgModel* finder )
{
return( ! finder->IsRelated( testblock->Model() ) );
--- 95,99 ----
}
! static bool fiducial_raytrace_match( StgBlock* testblock, StgModel* finder,
const void* dummy )
{
return( ! finder->IsRelated( testblock->Model() ) );
***************
*** 156,172 ****
- StgModel* hitmod = NULL;
-
//printf( "bearing %.2f\n", RTOD(bearing) );
! range = Raytrace( dtheta,
! max_range_anon,
! (stg_block_match_func_t)fiducial_raytrace_match,
! (const void*)this,
! &hitmod );
! //printf( "ray hit %s and was seeking LOS to %s\n",
! // hitmod ? hitmod->Token() : "null",
! // him->Token() );
// if it was him, we can see him
--- 156,175 ----
//printf( "bearing %.2f\n", RTOD(bearing) );
! stg_raytrace_sample_t ray;
! Raytrace( dtheta,
! max_range_anon,
! fiducial_raytrace_match,
! NULL,
! &ray );
!
! range = ray.range;
! StgModel* hitmod = ray.block->Model();
!
! printf( "ray hit %s and was seeking LOS to %s\n",
! hitmod ? hitmod->Token() : "null",
! him->Token() );
// if it was him, we can see him
***************
*** 247,267 ****
{
// draw the FOV
! GLUquadric* quadric = gluNewQuadric();
!
! PushColor( 0,0,0,0.2 );
! gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
! //glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
! gluPartialDisk( quadric,
! 0,
! max_range_anon,
! 20, // slices
! 1, // loops
! RTOD( M_PI/2.0 + fov/2.0), // start angle
! RTOD(-fov) ); // sweep angle
! gluDeleteQuadric( quadric );
! PopColor();
if( data->len == 0 )
--- 250,269 ----
{
// draw the FOV
! // GLUquadric* quadric = gluNewQuadric();
! // PushColor( 0,0,0,0.2 );
! // gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
! // //glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
! // gluPartialDisk( quadric,
! // 0,
! // max_range_anon,
! // 20, // slices
! // 1, // loops
! // RTOD( M_PI/2.0 + fov/2.0), // start angle
! // RTOD(-fov) ); // sweep angle
! // gluDeleteQuadric( quadric );
! // PopColor();
if( data->len == 0 )
Index: Makefile.manual
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/Makefile.manual,v
retrieving revision 1.1.2.10
retrieving revision 1.1.2.11
diff -C2 -d -r1.1.2.10 -r1.1.2.11
*** Makefile.manual 24 Dec 2007 10:50:45 -0000 1.1.2.10
--- Makefile.manual 7 Jan 2008 05:33:17 -0000 1.1.2.11
***************
*** 14,23 ****
model.cc \
model_callbacks.cc \
model_laser.cc \
model_load.cc \
model_position.cc \
model_props.cc \
- model_ranger.cc \
- model_fiducial.cc \
stest.cc \
stage.cc \
--- 14,24 ----
model.cc \
model_callbacks.cc \
+ model_ranger.cc \
+ model_blobfinder.cc \
model_laser.cc \
+ model_fiducial.cc \
model_load.cc \
model_position.cc \
model_props.cc \
stest.cc \
stage.cc \
***************
*** 27,30 ****
--- 28,33 ----
worldgui.cc
+
+
CXXFLAGS+= -g -Wall -I.. -I../replace `fltk-config --use-gl --cflags`
`pkg-config --cflags glib-2.0 gdk-pixbuf-2.0`
LIBS= `fltk-config --use-gl --ldflags` `pkg-config --libs gdk-pixbuf-2.0`
-framework OpenGL
Index: stage.hh
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/stage.hh,v
retrieving revision 1.1.2.22
retrieving revision 1.1.2.23
diff -C2 -d -r1.1.2.22 -r1.1.2.23
*** stage.hh 29 Dec 2007 05:07:40 -0000 1.1.2.22
--- stage.hh 7 Jan 2008 05:33:17 -0000 1.1.2.23
***************
*** 99,135 ****
- /** @defgroup libstage libstage API reference
-
- libstage (The Stage Library) provides a C++ code library for
- simulating a population of mobile robots and sensors. It is
- usually used as a plugin driver for <a
- href="http://playerstage.sf.net/player/player.html";>Player</a>,
- but it can also be used directly to build custom simulations.
-
- libstage is modular and fairly simple to use. The following code is
- enough to get a complete robot simulation running:
-
- @verbatim
- #include "stage.h"
-
- int main( int argc, char* argv[] )
- {
- stg_world_t* world = stg_world_create_from_file( argv[1] );
-
- while( (stg_world_update( world,TRUE )==0) )
- {}
-
- stg_world_destroy( world );
-
- return 0;
- }
- @endverbatim
-
- @par Contact and support
-
- For help with libstage, please use the mailing list [EMAIL PROTECTED]
- @{
- */
-
// forward declare
class StgModel;
--- 99,102 ----
***************
*** 147,157 ****
#define STG_TOKEN_MAX 64
- /** @defgroup types Measurement Types
- Basic self-describing measurement types. All packets with real
- measurements are specified in these terms so changing types here
- should work throughout the code.
- @{
- */
-
/** uniquely identify a model */
typedef uint32_t stg_id_t;
--- 114,117 ----
***************
*** 272,287 ****
} stg_ptz_t;
- /[EMAIL PROTECTED]/ // end types
-
-
- /** Returns the real (wall-clock) time in milliseconds since the
- epoch. */
- //stg_msec_t stg_realtime();
-
- /** Returns the real (wall-clock) time in milliseconds since the
- simulation started. */
- //stg_msec_t stg_realtime_since_start();
-
-
/** take binary sign of a, either -1, or 1 if >= 0 */
--- 232,235 ----
***************
*** 294,304 ****
- // POINTS ---------------------------------------------------------
- /** @ingroup libstage
- @defgroup stg_points Points
- Defines a point on the 2D plane.
- @{
- */
-
/** define a point on the plane */
typedef struct
--- 242,245 ----
***************
*** 337,341 ****
square.*/
stg_point_t* stg_unit_square_points_create();
- /[EMAIL PROTECTED]/
--- 278,281 ----
***************
*** 363,374 ****
- // MODEL --------------------------------------------------------
-
- // group the docs of all the model types
- /** @ingroup libstage
- @defgroup stg_model Models
- Implements the basic object
- @{
- */
// Movement masks for figures
--- 303,306 ----
***************
*** 377,383 ****
#define STG_MOVE_SCALE (1 << 2)
! typedef int stg_movemask_t;
!
!
#define STG_MP_PREFIX "_mp_"
--- 309,313 ----
#define STG_MOVE_SCALE (1 << 2)
! typedef int stg_movemask_t;
#define STG_MP_PREFIX "_mp_"
***************
*** 395,430 ****
#define STG_MP_MASS "_mp_mass"
- /* TODO - complete the set of named properties */
-
- /** @brief Set a named property of a Stage model.
-
- Set a property of a Stage model.
-
- This function can set both predefined and user-defined
- properties of a model. Predefined properties are intrinsic to
- every model, such as pose and color. Every supported predefined
- properties has its identifying string defined as a preprocessor
- macro in stage.h. Users should use the macro instead of a
- hard-coded string, so that the compiler can help you to avoid
- mis-naming properties.
-
- User-defined properties allow the user to attach arbitrary data
- pointers to a model. User-defined property data is not copied,
- so the original pointer must remain valid. User-defined property
- names are simple strings. Names beginning with an underscore
- ('_') are reserved for internal libstage use: users should not
- use names beginning with underscore (at risk of causing very
- weird behaviour).
-
- Any callbacks registered for the named property will be called.
! Returns 0 on success, or a positive error code on failure.
!
! *CAUTION* The caller is responsible for making sure the pointer
! points to data of the correct type for the property, so use
! carefully. Check the docs or the equivalent
! stg_model_set_<property>() function definition to see the type
! of data required for each property.
! */
/// laser return value
--- 325,359 ----
#define STG_MP_MASS "_mp_mass"
! /** @brief Set a named property of a Stage model.
!
! Set a property of a Stage model.
!
! This function can set both predefined and user-defined
! properties of a model. Predefined properties are intrinsic to
! every model, such as pose and color. Every supported predefined
! properties has its identifying string defined as a preprocessor
! macro in stage.h. Users should use the macro instead of a
! hard-coded string, so that the compiler can help you to avoid
! mis-naming properties.
!
! User-defined properties allow the user to attach arbitrary data
! pointers to a model. User-defined property data is not copied,
! so the original pointer must remain valid. User-defined property
! names are simple strings. Names beginning with an underscore
! ('_') are reserved for internal libstage use: users should not
! use names beginning with underscore (at risk of causing very
! weird behaviour).
!
! Any callbacks registered for the named property will be called.
!
! Returns 0 on success, or a positive error code on failure.
!
! *CAUTION* The caller is responsible for making sure the pointer
! points to data of the correct type for the property, so use
! carefully. Check the docs or the equivalent
! stg_model_set_<property>() function definition to see the type
! of data required for each property.
! */
/// laser return value
***************
*** 514,522 ****
// Some useful macros
- /** @ingroup libstage
- @defgroup libstage_util Utilities
- Various useful macros and functions that don't belong anywhere else.
- @{
- */
/** Look up the color in the X11 database. (i.e. transform color
--- 443,446 ----
***************
*** 546,582 ****
-
- /** @ingroup libstage_util
- @defgroup floatcomparison Floating point comparisons
-
- Macros for comparing floating point numbers. It's a troublesome
- limitation of C and C++ that floating point comparisons are not very
- accurate. These macros multiply their arguments by a large number
- before comparing them, to improve resolution.
-
- @{
- */
-
- /** Precision of comparison. The number of zeros to the left of the
- decimal point determines the accuracy of the comparison in decimal
- places to the right of the point. E.g. precision of 100000.0 gives
- a comparison precision of within 0.000001 */
- // #define PRECISION 100000.0
-
- // /** TRUE iff A and B are equal to within PRECISION */
- // #define EQ(A,B) ((lrint(A*PRECISION))==(lrint(B*PRECISION)))
-
- // /** TRUE iff A is less than B, subject to PRECISION */
- // #define LT(A,B) ((lrint(A*PRECISION))<(lrint(B*PRECISION)))
-
- // /** TRUE iff A is greater than B, subject to PRECISION */
- // #define GT(A,B) ((lrint(A*PRECISION))>(lrint(B*PRECISION)))
-
- // /** TRUE iff A is greater than or equal B, subject to PRECISION */
- // #define GTE(A,B) ((lrint(A*PRECISION))>=(lrint(B*PRECISION)))
-
- // /** TRUE iff A is less than or equal to B, subject to PRECISION */
- // #define LTE(A,B) ((lrint(A*PRECISION))<=(lrint(B*PRECISION)))
-
#define STG_SHOW_BLOCKS 1
#define STG_SHOW_DATA 2
--- 470,473 ----
***************
*** 598,612 ****
class StgModel;
- /** @ingroup libstage
- @defgroup libstage_internal Internals
-
- These are internal docs. Don't use these functions in user
- code. Let us know if there is anything useful here that should be
- exposed in the external interface.
-
- @{
- */
-
-
#define STG_DEFAULT_WINDOW_WIDTH 800
--- 489,492 ----
***************
*** 666,675 ****
- // // ROTATED RECTANGLES -------------------------------------------------
-
- /** @ingroup libstage_internal
- @defgroup rotrect Rotated Rectangles
- @{
- */
/** defines a rectangle of [size] located at [pose] */
--- 546,549 ----
***************
*** 696,707 ****
unsigned int* heightp );
- /[EMAIL PROTECTED]/
-
// /** matching function should return 0 iff the candidate block is
// acceptable */
class StgBlock;
! typedef bool (*stg_block_match_func_t)(StgBlock* candidate, const void* arg );
!
// TODO - some of this needs to be implemented, the rest junked.
--- 570,578 ----
unsigned int* heightp );
// /** matching function should return 0 iff the candidate block is
// acceptable */
class StgBlock;
! typedef bool (*stg_block_match_func_t)(StgBlock* candidate, StgModel* finder,
const void* arg );
// TODO - some of this needs to be implemented, the rest junked.
***************
*** 971,975 ****
};
!
--- 842,853 ----
};
! /** raytrace sample
! */
! typedef struct
! {
! stg_pose_t pose; ///< location and direction of the ray origin
! stg_meters_t range; ///< range to beam hit in meters
! StgBlock* block; ///< the block struck by this beam
! } stg_raytrace_sample_t;
***************
*** 1054,1063 ****
//void MapBlock( StgBlock* block );
! stg_meters_t Raytrace( StgModel* finder,
! stg_pose_t* pose,
! stg_meters_t max_range,
! stg_block_match_func_t func,
! const void* arg,
! StgModel** hit_model );
void RemoveBlock( int x, int y, StgBlock* block )
--- 932,950 ----
//void MapBlock( StgBlock* block );
! void Raytrace( stg_pose_t pose,
! stg_meters_t range,
! stg_block_match_func_t func,
! StgModel* finder,
! const void* arg,
! stg_raytrace_sample_t* sample );
!
! void Raytrace( stg_pose_t pose,
! stg_meters_t range,
! stg_radians_t fov,
! stg_block_match_func_t func,
! StgModel* finder,
! const void* arg,
! stg_raytrace_sample_t* samples,
! uint32_t sample_count );
void RemoveBlock( int x, int y, StgBlock* block )
***************
*** 1131,1134 ****
--- 1018,1022 ----
};
+
typedef struct {
stg_pose_t pose;
***************
*** 1229,1247 ****
int TreeToPtrArray( GPtrArray* array );
-
- // this version raytraces from our local origin
- stg_meters_t Raytrace( stg_radians_t angle,
- stg_meters_t range,
- stg_block_match_func_t func,
- const void* arg,
- StgModel** hitmod );
// raytraces from the point and heading identified by pose, in local coords
! stg_meters_t Raytrace( stg_pose_t* pose,
! stg_meters_t range,
! stg_block_match_func_t func,
! const void* arg,
! StgModel** hitmod );
/** Causes this model and its children to recompute their global
position instead of using a cached pose in
--- 1117,1155 ----
int TreeToPtrArray( GPtrArray* array );
+
// raytraces from the point and heading identified by pose, in local coords
! void Raytrace( stg_pose_t pose,
! stg_meters_t range,
! stg_block_match_func_t func,
! const void* arg,
! stg_raytrace_sample_t* sample );
!
! // this version raytraces from our local origin in the direction [angle]
! void Raytrace( stg_radians_t angle,
! stg_meters_t range,
! stg_block_match_func_t func,
! const void* arg,
! stg_raytrace_sample_t* sample );
!
! // raytraces from the point and heading identified by pose, in local coords
! void Raytrace( stg_pose_t pose,
! stg_meters_t range,
! stg_radians_t fov,
! stg_block_match_func_t func,
! const void* arg,
! stg_raytrace_sample_t* samples,
! uint32_t sample_count );
+ // this version raytraces from our local origin in the direction [angle]
+ void Raytrace( stg_radians_t angle,
+ stg_meters_t range,
+ stg_radians_t fov,
+ stg_block_match_func_t func,
+ const void* arg,
+ stg_raytrace_sample_t* samples,
+ uint32_t sample_count );
+
+
/** Causes this model and its children to recompute their global
position instead of using a cached pose in
***************
*** 1537,1545 ****
StgModel* Model(){ return mod; };
! inline stg_point_t* Points( unsigned int *count )
{ if( count ) *count = pt_count; return pts; };
!
! inline bool IntersectGlobalZ( stg_meters_t z )
{ return( z >= global_zmin && z < global_zmax ); }
private:
--- 1445,1456 ----
StgModel* Model(){ return mod; };
! stg_point_t* Points( unsigned int *count )
{ if( count ) *count = pt_count; return pts; };
!
! bool IntersectGlobalZ( stg_meters_t z )
{ return( z >= global_zmin && z < global_zmax ); }
+
+ stg_color_t Color()
+ { return color; };
private:
***************
*** 1740,1776 ****
// end doc group libstage_utilities
- /** @} */
// BLOBFINDER MODEL --------------------------------------------------------
! #define STG_BLOB_CHANNELS_MAX 16
! /** blobfinder config packet
! */
! typedef struct
! {
! int channel_count; // 0 to STG_BLOBFINDER_CHANNELS_MAX
! // todo - get rid of this static size
! stg_color_t channels[STG_BLOB_CHANNELS_MAX];
! int scan_width;
! int scan_height;
! stg_meters_t range_max;
! } stg_blobfinder_config_t;
! /** blobfinder data packet
! */
! typedef struct
! {
! int channel;
! stg_color_t color;
! int xpos, ypos; // all values are in pixels
! //int width, height;
! int left, top, right, bottom;
! int area;
! stg_meters_t range;
! } stg_blobfinder_blob_t;
// ENERGY model --------------------------------------------------------------
--- 1651,1734 ----
// end doc group libstage_utilities
// BLOBFINDER MODEL --------------------------------------------------------
! /** blobfinder data packet
! */
! typedef struct
! {
! //int channel;
! stg_color_t color;
! int xpos, ypos; // all values are in pixels
! //int width, height;
! int left, top, right, bottom;
! int area;
! stg_meters_t range;
! } stg_blobfinder_blob_t;
!
!
! class StgModelBlobfinder : public StgModel
! {
! private:
! GArray* colors;
! GArray* blobs;
! // predicate for ray tracing
! static bool BlockMatcher( StgBlock* testblock, StgModel* finder );
!
! public:
!
! unsigned int scan_width;
! unsigned int scan_height;
! stg_meters_t range;
! stg_radians_t fov;
!
! // TODO
! // stg_radians_t pan;
!
! // constructor
! StgModelBlobfinder( StgWorld* world,
! StgModel* parent,
! stg_id_t id,
! char* typestr);
!
! // destructor
! ~StgModelBlobfinder();
!
! virtual void Startup();
! virtual void Shutdown();
! virtual void Update();
! virtual void Load();
! virtual void DataVisualize();
!
! 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 );
+ /** Stop tracking blobs with this color */
+ void RemoveColor( stg_color_t col );
+
+ /** Stop tracking all colors. Call this to clear the defaults, then
+ add colors individually with AddColor(); */
+ void RemoveAllColors();
+
+ // static wrapper for the constructor - all models must implement
+ // this method and add an entry in typetable.cc
+ static StgModel* Create( StgWorld* world,
+ StgModel* parent,
+ stg_id_t id,
+ char* typestr )
+ {
+ return (StgModel*)new StgModelBlobfinder( world, parent, id, typestr );
+ }
+ };
+
// ENERGY model --------------------------------------------------------------
***************
*** 1842,1846 ****
! // LASER MODEL --------------------------------------------------------
/** laser sample packet
--- 1800,1841 ----
! // SCANNER MODEL --------------------------------------------------------
!
!
! // typedef struct
! // {
! // uint32_t sample_count;
! // uint32_t resolution;
! // stg_pose_t pose;
! // stg_range_bounds_t range_bounds;
! // stg_radians_t fov;
! // stg_block_match_func_t func; ///< block visibility predicate
! // void* arg; ///< argument passed to the block match predicate
! // } stg_scanner_cfg_t;
!
! // class StgScanner
! // {
! // protected:
!
! // stg_scanner_sample_t* samples;
! // stg_scanner_cfg_t cfg;
! // StgModel* model;
!
! // public:
! // StgScanner();
! // ~StgScanner();
!
! // stg_scanner_sample_t* Scan( StgModel uint32_t* count );
! // stg_scanner_sample_t* LastScan( uint32_t* count );
!
! // void Load( CWorldfile* wf, int section );
! // void Save( CWorldfile* wf, int section );
! // void Visualize();
!
! // void GetConfig( stg_scanner_cfg_t* cfg );
! // void SetConfig( stg_scanner_cfg_t* cfg );
! // };
!
! // LASER MODEL --------------------------------------------------------
/** laser sample packet
***************
*** 2272,2280 ****
- // end the group of all models
- /[EMAIL PROTECTED]/
-
- // end documentation group libstage
- /[EMAIL PROTECTED]/
-
#endif
--- 2267,2269 ----
Index: canvas.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/canvas.cc,v
retrieving revision 1.1.2.8
retrieving revision 1.1.2.9
diff -C2 -d -r1.1.2.8 -r1.1.2.9
*** canvas.cc 29 Dec 2007 05:07:40 -0000 1.1.2.8
--- canvas.cc 7 Jan 2008 05:33:17 -0000 1.1.2.9
***************
*** 488,492 ****
}
! for( GList* it=world->children; it; it=it->next )
{
uint32_t flags = showflags;
--- 488,493 ----
}
! if( showflags & STG_SHOW_BLOCKS )
! for( GList* it=world->children; it; it=it->next )
{
uint32_t flags = showflags;
-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2005.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
