Revision: 7308
http://playerstage.svn.sourceforge.net/playerstage/?rev=7308&view=rev
Author: rtv
Date: 2009-01-28 09:05:20 +0000 (Wed, 28 Jan 2009)
Log Message:
-----------
improved fasr example. working on reparenting ready for gripper - not yet
functional
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/libstage/CMakeLists.txt
code/stage/trunk/libstage/ancestor.cc
code/stage/trunk/libstage/canvas.cc
code/stage/trunk/libstage/charger.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_callbacks.cc
code/stage/trunk/libstage/model_fiducial.cc
code/stage/trunk/libstage/model_load.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/worlds/fasr.world
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2009-01-27 08:59:09 UTC (rev
7307)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2009-01-28 09:05:20 UTC (rev
7308)
@@ -1,17 +1,18 @@
#include "stage.hh"
using namespace Stg;
+const bool verbose = false;
+
+// navigation control params
const double cruisespeed = 0.4;
const double avoidspeed = 0.05;
const double avoidturn = 0.5;
const double minfrontdistance = 0.7;
const double stopdist = 0.5;
-const bool verbose = false;
const int avoidduration = 10;
const int workduration = 20;
const int payload = 4;
-
double have[4][4] = {
{ 90, 180, 180, 180 },
{ 90, -90, 0, -90 },
@@ -25,10 +26,11 @@
{ -90, -90, 180, 180 },
{ -90, -180, -90, -90 }
};
-
-typedef struct
+
+class Robot
{
+private:
ModelPosition* pos;
ModelLaser* laser;
ModelRanger* ranger;
@@ -37,56 +39,72 @@
Model *source, *sink;
int avoidcount, randcount;
int work_get, work_put;
+
+ static int LaserUpdate( ModelLaser* mod, Robot* robot );
+ static int PositionUpdate( ModelPosition* mod, Robot* robot );
+ static int FiducialUpdate( ModelFiducial* mod, Robot* robot );
-} robot_t;
+public:
+ Robot( ModelPosition* pos,
+ ModelLaser* laser,
+ ModelRanger* ranger,
+ ModelFiducial* fiducial,
+ ModelBlobfinder* blob,
+ Model* source,
+ Model* sink )
+ : pos(pos),
+ laser(laser),
+ ranger(ranger),
+ blobfinder(blobfinder),
+ fiducial(fiducial),
+ source(source),
+ sink(sink),
+ avoidcount(0),
+ randcount(0),
+ work_get(0),
+ work_put(0)
+ {
+ // need at least these models to get any work done
+ assert( pos );
+ assert( laser );
+
+ pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, this );
+ laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this );
-int LaserUpdate( Model* mod, robot_t* robot );
-int PositionUpdate( Model* mod, robot_t* robot );
+ if( fiducial ) // optional
+ fiducial->AddUpdateCallback(
(stg_model_callback_t)FiducialUpdate, this );
+ }
+};
+
// Stage calls this when the model starts up
extern "C" int Init( Model* mod )
{
- robot_t* robot = new robot_t;
- robot->work_get = 0;
- robot->work_put = 0;
-
- robot->pos = (ModelPosition*)mod;
-
- robot->laser = (ModelLaser*)mod->GetUnusedModelOfType( MODEL_TYPE_LASER );
- assert( robot->laser );
- robot->laser->Subscribe();
-
- robot->fiducial = (ModelFiducial*)mod->GetUnusedModelOfType(
MODEL_TYPE_FIDUCIAL );
- assert( robot->fiducial );
- robot->fiducial->Subscribe();
-
- robot->ranger = (ModelRanger*)mod->GetUnusedModelOfType( MODEL_TYPE_RANGER );
- assert( robot->ranger );
- //robot->ranger->Subscribe();
-
- robot->avoidcount = 0;
- robot->randcount = 0;
-
- robot->laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, robot );
- robot->pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, robot );
-
- robot->source = mod->GetWorld()->GetModel( "source" );
- assert(robot->source);
-
- robot->sink = mod->GetWorld()->GetModel( "sink" );
- assert(robot->sink);
+ Robot* robot = new Robot( (ModelPosition*)mod,
+
(ModelLaser*)mod->GetUnusedModelOfType( MODEL_TYPE_LASER ),
+
(ModelRanger*)mod->GetUnusedModelOfType( MODEL_TYPE_RANGER ),
+
(ModelFiducial*)mod->GetUnusedModelOfType( MODEL_TYPE_FIDUCIAL ),
+ NULL,
+
mod->GetWorld()->GetModel( "source" ),
+
mod->GetWorld()->GetModel( "sink" ) );
return 0; //ok
}
// inspect the laser data and decide what to do
-int LaserUpdate( Model* mod, robot_t* robot )
+int Robot::LaserUpdate( ModelLaser* laser, Robot* robot )
{
- // get the data
+ if( laser->power_pack && laser->power_pack->charging )
+ printf( "model %s power pack @%p is charging\n",
+ laser->Token(), laser->power_pack );
+
+ // Get the data
uint32_t sample_count=0;
- stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count );
- assert(scan);
+ stg_laser_sample_t* scan = laser->GetSamples( &sample_count );
+
+ if( scan == NULL )
+ return 0;
bool obstruction = false;
bool stop = false;
@@ -194,9 +212,9 @@
return 0;
}
-int PositionUpdate( Model* mod, robot_t* robot )
+int Robot::PositionUpdate( ModelPosition* pos, Robot* robot )
{
- Pose pose = robot->pos->GetPose();
+ Pose pose = pos->GetPose();
//printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
// pose.x, pose.y, pose.z, pose.a );
@@ -204,7 +222,7 @@
//pose.z += 0.0001;
//robot->pos->SetPose( pose );
- if( robot->pos->GetFlagCount() < payload &&
+ if( pos->GetFlagCount() < payload &&
hypot( -7-pose.x, -7-pose.y ) < 2.0 )
{
if( ++robot->work_get > workduration )
@@ -213,7 +231,7 @@
robot->source->Lock();
// transfer a chunk from source to robot
- robot->pos->PushFlag( robot->source->PopFlag() );
+ pos->PushFlag( robot->source->PopFlag() );
robot->source->Unlock();
robot->work_get = 0;
@@ -229,7 +247,7 @@
//puts( "dropping" );
// transfer a chunk between robot and goal
- robot->sink->PushFlag( robot->pos->PopFlag() );
+ robot->sink->PushFlag( pos->PopFlag() );
robot->sink->Unlock();
robot->work_put = 0;
@@ -240,3 +258,27 @@
return 0; // run again
}
+
+
+int Robot::FiducialUpdate( ModelFiducial* mod, Robot* robot )
+{
+ for( unsigned int i = 0; i < mod->fiducial_count; i++ )
+ {
+ stg_fiducial_t* f = &mod->fiducials[i];
+
+ //printf( "fiducial %d is %d at %.2f m %.2f radians\n",
+ // i, f->id, f->range, f->bearing );
+
+ if( f->range < 1 )
+ {
+ printf( "attempt to grab model @%p %s\n",
+ f->mod, f->mod->Token() );
+
+ // working on picking up models
+ //robot->pos->BecomeParentOf( f->mod );
+ }
+
+ }
+
+ return 0; // run again
+}
Modified: code/stage/trunk/libstage/CMakeLists.txt
===================================================================
--- code/stage/trunk/libstage/CMakeLists.txt 2009-01-27 08:59:09 UTC (rev
7307)
+++ code/stage/trunk/libstage/CMakeLists.txt 2009-01-28 09:05:20 UTC (rev
7308)
@@ -28,6 +28,7 @@
options_dlg.cc
options_dlg.hh
powerpack.cc
+ puck.cc
region.cc
resource.cc
stage.cc
Modified: code/stage/trunk/libstage/ancestor.cc
===================================================================
--- code/stage/trunk/libstage/ancestor.cc 2009-01-27 08:59:09 UTC (rev
7307)
+++ code/stage/trunk/libstage/ancestor.cc 2009-01-28 09:05:20 UTC (rev
7308)
@@ -1,15 +1,14 @@
#include "stage.hh"
using namespace Stg;
-Ancestor::Ancestor()
+Ancestor::Ancestor() :
+ children( NULL ),
+ debug( false ),
+ puck_list( NULL ),
+ token( NULL )
{
- token = NULL;
- children = NULL;
-
- for( int i=0; i<MODEL_TYPE_COUNT; i++ )
- child_type_counts[i] = 0;
-
- debug = false;
+ for( int i=0; i<MODEL_TYPE_COUNT; i++ )
+ child_type_counts[i] = 0;
}
Ancestor::~Ancestor()
@@ -20,8 +19,7 @@
delete (Model*)it->data;
g_list_free( children );
- }
-
+ }
}
void Ancestor::AddChild( Model* mod )
@@ -73,3 +71,12 @@
}
}
+
+
+void Ancestor::Load( Worldfile* wf, int section )
+{
+}
+
+void Ancestor::Save( Worldfile* wf, int section )
+{
+}
Modified: code/stage/trunk/libstage/canvas.cc
===================================================================
--- code/stage/trunk/libstage/canvas.cc 2009-01-27 08:59:09 UTC (rev 7307)
+++ code/stage/trunk/libstage/canvas.cc 2009-01-28 09:05:20 UTC (rev 7308)
@@ -804,6 +804,9 @@
if( showBBoxes )
DrawBoundingBoxes();
+
+ LISTMETHOD( world->puck_list, Puck*, Draw );
+
// TODO - finish this properly
//LISTMETHOD( models_sorted, Model*, DrawWaypoints );
Modified: code/stage/trunk/libstage/charger.cc
===================================================================
--- code/stage/trunk/libstage/charger.cc 2009-01-27 08:59:09 UTC (rev
7307)
+++ code/stage/trunk/libstage/charger.cc 2009-01-28 09:05:20 UTC (rev
7308)
@@ -13,7 +13,7 @@
Charger::Charger( World* world )
: world( world ), watts( 1000.0 )
{
- printf( "Charger constructed" );
+ //printf( "Charger constructed" );
}
void Charger::ChargeIfContained( PowerPack* pp, Pose pose )
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-01-27 08:59:09 UTC (rev 7307)
+++ code/stage/trunk/libstage/model.cc 2009-01-28 09:05:20 UTC (rev 7308)
@@ -126,6 +126,37 @@
uint32_t Model::count = 0;
GHashTable* Model::modelsbyid = g_hash_table_new( NULL, NULL );
+
+void Size::Load( Worldfile* wf, int section, const char* keyword )
+{
+ x = wf->ReadTupleLength( section, keyword, 0, x );
+ y = wf->ReadTupleLength( section, keyword, 1, y );
+ z = wf->ReadTupleLength( section, keyword, 2, z );
+}
+
+void Size::Save( Worldfile* wf, int section, const char* keyword )
+{
+ wf->WriteTupleLength( section, keyword, 0, x );
+ wf->WriteTupleLength( section, keyword, 1, y );
+ wf->WriteTupleLength( section, keyword, 2, z );
+}
+
+void Pose::Load( Worldfile* wf, int section, const char* keyword )
+{
+ x = wf->ReadTupleLength( section, keyword, 0, x );
+ y = wf->ReadTupleLength( section, keyword, 1, y );
+ z = wf->ReadTupleLength( section, keyword, 2, z );
+ a = wf->ReadTupleAngle( section, keyword, 3, a );
+}
+
+void Pose::Save( Worldfile* wf, int section, const char* keyword )
+{
+ wf->WriteTupleLength( section, keyword, 0, x );
+ wf->WriteTupleLength( section, keyword, 1, y );
+ wf->WriteTupleLength( section, keyword, 2, z );
+ wf->WriteTupleAngle( section, keyword, 3, a );
+}
+
Visibility::Visibility() :
blob_return( true ),
fiducial_key( 0 ),
@@ -166,7 +197,7 @@
// constructor
Model::Model( World* world,
Model* parent,
- const stg_model_type_t type )
+ const stg_model_type_t type )
: Ancestor(),
access_mutex(NULL),
blinkenlights( g_ptr_array_new() ),
@@ -177,7 +208,7 @@
color( 0xFFFF0000 ), // red
data_fresh(false),
disabled(false),
- custom_visual_list( NULL ),
+ custom_visual_list( NULL ),
flag_list(NULL),
geom(),
has_default_block( true ),
@@ -705,7 +736,7 @@
{
return( world->sim_time >= (last_update + interval) );
}
-
+
void Model::Update( void )
{
// printf( "[%llu] %s update (%d subs)\n",
@@ -1748,3 +1779,15 @@
{
blockgroup.UnMap();
}
+
+void Model::BecomeParentOf( Model* child )
+{
+ if( child->parent )
+ child->parent->RemoveChild( child );
+
+ child->parent = this;
+
+ this->AddChild( child );
+
+ world->dirty = true;
+}
Modified: code/stage/trunk/libstage/model_callbacks.cc
===================================================================
--- code/stage/trunk/libstage/model_callbacks.cc 2009-01-27 08:59:09 UTC
(rev 7307)
+++ code/stage/trunk/libstage/model_callbacks.cc 2009-01-28 09:05:20 UTC
(rev 7308)
@@ -73,8 +73,12 @@
{
//puts( "callback was not installed" );
}
+
+ // return the number of callbacks now in the list. Useful for
+ // detecting when the list is empty.
+ //return g_list_length( cb_list );
- return 0; //ok
+ return 0;
}
Modified: code/stage/trunk/libstage/model_fiducial.cc
===================================================================
--- code/stage/trunk/libstage/model_fiducial.cc 2009-01-27 08:59:09 UTC (rev
7307)
+++ code/stage/trunk/libstage/model_fiducial.cc 2009-01-28 09:05:20 UTC (rev
7308)
@@ -177,46 +177,44 @@
NULL,
false );
- range = ray.range;
+ //range = ray.range;
Model* hitmod = ray.mod;
// printf( "ray hit %s and was seeking LOS to %s\n",
// hitmod ? hitmod->Token() : "null",
// him->Token() );
- //assert( ! (hitmod == this) );
-
// 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
+ stg_fiducial_t fid;
+ fid.mod = him;
+ fid.range = range;
+ fid.bearing = dtheta;
+ fid.geom.x = hisgeom.size.x;
+ fid.geom.y = hisgeom.size.y;
+ fid.geom.a = normalize( hispose.a - mypose.a);
+
+ // store the global pose of the fiducial (mainly for the GUI)
+ memcpy( &fid.pose, &hispose, sizeof(fid.pose));
- // record where we saw him and what he looked like
- stg_fiducial_t fid;
- fid.range = range;
- fid.bearing = dtheta;
- fid.geom.x = hisgeom.size.x;
- fid.geom.y = hisgeom.size.y;
- fid.geom.a = normalize( hispose.a - mypose.a);
-
- // store the global pose of the fiducial (mainly for the GUI)
- memcpy( &fid.pose, &hispose, sizeof(fid.pose));
-
- // if he's within ID range, get his fiducial.return value, else
- // we see value 0
- fid.id = range < max_range_id ? hitmod->vis.fiducial_return : 0;
-
- PRINT_DEBUG2( "adding %s's value %d to my list of fiducials",
- him->Token(), him->vis.fiducial_return );
-
- g_array_append_val( data, fid );
- }
-
+ // if he's within ID range, get his fiducial.return value, else
+ // we see value 0
+ fid.id = range < max_range_id ? hitmod->vis.fiducial_return :
0;
+
+ PRINT_DEBUG2( "adding %s's value %d to my list of fiducials",
+ him->Token(),
him->vis.fiducial_return );
+
+ g_array_append_val( data, fid );
+ }
+
fiducials = (stg_fiducial_t*)data->data;
fiducial_count = data->len;
}
-
///////////////////////////////////////////////////////////////////////////
// Update the beacon data
//
@@ -265,22 +263,22 @@
return;
// draw the FOV
- // GLUquadric* quadric = gluNewQuadric();
+ GLUquadric* quadric = gluNewQuadric();
- // PushColor( 0,0,0,0.2 );
+ PushColor( 0,0,0,0.2 );
- // gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
+ gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
- // gluPartialDisk( quadric,
- // 0,
- // max_range_anon,
- // 20, // slices
- // 1, // loops
- // rtod( M_PI/2.0 + fov/2.0), // start angle
- // rtod(-fov) ); // sweep angle
+ 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();
+ gluDeleteQuadric( quadric );
+ PopColor();
if( data->len == 0 )
return;
Modified: code/stage/trunk/libstage/model_load.cc
===================================================================
--- code/stage/trunk/libstage/model_load.cc 2009-01-27 08:59:09 UTC (rev
7307)
+++ code/stage/trunk/libstage/model_load.cc 2009-01-28 09:05:20 UTC (rev
7308)
@@ -99,49 +99,38 @@
if( wf->PropertyExists( wf_entity, "origin" ) )
{
Geom geom = GetGeom();
- geom.pose.x = wf->ReadTupleLength(wf_entity, "origin", 0, geom.pose.x );
- geom.pose.y = wf->ReadTupleLength(wf_entity, "origin", 1, geom.pose.y );
- geom.pose.z = wf->ReadTupleLength(wf_entity, "origin", 2, geom.pose.z );
- geom.pose.a = wf->ReadTupleAngle(wf_entity, "origin", 3, geom.pose.a );
- this->SetGeom( geom );
+ geom.pose.Load( wf, wf_entity, "origin" );
+ SetGeom( geom );
}
-
+
if( wf->PropertyExists( wf_entity, "size" ) )
{
Geom geom = GetGeom();
- geom.size.x = wf->ReadTupleLength(wf_entity, "size", 0, geom.size.x );
- geom.size.y = wf->ReadTupleLength(wf_entity, "size", 1, geom.size.y );
- geom.size.z = wf->ReadTupleLength(wf_entity, "size", 2, geom.size.z );
- this->SetGeom( geom );
+ geom.size.Load( wf, wf_entity, "size" );
+ SetGeom( geom );
}
-
+
if( wf->PropertyExists( wf_entity, "pose" ))
{
Pose pose = GetPose();
- pose.x = wf->ReadTupleLength(wf_entity, "pose", 0, pose.x );
- pose.y = wf->ReadTupleLength(wf_entity, "pose", 1, pose.y );
- pose.z = wf->ReadTupleLength(wf_entity, "pose", 2, pose.z );
- pose.a = wf->ReadTupleAngle(wf_entity, "pose", 3, pose.a );
- this->SetPose( pose );
+ pose.Load( wf, wf_entity, "pose" );
+ SetPose( pose );
}
-
+
if( wf->PropertyExists( wf_entity, "velocity" ))
{
Velocity vel = GetVelocity();
- vel.x = wf->ReadTupleLength(wf_entity, "velocity", 0, vel.x );
- vel.y = wf->ReadTupleLength(wf_entity, "velocity", 1, vel.y );
- vel.z = wf->ReadTupleLength(wf_entity, "velocity", 2, vel.z );
- vel.a = wf->ReadTupleAngle(wf_entity, "velocity", 3, vel.a );
- this->SetVelocity( vel );
+ vel.Load( wf, wf_entity, "velocity" );
+ SetVelocity( vel );
}
-
+
if( wf->PropertyExists( wf_entity, "color" ))
{
stg_color_t col = 0xFFFF0000; // red;
const char* colorstr = wf->ReadString( wf_entity, "color", NULL );
if( colorstr )
- {
- if( strcmp( colorstr, "random" ) == 0 )
+ {
+ if( strcmp( colorstr, "random" ) == 0 )
{
col = (uint32_t)random();
col |= 0xFF000000; // set the alpha channel to max
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-01-27 08:59:09 UTC (rev 7307)
+++ code/stage/trunk/libstage/stage.hh 2009-01-28 09:05:20 UTC (rev 7308)
@@ -220,7 +220,7 @@
{
public:
stg_meters_t x, y, z;
-
+
Size( stg_meters_t x,
stg_meters_t y,
stg_meters_t z )
@@ -230,6 +230,9 @@
/** default constructor uses default non-zero values */
Size() : x( 0.1 ), y( 0.1 ), z( 0.1 )
{/*empty*/}
+
+ void Load( Worldfile* wf, int section, const char* keyword );
+ void Save( Worldfile* wf, int section, const char* keyword );
};
/** Specify a 3 axis position, in x, y and heading. */
@@ -270,6 +273,9 @@
printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
prefix, x,y,z,a );
}
+
+ void Load( Worldfile* wf, int section, const char* keyword );
+ void Save( Worldfile* wf, int section, const char* keyword );
};
@@ -778,8 +784,26 @@
/** Define a callback function type that can be attached to a
record within a model and called whenever the record is set.*/
typedef int (*stg_model_callback_t)( Model* mod, void* user );
+
+ class Puck
+ {
+ private:
+ void BuildDisplayList();
-
+ public:
+ stg_color_t color;
+ int displaylist;
+ stg_meters_t height;
+ Pose pose;
+ stg_meters_t radius;
+
+ Puck();
+ void Load( Worldfile* wf, int section );
+ void Save( Worldfile* wf, int section );
+
+ void Draw();
+ };
+
// ANCESTOR CLASS
/** Base class for Model and World */
class Ancestor
@@ -788,9 +812,13 @@
protected:
GList* children;
+ bool debug;
+ GList* puck_list;
char* token;
- bool debug;
+ void Load( Worldfile* wf, int section );
+ void Save( Worldfile* wf, int section );
+
public:
/** recursively call func( model, arg ) for each descendant */
@@ -810,7 +838,7 @@
{ return token; }
void SetToken( const char* str )
- { token = strdup( str ); } // teeny memory leak
+ { token = strdup( str ); } // little memory leak
};
/** raytrace sample
@@ -906,6 +934,7 @@
void LoadModel( Worldfile* wf, int entity, GHashTable* entitytable );
void LoadBlock( Worldfile* wf, int entity, GHashTable* entitytable );
void LoadBlockGroup( Worldfile* wf, int entity, GHashTable* entitytable );
+ void LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable );
void LoadCharger( Worldfile* wf, int entity );
SuperRegion* AddSuperRegion( const stg_point_int_t& coord );
@@ -1591,8 +1620,6 @@
global coordinate frame is the parent is NULL. */
Pose pose;
- /** Optional attached PowerPack, defaults to NULL */
- PowerPack* power_pack;
/** GData datalist can contain arbitrary named data items. Can be used
by derived model types to store properties, and for user code
@@ -1620,6 +1647,9 @@
Visibility vis;
+ /** Optional attached PowerPack, defaults to NULL */
+ PowerPack* power_pack;
+
void Lock()
{
if( access_mutex == NULL )
@@ -1795,7 +1825,8 @@
/** remove user supplied visualization to a model - supply the same ptr
passed to AddCustomVisualizer */
void RemoveCustomVisualizer( CustomVisualizer* custom_visual );
-
+ void BecomeParentOf( Model* child );
+
void Load( Worldfile* wf, int wf_entity )
{
/** Set the worldfile and worldfile entity ID - must be called
@@ -2006,12 +2037,18 @@
void RemoveSaveCallback( stg_model_callback_t cb )
{ RemoveCallback( &hooks.save, cb ); }
-
+
void AddUpdateCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &hooks.update, cb, user ); }
+ {
+ AddCallback( &hooks.update, cb, user );
+ Subscribe(); // if attaching a callback here, assume we want updates to
happen
+ }
void RemoveUpdateCallback( stg_model_callback_t cb )
- { RemoveCallback( &hooks.update, cb ); }
+ {
+ RemoveCallback( &hooks.update, cb );
+ Unsubscribe();
+ }
/** named-property interface
*/
@@ -2331,8 +2368,8 @@
stg_radians_t bearing; ///< bearing to the target
Pose geom; ///< size and relative angle of the target
Pose pose; ///< Absolute accurate position of the target in world
coordinates (it's cheating to use this in robot controllers!)
- int id; ///< the identifier of the target, or -1 if none can be detected.
-
+ Model* mod; ///< Pointer to the model (real fiducial detectors can't do
this!)
+ int id; ///< the fiducial identifier of the target (i.e. its fiducial_return
value), or -1 if none can be detected.
} stg_fiducial_t;
/// %ModelFiducial class
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2009-01-27 08:59:09 UTC (rev 7307)
+++ code/stage/trunk/libstage/world.cc 2009-01-28 09:05:20 UTC (rev 7308)
@@ -190,6 +190,19 @@
mod->LoadBlock( wf, entity );
}
+void World::LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable )
+{
+// // lookup the group in which this was defined
+// Ancestor* anc = (Ancestor*)g_hash_table_lookup( entitytable,
+//
(gpointer)wf->GetEntityParent( entity ) );
+
+
+ Puck* puck = new Puck();
+ puck->Load( wf, entity );
+ puck_list = g_list_prepend( puck_list, puck );
+}
+
+
void World::LoadModel( Worldfile* wf, int entity, GHashTable* entitytable )
{
int parent_entity = wf->GetEntityParent( entity );
@@ -321,10 +334,10 @@
{
/* do nothing here */
}
- //else if( strcmp( typestr, "blockgroup" ) == 0 )
- //LoadBlockGroup( wf, entity, entitytable );
else if( strcmp( typestr, "block" ) == 0 )
LoadBlock( wf, entity, entitytable );
+ else if( strcmp( typestr, "puck" ) == 0 )
+ LoadPuck( wf, entity, entitytable );
else if( strcmp( typestr, "charger" ) == 0 )
LoadCharger( wf, entity );
else
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2009-01-27 08:59:09 UTC (rev 7307)
+++ code/stage/trunk/worlds/fasr.world 2009-01-28 09:05:20 UTC (rev 7308)
@@ -22,9 +22,9 @@
(
size [ 600.000 599.000 ]
- center [ -0.424 -0.218 ]
+ center [ 0.051 -0.204 ]
rotate [ 0 0 ]
- scale 30.883
+ scale 33.822
pcam_loc [ 0 -4.000 2.000 ]
pcam_angle [ 70.000 0 ]
@@ -73,15 +73,20 @@
ctrl "sink"
)
+#puck( pose [ 0 0 0 0 ] )
+#puck( pose [ 1 0 0 0 ] )
+#puck( pose [ 2 0 0 0 ] )
+#puck( pose [ 3 0 0 0 ] )
+
define autorob pioneer2dx
(
sicklaser( samples 32 range_max 5 laser_return 2 watts 30 )
- fiducial( )
ctrl "fasr"
joules 1000000
fiducial_return 1
)
+#autorob( pose [4.116 6.107 0 -147.323] fiducial( range_max 3 ) )
autorob( pose [4.116 6.107 0 -147.323] )
autorob( pose [6.471 5.304 0 14.941] )
autorob( pose [5.937 4.858 0 -147.503] )
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
This SF.net email is sponsored by:
SourcForge Community
SourceForge wants to tell your story.
http://p.sf.net/sfu/sf-spreadtheword
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
