Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

expanded test prog, reduced unecessary displaylist building, moved co…

…nstants into namespace, fixed some bugs
  • Loading branch information...
commit 7dc54a694e4fb52566ae70e14df8bc6be0843315 1 parent 915edb0
rtv authored
View
6 CMakeLists.txt
@@ -10,6 +10,11 @@ SET( APIVERSION ${V_MAJOR}.${V_MINOR} )
SET( MIN_PLAYER_MAJOR 2 )
SET( MIN_PLAYER_MINOR 1 )
+#SET(CMAKE_BUILD_TYPE debug)
+#SET(CMAKE_CXX_FLAGS_DISTRIBUTION "-O3")
+#SET(CMAKE_C_FLAGS_DISTRIBUTION "-O3")
+
+
cmake_minimum_required( VERSION 2.4 FATAL_ERROR )
IF (CMAKE_MAJOR_VERSION EQUAL 2 AND NOT CMAKE_MINOR_VERSION LESS 6)
@@ -60,6 +65,7 @@ ENDIF (FLTKCONFIG)
IF (APPLE)
MESSAGE(STATUS "Setting Apple specific flags")
+ # work around the OS X 10.5 X11/OpenGL library bug
SET(APPLE_LIBRARIES "-Wl,-dylib_file,/System/Library/Frameworks/OpenGL.framework/Versions/A/Libraries/libGL.dylib:/System/Library/Frameworks/OpenGL.framework/Versions/A/Libraries/libGL.dylib")
ENDIF (APPLE)
View
4 libstage/CMakeLists.txt
@@ -1,5 +1,7 @@
add_library( stage SHARED
+ stage.cc
+ typetable.cc
stage.hh # get headers into IDE projects
file_manager.hh
file_manager.cc
@@ -21,8 +23,6 @@ add_library( stage SHARED
model_props.cc
model_ranger.cc
resource.cc
- stage.cc
- typetable.cc
world.cc
worldfile.cc
worldgui.cc
View
47 libstage/ancestor.cc
@@ -4,7 +4,10 @@ StgAncestor::StgAncestor()
{
token = NULL;
children = NULL;
- child_types = g_hash_table_new( g_str_hash, g_str_equal );
+
+ for( int i=0; i<MODEL_TYPE_COUNT; i++ )
+ child_type_counts[i] = 0;
+
debug = false;
}
@@ -18,45 +21,31 @@ StgAncestor::~StgAncestor()
g_list_free( children );
}
- g_hash_table_destroy( child_types );
}
-unsigned int StgAncestor::GetNumChildrenOfType( const char* typestr )
+void StgAncestor::AddChild( StgModel* mod )
{
- unsigned int *c = (unsigned int*)g_hash_table_lookup( child_types, typestr);
+
+ // poke a name into the child
+ char* buf = new char[TOKEN_MAX];
+ snprintf( buf, TOKEN_MAX, "%s.%s:%d",
+ token, typetable[mod->type].token, child_type_counts[mod->type] );
+
+ //printf( "%s generated a name for my child %s\n", token, buf );
- if( c )
- return *c;
- else
- return 0;
-}
-
-void StgAncestor::IncrementNumChildrenOfType( const char* typestr )
-{
- unsigned int* c = (unsigned int*)g_hash_table_lookup( child_types, typestr);
+ mod->SetToken( buf );
- if( c == NULL )
- {
- c = new unsigned int;
- g_hash_table_insert( child_types, (gpointer)typestr, (gpointer)c);
- *c = 1;
- }
- else
- (*c)++;
-}
+ children = g_list_append( children, mod );
-void StgAncestor::AddChild( StgModel* mod )
-{
- // increment the count of models of this type
- IncrementNumChildrenOfType( mod->typestr );
+ child_type_counts[mod->type]++;
- // store as a child
- children = g_list_append( children, mod );
+ delete buf;
}
void StgAncestor::RemoveChild( StgModel* mod )
{
- puts( "TODO: StgWorld::RemoveChild()" );
+ child_type_counts[mod->type]--;
+ children = g_list_remove( children, mod );
}
stg_pose_t StgAncestor::GetGlobalPose()
View
19 libstage/canvas.cc
@@ -74,6 +74,9 @@ void StgCanvas::InvertView( uint32_t invertflags )
StgModel* StgCanvas::Select( int x, int y )
{
+ // TODO XX
+ return NULL;
+
// render all models in a unique color
make_current(); // make sure the GL context is current
glClearColor ( 0,0,0,1 );
@@ -88,7 +91,8 @@ StgModel* StgCanvas::Select( int x, int y )
if( mod->GuiMask() & (STG_MOVE_TRANS | STG_MOVE_ROT ))
{
- uint32_t col = (mod->Id() | 0xFF000000);
+ // TODO XX
+ uint32_t col = (uint32_t)mod; //(mod->Id() | 0xFF000000);
glColor4ubv( (GLubyte*)&col );
mod->DrawPicker();
}
@@ -104,9 +108,9 @@ StgModel* StgCanvas::Select( int x, int y )
GL_RGBA,GL_UNSIGNED_BYTE,(void*)&id );
// strip off the alpha channel byte to retrieve the model id
- id &= 0x00FFFFFF;
+ //id &= 0x00FFFFFF;
- StgModel* mod = world->GetModel( id );
+ StgModel* mod = (StgModel*)id;//world->GetModel( id );
//printf("%p %s %d\n", mod, mod ? mod->Token() : "", id );
@@ -447,9 +451,12 @@ void StgCanvas::renderFrame( bool robot_camera )
if( mod->displaylist == 0 )
mod->displaylist = glGenLists(1);
- if( mod->body_dirty )
- mod->BuildDisplayList( showflags ); // ready to be rendered
-
+ if( mod->rebuild_displaylist )
+ {
+ //printf( "Model %s is dirty\n", mod->Token() );
+ mod->BuildDisplayList( showflags ); // ready to be rendered
+ }
+
// move into this model's local coordinate frame
glPushMatrix();
gl_pose_shift( &mod->pose );
View
266 libstage/model.cc
@@ -103,92 +103,78 @@ TODO PLAN: single array of all polygon vertices - model just keeps an index
#define _GNU_SOURCE
- //#define DEBUG
+ //#define DEBUG 0
#include "stage_internal.hh"
#include "texture_manager.hh"
#include <limits.h>
// basic model
- const bool STG_DEFAULT_MOD_BLOBRETURN = true;
- const bool STG_DEFAULT_MOD_BOUNDARY = false;
- const stg_color_t STG_DEFAULT_MOD_COLOR = (0xFFFF0000); // solid red
- const stg_joules_t STG_DEFAULT_MOD_ENERGY_CAPACITY = 1000.0;
- const bool STG_DEFAULT_MOD_ENERGY_CHARGEENABLE = true;
- const stg_watts_t STG_DEFAULT_MOD_ENERGY_GIVERATE = 0.0;
- const stg_meters_t STG_DEFAULT_MOD_ENERGY_PROBERANGE = 0.0;
- const stg_watts_t STG_DEFAULT_MOD_ENERGY_TRICKLERATE = 0.1;
- const stg_meters_t STG_DEFAULT_MOD_GEOM_SIZEX = 0.10;
- const stg_meters_t STG_DEFAULT_MOD_GEOM_SIZEY = 0.10;
- const stg_meters_t STG_DEFAULT_MOD_GEOM_SIZEZ = 0.10;
- const bool STG_DEFAULT_MOD_GRID = false;
- const bool STG_DEFAULT_MOD_GRIPPERRETURN = false;
- const stg_laser_return_t STG_DEFAULT_MOD_LASERRETURN = LaserVisible;
- const stg_meters_t STG_DEFAULT_MOD_MAP_RESOLUTION = 0.1;
- const stg_movemask_t STG_DEFAULT_MOD_MASK = (STG_MOVE_TRANS | STG_MOVE_ROT);
- const stg_kg_t STG_DEFAULT_MOD_MASS = 10.0;
- const bool STG_DEFAULT_MOD_NOSE = false;
- const bool STG_DEFAULT_MOD_OBSTACLERETURN = true;
- const bool STG_DEFAULT_MOD_OUTLINE = true;
- const bool STG_DEFAULT_MOD_RANGERRETURN = true;
-
-
- // speech bubble colors
- const stg_color_t STG_BUBBLE_FILL = 0xFFC8C8FF; // light blue/grey
- const stg_color_t STG_BUBBLE_BORDER = 0xFF000000; // black
- const stg_color_t STG_BUBBLE_TEXT = 0xFF000000; // black
-
- // constructor
+const bool StgModel::DEFAULT_BLOBRETURN = true;
+const bool StgModel::DEFAULT_BOUNDARY = false;
+const stg_color_t StgModel::DEFAULT_COLOR = (0xFFFF0000); // solid red
+const stg_joules_t StgModel::DEFAULT_ENERGY_CAPACITY = 1000.0;
+const bool StgModel::DEFAULT_ENERGY_CHARGEENABLE = true;
+const stg_watts_t StgModel::DEFAULT_ENERGY_GIVERATE = 0.0;
+const stg_meters_t StgModel::DEFAULT_ENERGY_PROBERANGE = 0.0;
+const stg_watts_t StgModel::DEFAULT_ENERGY_TRICKLERATE = 0.1;
+const stg_meters_t StgModel::DEFAULT_GEOM_SIZEX = 0.10;
+const stg_meters_t StgModel::DEFAULT_GEOM_SIZEY = 0.10;
+const stg_meters_t StgModel::DEFAULT_GEOM_SIZEZ = 0.10;
+const bool StgModel::DEFAULT_GRID = false;
+const bool StgModel::DEFAULT_GRIPPERRETURN = false;
+const stg_laser_return_t StgModel::DEFAULT_LASERRETURN = LaserVisible;
+const stg_meters_t StgModel::DEFAULT_MAP_RESOLUTION = 0.1;
+const stg_movemask_t StgModel::DEFAULT_MASK = (STG_MOVE_TRANS | STG_MOVE_ROT);
+const stg_kg_t StgModel::DEFAULT_MASS = 10.0;
+const bool StgModel::DEFAULT_NOSE = false;
+const bool StgModel::DEFAULT_OBSTACLERETURN = true;
+const bool StgModel::DEFAULT_OUTLINE = true;
+const bool StgModel::DEFAULT_RANGERRETURN = true;
+
+
+// speech bubble colors
+const stg_color_t StgModel::BUBBLE_FILL = 0xFFC8C8FF; // light blue/grey
+const stg_color_t StgModel::BUBBLE_BORDER = 0xFF000000; // black
+const stg_color_t StgModel::BUBBLE_TEXT = 0xFF000000; // black
+
+// constructor
StgModel::StgModel( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr )
+ StgModel* parent,
+ const stg_model_type_t type )
: StgAncestor()
-{
- PRINT_DEBUG4( "Constructing model world: %s parent: %s type: %s id: %d",
- world->Token(),
- parent ? parent->Token() : "(null)",
- typestr,
- id );
-
- this->id = id;
- this->typestr = typestr;
- this->parent = parent;
- this->world = world;
-
- this->debug = false;
-
- // generate a name. This might be overwritten if the "name" property
- // is set in the worldfile when StgModel::Load() is called
-
- StgAncestor* anc = parent ? (StgAncestor*)parent : (StgAncestor*)world;
-
- unsigned int cnt = anc->GetNumChildrenOfType( typestr );
- char* buf = new char[TOKEN_MAX];
-
- snprintf( buf, TOKEN_MAX, "%s.%s:%d",
- anc->Token(), typestr, cnt );
-
- this->token = strdup( buf );
- delete buf;
-
- PRINT_DEBUG2( "model has token \"%s\" and typestr \"%s\"",
- this->token, this->typestr );
-
- anc->AddChild( this );
- world->AddModel( this );
-
- bzero( &pose, sizeof(pose));
- if( parent )
- pose.z = parent->geom.size.z;
- bzero( &global_pose, sizeof(global_pose));
-
+{
+ assert( world );
+
+ PRINT_DEBUG3( "Constructing model world: %s parent: %s type: %d ",
+ world->Token(),
+ parent ? parent->Token() : "(null)",
+ type );
+
+ this->parent = parent;
+ this->world = world;
+ this->debug = false;
+ this->type = type;
+
+ // Adding this model to its ancestor also gives this model a
+ // sensible default name
+
+ StgAncestor* anc = parent ? (StgAncestor*)parent : (StgAncestor*)world;
+
+ anc->AddChild( this );
+ world->AddModel( this );
+
+ bzero( &pose, sizeof(pose));
+ if( parent )
+ pose.z = parent->geom.size.z;
+ bzero( &global_pose, sizeof(global_pose));
+
this->trail = g_array_new( false, false, sizeof(stg_trail_item_t) );
this->data_fresh = false;
this->disabled = false;
this->d_list = NULL;
this->blocks = NULL;
- this->body_dirty = true;
+ this->rebuild_displaylist = true;
this->data_dirty = true;
this->gpose_dirty = true;
this->say_string = NULL;
@@ -197,23 +183,23 @@ StgModel::StgModel( StgWorld* world,
this->displaylist = 0;
- this->geom.size.x = STG_DEFAULT_MOD_GEOM_SIZEX;
- this->geom.size.y = STG_DEFAULT_MOD_GEOM_SIZEX;
- this->geom.size.z = STG_DEFAULT_MOD_GEOM_SIZEX;
+ this->geom.size.x = StgModel::DEFAULT_GEOM_SIZEX;
+ this->geom.size.y = StgModel::DEFAULT_GEOM_SIZEX;
+ this->geom.size.z = StgModel::DEFAULT_GEOM_SIZEX;
memset( &this->geom.pose, 0, sizeof(this->geom.pose));
- this->obstacle_return = STG_DEFAULT_MOD_OBSTACLERETURN;
- this->ranger_return = STG_DEFAULT_MOD_RANGERRETURN;
- this->blob_return = STG_DEFAULT_MOD_BLOBRETURN;
- this->laser_return = STG_DEFAULT_MOD_LASERRETURN;
- this->gripper_return = STG_DEFAULT_MOD_GRIPPERRETURN;
- this->boundary = STG_DEFAULT_MOD_BOUNDARY;
- this->color = STG_DEFAULT_MOD_COLOR;
- this->map_resolution = STG_DEFAULT_MOD_MAP_RESOLUTION; // meters
- this->gui_nose = STG_DEFAULT_MOD_NOSE;
- this->gui_grid = STG_DEFAULT_MOD_GRID;
- this->gui_outline = STG_DEFAULT_MOD_OUTLINE;
- this->gui_mask = this->parent ? 0 : STG_DEFAULT_MOD_MASK;
+ this->obstacle_return = StgModel::DEFAULT_OBSTACLERETURN;
+ this->ranger_return = StgModel::DEFAULT_RANGERRETURN;
+ this->blob_return = StgModel::DEFAULT_BLOBRETURN;
+ this->laser_return = StgModel::DEFAULT_LASERRETURN;
+ this->gripper_return = StgModel::DEFAULT_GRIPPERRETURN;
+ this->boundary = StgModel::DEFAULT_BOUNDARY;
+ this->color = StgModel::DEFAULT_COLOR;
+ this->map_resolution = StgModel::DEFAULT_MAP_RESOLUTION; // meters
+ this->gui_nose = StgModel::DEFAULT_NOSE;
+ this->gui_grid = StgModel::DEFAULT_GRID;
+ this->gui_outline = StgModel::DEFAULT_OUTLINE;
+ this->gui_mask = this->parent ? 0 : StgModel::DEFAULT_MASK;
this->fiducial_return = 0;
this->fiducial_key = 0;
@@ -232,22 +218,34 @@ StgModel::StgModel( StgWorld* world,
this->interval = 1e4; // 10msec
this->initfunc = NULL;
- //this->updatefunc = NULL;
+
+ this->startup_hook = NULL;
+ this->shutdown_hook = NULL;
+ this->load_hook = NULL;
+ this->save_hook = NULL;
+
+ this->wf = NULL;
+ this->wf_entity = 0;
// now we can add the basic square shape
this->AddBlockRect( -0.5,-0.5,1,1 );
- PRINT_DEBUG2( "finished model %s (%d).",
- this->token,
- this->id );
+ PRINT_DEBUG2( "finished model %s @ %p",
+ this->token, this );
}
StgModel::~StgModel( void )
{
- // remove from parent, if there is one
- if( parent )
- parent->children = g_list_remove( parent->children, this );
+ UnMap(); // remove from the raytrace bitmap
+ // children are removed in ancestor class
+
+ // remove from parent, if there is one
+ if( parent )
+ parent->children = g_list_remove( parent->children, this );
+ else
+ world->children = g_list_remove( world->children, this );
+
if( callbacks ) g_hash_table_destroy( callbacks );
world->RemoveModel( this );
@@ -260,29 +258,6 @@ void StgModel::Init()
{
if( initfunc )
Subscribe();
-
- // anything else to do here?
-
- // // try some things out;
- // stg_blinkenlight_t* b = new stg_blinkenlight_t;
- // b->pose.x = 0;
- // b->pose.y = 0;
- // b->pose.z = 0.4;
- // b->pose.a = 0;
- // b->enabled = true;
- // b->color = stg_color_pack( 1,1,0,0 );
- // b->size = 0.1;
-
- // //AddBlinkenlight( b );
-
- // stg_blinkenlight_t* c = new stg_blinkenlight_t;
- // c->pose.x = 0.1;
- // c->pose.y = 0;
- // c->pose.z = 0.4;
- // c->pose.a = 0;
- // c->enabled = false;
- // c->color = stg_color_pack( 1,1,0,0 );
- // c->size = 0.1;
}
void StgModel::AddFlag( StgFlag* flag )
@@ -689,10 +664,13 @@ void StgModel::Print( char* prefix )
else
printf( "Model ");
- printf( "%d %s:%s\n",
- id,
+ printf( "%s:%s\n",
+ // id,
world->Token(),
token );
+
+ for( GList* it=children; it; it=it->next )
+ ((StgModel*)it->data)->Print( prefix );
}
const char* StgModel::PrintWithPose()
@@ -718,7 +696,7 @@ void StgModel::Startup( void )
world->StartUpdatingModel( this );
- CallCallbacks( &startup );
+ CallCallbacks( &startup_hook );
}
void StgModel::Shutdown( void )
@@ -727,7 +705,7 @@ void StgModel::Shutdown( void )
world->StopUpdatingModel( this );
- CallCallbacks( &shutdown );
+ CallCallbacks( &shutdown_hook );
}
void StgModel::UpdateIfDue( void )
@@ -742,12 +720,7 @@ void StgModel::Update( void )
//printf( "[%lu] %s update (%d subs)\n",
// this->world->sim_time_ms, this->token, this->subs );
- //puts( "UPDATE" );
- // if( updatefunc )
- // updatefunc( this );
-
- CallCallbacks( &update );
-
+ CallCallbacks( &update_hook );
last_update = world->sim_time;
}
@@ -890,6 +863,8 @@ void StgModel::DrawTrailArrows()
void StgModel::DrawBlocks( )
{
+ //printf( "model %s drawing blocks\n", token );
+
LISTMETHOD( this->blocks, StgBlock*, Draw );
// recursively draw the tree below this model
@@ -900,9 +875,7 @@ void StgModel::DrawBlocks( )
glPushMatrix();
gl_pose_shift( &child->pose );
gl_pose_shift( &child->geom.pose );
-
child->DrawBlocks();
-
glPopMatrix();
}
@@ -988,7 +961,7 @@ void StgModel::Draw( uint32_t flags, Stg::StgCanvas* canvas )
// draw inside of bubble
- PushColor( STG_BUBBLE_FILL );
+ PushColor( BUBBLE_FILL );
glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT );
glPolygonMode( GL_FRONT, GL_FILL );
glEnable( GL_POLYGON_OFFSET_FILL );
@@ -997,7 +970,7 @@ void StgModel::Draw( uint32_t flags, Stg::StgCanvas* canvas )
glDisable( GL_POLYGON_OFFSET_FILL );
PopColor();
// draw outline of bubble
- PushColor( STG_BUBBLE_BORDER );
+ PushColor( BUBBLE_BORDER );
glLineWidth( 1 );
glEnable( GL_LINE_SMOOTH );
glPolygonMode( GL_FRONT, GL_LINE );
@@ -1007,7 +980,7 @@ void StgModel::Draw( uint32_t flags, Stg::StgCanvas* canvas )
// draw text
- PushColor( STG_BUBBLE_TEXT );
+ PushColor( BUBBLE_TEXT );
glTranslatef( 0, 0, 0.1 ); // draw text forwards of bubble
gl_draw_string( m, m, 0.0, this->say_string );
PopColor();
@@ -1137,12 +1110,11 @@ void StgModel::DrawPicker( void )
void StgModel::BuildDisplayList( int flags )
{
- glNewList( displaylist, GL_COMPILE );
- //printf("Model %s blocks %d\n", token, g_list_length( blocks ) );
-
+ glNewList( displaylist, GL_COMPILE );
DrawBlocks();
-
glEndList();
+
+ rebuild_displaylist = false; // we just did it
}
void StgModel::DataVisualize( void )
@@ -1191,8 +1163,10 @@ void StgModel::SetVelocity( stg_velocity_t vel )
void StgModel::NeedRedraw( void )
{
- this->body_dirty = true;
- //this->world->dirty = true;
+ this->rebuild_displaylist = true;
+
+ if( parent )
+ parent->NeedRedraw();
}
void StgModel::GPoseDirtyTree( void )
@@ -1217,7 +1191,7 @@ void StgModel::SetPose( stg_pose_t pose )
this->pose = pose;
this->pose.a = normalize(this->pose.a);
- this->NeedRedraw();
+ //this->NeedRedraw();
this->GPoseDirtyTree(); // global pose may have changed
MapWithChildren();
@@ -1690,19 +1664,19 @@ int StgModel::TreeToPtrArray( GPtrArray* array )
return added;
}
-StgModel* StgModel::GetUnsubscribedModelOfType( char* modelstr )
+StgModel* StgModel::GetUnsubscribedModelOfType( stg_model_type_t type )
{
- // printf( "searching for %s in model %s with string %s\n", modelstr, token, typestr );
-
- if( subs == 0 && (strcmp( typestr, modelstr ) == 0) )
- return this;
+ // printf( "searching for %s in model %s with string %s\n", modelstr, token, typestr );
+
+ if( (this->type == type) && (this->subs == 0) )
+ return this;
// this model is no use. try children recursively
for( GList* it = children; it; it=it->next )
{
StgModel* child = (StgModel*)it->data;
- StgModel* found = child->GetUnsubscribedModelOfType( modelstr );
+ StgModel* found = child->GetUnsubscribedModelOfType( type );
if( found )
return found;
}
View
29 libstage/model_blinkenlight.cc
@@ -48,19 +48,10 @@ enabled 1
//#define DEBUG 1
#include "stage_internal.hh"
-
-//static gboolean blink( bool* enabled )
-//{
-// *enabled = ! *enabled;
-// puts( "blink" );
-// return true;
-//}
-
-StgModelBlinkenlight::StgModelBlinkenlight( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr )
-: StgModel( world, parent, id, typestr )
+
+StgModelBlinkenlight::StgModelBlinkenlight( StgWorld* world,
+ StgModel* parent )
+ : StgModel( world, parent, MODEL_TYPE_BLINKENLIGHT )
{
PRINT_DEBUG2( "Constructing StgModelBlinkenlight %d (%s)\n",
id, typestr );
@@ -91,13 +82,11 @@ StgModelBlinkenlight::~StgModelBlinkenlight()
void StgModelBlinkenlight::Load( void )
{
- StgModel::Load();
-
- Worldfile* wf = world->GetWorldFile();
-
- this->dutycycle = wf->ReadFloat( id, "dutycycle", this->dutycycle );
- this->period = wf->ReadInt( id, "period", this->period );
- this->enabled = wf->ReadInt( id, "dutycycle", this->enabled );
+ StgModel::Load();
+
+ this->dutycycle = wf->ReadFloat( wf_entity, "dutycycle", this->dutycycle );
+ this->period = wf->ReadInt( wf_entity, "period", this->period );
+ this->enabled = wf->ReadInt( wf_entity, "dutycycle", this->enabled );
}
View
22 libstage/model_blobfinder.cc
@@ -80,10 +80,8 @@ resolution
StgModelBlobfinder::StgModelBlobfinder( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr )
-: StgModel( world, parent, id, typestr )
+ StgModel* parent )
+ : StgModel( world, parent, MODEL_TYPE_BLOBFINDER )
{
PRINT_DEBUG2( "Constructing StgModelBlobfinder %d (%s)\n",
id, typestr );
@@ -150,22 +148,22 @@ void StgModelBlobfinder::Load( void )
Worldfile* wf = world->GetWorldFile();
- scan_width = (int)wf->ReadTupleFloat( id, "image", 0, scan_width );
- scan_height= (int)wf->ReadTupleFloat( id, "image", 1, scan_height );
- range = wf->ReadFloat( id, "range", range );
- fov = wf->ReadAngle( id, "fov", fov );
- pan = wf->ReadAngle( id, "pan", pan );
+ scan_width = (int)wf->ReadTupleFloat( wf_entity, "image", 0, scan_width );
+ scan_height= (int)wf->ReadTupleFloat( wf_entity, "image", 1, scan_height );
+ range = wf->ReadFloat( wf_entity, "range", range );
+ fov = wf->ReadAngle( wf_entity, "fov", fov );
+ pan = wf->ReadAngle( wf_entity, "pan", pan );
- if( wf->PropertyExists( id, "colors" ) )
+ if( wf->PropertyExists( wf_entity, "colors" ) )
{
RemoveAllColors(); // empty the color list to start from scratch
- unsigned int count = wf->ReadFloat( id, "colors_count", 0 );
+ unsigned int count = wf->ReadFloat( wf_entity, "colors_count", 0 );
for( unsigned int c=0; c<count; c++ )
{
const char* colorstr =
- wf->ReadTupleString( id, "colors", c, NULL );
+ wf->ReadTupleString( wf_entity, "colors", c, NULL );
if( ! colorstr )
break;
View
16 libstage/model_camera.cc
@@ -15,10 +15,8 @@
StgModelCamera::StgModelCamera( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr )
-: StgModel( world, parent, id, typestr ),
+ StgModel* parent )
+ : StgModel( world, parent, MODEL_TYPE_CAMERA ),
_frame_data( NULL ), _frame_data_width( 0 ), _frame_data_height( 0 ), _width( 0 )
{
PRINT_DEBUG2( "Constructing StgModelCamera %d (%s)\n",
@@ -54,18 +52,16 @@ StgModelCamera::~StgModelCamera()
void StgModelCamera::Load( void )
{
StgModel::Load();
- Worldfile* wf = world->GetWorldFile();
- _camera.setFov( wf->ReadLength( id, "fov", _camera.fov() ) );
- _camera.setYaw( wf->ReadLength( id, "yaw", _camera.yaw() ) );
- _camera.setPitch( wf->ReadLength( id, "pitch", _camera.pitch() ) );
+ _camera.setFov( wf->ReadLength( wf_entity, "fov", _camera.fov() ) );
+ _camera.setYaw( wf->ReadLength( wf_entity, "yaw", _camera.yaw() ) );
+ _camera.setPitch( wf->ReadLength( wf_entity, "pitch", _camera.pitch() ) );
- _width = wf->ReadLength( id, "width", _width );
+ _width = wf->ReadLength( wf_entity, "width", _width );
//TODO move to constructor
_frame_data_width = _width;
_frame_data_height = 100;
-
}
View
16 libstage/model_fiducial.cc
@@ -62,10 +62,8 @@ size [0 0]
*/
StgModelFiducial::StgModelFiducial( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr )
-: StgModel( world, parent, id, typestr )
+ StgModel* parent )
+ : StgModel( world, parent, MODEL_TYPE_FIDUCIAL )
{
PRINT_DEBUG2( "Constructing StgModelFiducial %d (%s)\n",
id, typestr );
@@ -235,13 +233,11 @@ void StgModelFiducial::Load( void )
StgModel::Load();
- Worldfile* wf = world->GetWorldFile();
-
// load fiducial-specific properties
- min_range = wf->ReadLength( id, "range_min", min_range );
- max_range_anon = wf->ReadLength( id, "range_max", max_range_anon );
- max_range_id = wf->ReadLength( id, "range_max_id", max_range_id );
- fov = wf->ReadAngle( id, "fov", fov );
+ min_range = wf->ReadLength( wf_entity, "range_min", min_range );
+ max_range_anon = wf->ReadLength( wf_entity, "range_max", max_range_anon );
+ max_range_id = wf->ReadLength( wf_entity, "range_max_id", max_range_id );
+ fov = wf->ReadAngle( wf_entity, "fov", fov );
}
View
19 libstage/model_laser.cc
@@ -76,10 +76,8 @@ watts 17.5 # approximately correct for SICK LMS200
*/
StgModelLaser::StgModelLaser( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr )
-: StgModel( world, parent, id, typestr )
+ StgModel* parent )
+: StgModel( world, parent, MODEL_TYPE_LASER )
{
PRINT_DEBUG2( "Constructing StgModelLaser %d (%s)\n",
id, typestr );
@@ -121,14 +119,11 @@ StgModelLaser::~StgModelLaser( void )
void StgModelLaser::Load( void )
{
StgModel::Load();
-
- Worldfile* wf = world->GetWorldFile();
-
- sample_count = wf->ReadInt( id, "samples", sample_count );
- range_min = wf->ReadLength( id, "range_min", range_min);
- range_max = wf->ReadLength( id, "range_max", range_max );
- fov = wf->ReadAngle( id, "fov", fov );
- resolution = wf->ReadInt( id, "laser_sample_skip", resolution );
+ sample_count = wf->ReadInt( wf_entity, "samples", sample_count );
+ range_min = wf->ReadLength( wf_entity, "range_min", range_min);
+ range_max = wf->ReadLength( wf_entity, "range_max", range_max );
+ fov = wf->ReadAngle( wf_entity, "fov", fov );
+ resolution = wf->ReadInt( wf_entity, "laser_sample_skip", resolution );
if( resolution < 1 )
{
View
212 libstage/model_load.cc
@@ -12,22 +12,23 @@ void FooInit( StgModel* );
void FooUpdate( StgModel* );
-void StgModel::Load( void )
+void StgModel::Load()
{
- PRINT_DEBUG1( "Model \"%s\" loading...", token );
+ assert( wf );
+ assert( wf_entity );
- Worldfile* wf = world->wf;
+ PRINT_DEBUG1( "Model \"%s\" loading...", token );
- if( wf->PropertyExists( this->id, "debug" ) )
+ if( wf->PropertyExists( wf_entity, "debug" ) )
{
PRINT_WARN2( "debug property specified for model %d %s\n",
- this->id, this->token );
- this->debug = wf->ReadInt( this->id, "debug", this->debug );
+ wf_entity, this->token );
+ this->debug = wf->ReadInt( wf_entity, "debug", this->debug );
}
- if( wf->PropertyExists( this->id, "name" ) )
+ if( wf->PropertyExists( wf_entity, "name" ) )
{
- char *name = (char*)wf->ReadString(this->id, "name", NULL );
+ char *name = (char*)wf->ReadString(wf_entity, "name", NULL );
if( name )
{
//printf( "changed %s to %s\n", this->token, token );
@@ -43,93 +44,93 @@ void StgModel::Load( void )
//PRINT_WARN1( "%s::Load", token );
- if( wf->PropertyExists( this->id, "origin" ) )
+ if( wf->PropertyExists( wf_entity, "origin" ) )
{
stg_geom_t geom = GetGeom();
- geom.pose.x = wf->ReadTupleLength(this->id, "origin", 0, geom.pose.x );
- geom.pose.y = wf->ReadTupleLength(this->id, "origin", 1, geom.pose.y );
- geom.pose.a = wf->ReadTupleAngle(this->id, "origin", 2, geom.pose.a );
+ 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.a = wf->ReadTupleAngle(wf_entity, "origin", 2, geom.pose.a );
this->SetGeom( geom );
}
- if( wf->PropertyExists( this->id, "origin4" ) )
+ if( wf->PropertyExists( wf_entity, "origin4" ) )
{
stg_geom_t geom = GetGeom();
- geom.pose.x = wf->ReadTupleLength(this->id, "origin4", 0, geom.pose.x );
- geom.pose.y = wf->ReadTupleLength(this->id, "origin4", 1, geom.pose.y );
- geom.pose.z = wf->ReadTupleLength(this->id, "origin4", 2, geom.pose.z );
- geom.pose.a = wf->ReadTupleAngle(this->id, "origin4", 3, geom.pose.a );
+ geom.pose.x = wf->ReadTupleLength(wf_entity, "origin4", 0, geom.pose.x );
+ geom.pose.y = wf->ReadTupleLength(wf_entity, "origin4", 1, geom.pose.y );
+ geom.pose.z = wf->ReadTupleLength(wf_entity, "origin4", 2, geom.pose.z );
+ geom.pose.a = wf->ReadTupleAngle(wf_entity, "origin4", 3, geom.pose.a );
this->SetGeom( geom );
}
- if( wf->PropertyExists( this->id, "size" ) )
+ if( wf->PropertyExists( wf_entity, "size" ) )
{
stg_geom_t geom = GetGeom();
- geom.size.x = wf->ReadTupleLength(this->id, "size", 0, geom.size.x );
- geom.size.y = wf->ReadTupleLength(this->id, "size", 1, geom.size.y );
+ geom.size.x = wf->ReadTupleLength(wf_entity, "size", 0, geom.size.x );
+ geom.size.y = wf->ReadTupleLength(wf_entity, "size", 1, geom.size.y );
this->SetGeom( geom );
}
- if( wf->PropertyExists( this->id, "size3" ) )
+ if( wf->PropertyExists( wf_entity, "size3" ) )
{
stg_geom_t geom = GetGeom();
- geom.size.x = wf->ReadTupleLength(this->id, "size3", 0, geom.size.x );
- geom.size.y = wf->ReadTupleLength(this->id, "size3", 1, geom.size.y );
- geom.size.z = wf->ReadTupleLength(this->id, "size3", 2, geom.size.z );
+ geom.size.x = wf->ReadTupleLength(wf_entity, "size3", 0, geom.size.x );
+ geom.size.y = wf->ReadTupleLength(wf_entity, "size3", 1, geom.size.y );
+ geom.size.z = wf->ReadTupleLength(wf_entity, "size3", 2, geom.size.z );
this->SetGeom( geom );
}
- if( wf->PropertyExists( this->id, "pose" ))
+ if( wf->PropertyExists( wf_entity, "pose" ))
{
stg_pose_t pose = GetPose();
- pose.x = wf->ReadTupleLength(this->id, "pose", 0, pose.x );
- pose.y = wf->ReadTupleLength(this->id, "pose", 1, pose.y );
- pose.a = wf->ReadTupleAngle(this->id, "pose", 2, pose.a );
+ pose.x = wf->ReadTupleLength(wf_entity, "pose", 0, pose.x );
+ pose.y = wf->ReadTupleLength(wf_entity, "pose", 1, pose.y );
+ pose.a = wf->ReadTupleAngle(wf_entity, "pose", 2, pose.a );
this->SetPose( pose );
}
- if( wf->PropertyExists( this->id, "pose4" ))
+ if( wf->PropertyExists( wf_entity, "pose4" ))
{
stg_pose_t pose = GetPose();
- pose.x = wf->ReadTupleLength(this->id, "pose4", 0, pose.x );
- pose.y = wf->ReadTupleLength(this->id, "pose4", 1, pose.y );
- pose.z = wf->ReadTupleLength(this->id, "pose4", 2, pose.z );
- pose.a = wf->ReadTupleAngle( this->id, "pose4", 3, pose.a );
+ pose.x = wf->ReadTupleLength(wf_entity, "pose4", 0, pose.x );
+ pose.y = wf->ReadTupleLength(wf_entity, "pose4", 1, pose.y );
+ pose.z = wf->ReadTupleLength(wf_entity, "pose4", 2, pose.z );
+ pose.a = wf->ReadTupleAngle( wf_entity, "pose4", 3, pose.a );
this->SetPose( pose );
}
- if( wf->PropertyExists( this->id, "velocity" ))
+ if( wf->PropertyExists( wf_entity, "velocity" ))
{
stg_velocity_t vel = GetVelocity();
- vel.x = wf->ReadTupleLength(this->id, "velocity", 0, vel.x );
- vel.y = wf->ReadTupleLength(this->id, "velocity", 1, vel.y );
- vel.a = wf->ReadTupleAngle(this->id, "velocity", 3, vel.a );
+ vel.x = wf->ReadTupleLength(wf_entity, "velocity", 0, vel.x );
+ vel.y = wf->ReadTupleLength(wf_entity, "velocity", 1, vel.y );
+ vel.a = wf->ReadTupleAngle(wf_entity, "velocity", 3, vel.a );
this->SetVelocity( vel );
if( vel.x || vel.y || vel.z || vel.a )
world->StartUpdatingModel( this );
}
- if( wf->PropertyExists( this->id, "velocity4" ))
+ if( wf->PropertyExists( wf_entity, "velocity4" ))
{
stg_velocity_t vel = GetVelocity();
- vel.x = wf->ReadTupleLength(this->id, "velocity4", 0, vel.x );
- vel.y = wf->ReadTupleLength(this->id, "velocity4", 1, vel.y );
- vel.z = wf->ReadTupleLength(this->id, "velocity4", 2, vel.z );
- vel.a = wf->ReadTupleAngle(this->id, "velocity4", 3, vel.a );
+ vel.x = wf->ReadTupleLength(wf_entity, "velocity4", 0, vel.x );
+ vel.y = wf->ReadTupleLength(wf_entity, "velocity4", 1, vel.y );
+ vel.z = wf->ReadTupleLength(wf_entity, "velocity4", 2, vel.z );
+ vel.a = wf->ReadTupleAngle(wf_entity, "velocity4", 3, vel.a );
this->SetVelocity( vel );
}
- if( wf->PropertyExists( this->id, "boundary" ))
+ if( wf->PropertyExists( wf_entity, "boundary" ))
{
- this->SetBoundary( wf->ReadInt(this->id, "boundary", this->boundary ));
+ this->SetBoundary( wf->ReadInt(wf_entity, "boundary", this->boundary ));
}
- if( wf->PropertyExists( this->id, "color" ))
+ if( wf->PropertyExists( wf_entity, "color" ))
{
stg_color_t col = 0xFFFF0000; // red;
- const char* colorstr = wf->ReadString( this->id, "color", NULL );
+ const char* colorstr = wf->ReadString( wf_entity, "color", NULL );
if( colorstr )
{
if( strcmp( colorstr, "random" ) == 0 )
@@ -145,19 +146,19 @@ void StgModel::Load( void )
}
- if( wf->PropertyExists( this->id, "color_rgba" ))
+ if( wf->PropertyExists( wf_entity, "color_rgba" ))
{
- double red = wf->ReadTupleFloat( this->id, "color_rgba", 0, 0);
- double green = wf->ReadTupleFloat( this->id, "color_rgba", 1, 0);
- double blue = wf->ReadTupleFloat( this->id, "color_rgba", 2, 0);
- double alpha = wf->ReadTupleFloat( this->id, "color_rgba", 3, 0);
+ double red = wf->ReadTupleFloat( wf_entity, "color_rgba", 0, 0);
+ double green = wf->ReadTupleFloat( wf_entity, "color_rgba", 1, 0);
+ double blue = wf->ReadTupleFloat( wf_entity, "color_rgba", 2, 0);
+ double alpha = wf->ReadTupleFloat( wf_entity, "color_rgba", 3, 0);
this->SetColor( stg_color_pack( red, green, blue, alpha ));
}
- if( wf->PropertyExists( this->id, "bitmap" ) )
+ if( wf->PropertyExists( wf_entity, "bitmap" ) )
{
- const char* bitmapfile = wf->ReadString( this->id, "bitmap", NULL );
+ const char* bitmapfile = wf->ReadString( wf_entity, "bitmap", NULL );
assert( bitmapfile );
char full[_POSIX_PATH_MAX];
@@ -211,7 +212,7 @@ void StgModel::Load( void )
StgBlock::ScaleList( this->blocks, &this->geom.size );
this->Map();
- this->body_dirty = true;
+ this->NeedRedraw();
g_free( rects );
}
@@ -220,9 +221,9 @@ void StgModel::Load( void )
// token, g_list_length( blocks ));
}
- if( wf->PropertyExists( this->id, "blocks" ) )
+ if( wf->PropertyExists( wf_entity, "blocks" ) )
{
- int blockcount = wf->ReadInt( this->id, "blocks", -1 );
+ int blockcount = wf->ReadInt( wf_entity, "blocks", -1 );
this->UnMap();
this->ClearBlocks();
@@ -233,7 +234,7 @@ void StgModel::Load( void )
for( int l=0; l<blockcount; l++ )
{
snprintf(key, sizeof(key), "block[%d].points", l);
- int pointcount = wf->ReadInt(this->id,key,0);
+ int pointcount = wf->ReadInt(wf_entity,key,0);
//printf( "expecting %d points in block %d\n",
//pointcount, l );
@@ -244,8 +245,8 @@ void StgModel::Load( void )
for( p=0; p<pointcount; p++ ) {
snprintf(key, sizeof(key), "block[%d].point[%d]", l, p );
- pts[p].x = wf->ReadTupleLength(this->id, key, 0, 0);
- pts[p].y = wf->ReadTupleLength(this->id, key, 1, 0);
+ pts[p].x = wf->ReadTupleLength(wf_entity, key, 0, 0);
+ pts[p].y = wf->ReadTupleLength(wf_entity, key, 1, 0);
//printf( "key %s x: %.2f y: %.2f\n",
// key, pt.x, pt.y );
@@ -255,10 +256,10 @@ void StgModel::Load( void )
snprintf(key, sizeof(key), "block[%d].z", l);
stg_meters_t zmin =
- wf->ReadTupleLength(this->id, key, 0, 0.0 );
+ wf->ReadTupleLength(wf_entity, key, 0, 0.0 );
stg_meters_t zmax =
- wf->ReadTupleLength(this->id, key, 1, 1.0 );
+ wf->ReadTupleLength(wf_entity, key, 1, 1.0 );
// block color
stg_color_t blockcol = this->color;
@@ -266,7 +267,7 @@ void StgModel::Load( void )
snprintf(key, sizeof(key), "block[%d].color", l);
- const char* colorstr = wf->ReadString( this->id, key, NULL );
+ const char* colorstr = wf->ReadString( wf_entity, key, NULL );
if( colorstr )
{
blockcol = stg_lookup_color( colorstr );
@@ -295,48 +296,48 @@ void StgModel::Load( void )
this->Map();
}
- if( wf->PropertyExists( this->id, "mass" ))
- this->SetMass( wf->ReadFloat(this->id, "mass", this->mass ));
+ if( wf->PropertyExists( wf_entity, "mass" ))
+ this->SetMass( wf->ReadFloat(wf_entity, "mass", this->mass ));
- if( wf->PropertyExists( this->id, "fiducial_return" ))
- this->SetFiducialReturn( wf->ReadInt( this->id, "fiducial_return", this->fiducial_return ));
+ if( wf->PropertyExists( wf_entity, "fiducial_return" ))
+ this->SetFiducialReturn( wf->ReadInt( wf_entity, "fiducial_return", this->fiducial_return ));
- if( wf->PropertyExists( this->id, "fiducial_key" ))
- this->SetFiducialKey( wf->ReadInt( this->id, "fiducial_key", this->fiducial_key ));
+ if( wf->PropertyExists( wf_entity, "fiducial_key" ))
+ this->SetFiducialKey( wf->ReadInt( wf_entity, "fiducial_key", this->fiducial_key ));
- if( wf->PropertyExists( this->id, "obstacle_return" ))
- this->SetObstacleReturn( wf->ReadInt( this->id, "obstacle_return", this->obstacle_return ));
+ if( wf->PropertyExists( wf_entity, "obstacle_return" ))
+ this->SetObstacleReturn( wf->ReadInt( wf_entity, "obstacle_return", this->obstacle_return ));
- if( wf->PropertyExists( this->id, "ranger_return" ))
- this->SetRangerReturn( wf->ReadInt( this->id, "ranger_return", this->ranger_return ));
+ if( wf->PropertyExists( wf_entity, "ranger_return" ))
+ this->SetRangerReturn( wf->ReadInt( wf_entity, "ranger_return", this->ranger_return ));
- if( wf->PropertyExists( this->id, "blob_return" ))
- this->SetBlobReturn( wf->ReadInt( this->id, "blob_return", this->blob_return ));
+ if( wf->PropertyExists( wf_entity, "blob_return" ))
+ this->SetBlobReturn( wf->ReadInt( wf_entity, "blob_return", this->blob_return ));
- if( wf->PropertyExists( this->id, "laser_return" ))
- this->SetLaserReturn( (stg_laser_return_t)wf->ReadInt(this->id, "laser_return", this->laser_return ));
+ if( wf->PropertyExists( wf_entity, "laser_return" ))
+ this->SetLaserReturn( (stg_laser_return_t)wf->ReadInt(wf_entity, "laser_return", this->laser_return ));
- if( wf->PropertyExists( this->id, "gripper_return" ))
- this->SetGripperReturn( wf->ReadInt( this->id, "gripper_return", this->gripper_return ));
+ if( wf->PropertyExists( wf_entity, "gripper_return" ))
+ this->SetGripperReturn( wf->ReadInt( wf_entity, "gripper_return", this->gripper_return ));
- if( wf->PropertyExists( this->id, "gui_nose" ))
- this->SetGuiNose( wf->ReadInt(this->id, "gui_nose", this->gui_nose ));
+ if( wf->PropertyExists( wf_entity, "gui_nose" ))
+ this->SetGuiNose( wf->ReadInt(wf_entity, "gui_nose", this->gui_nose ));
- if( wf->PropertyExists( this->id, "gui_grid" ))
- this->SetGuiGrid( wf->ReadInt(this->id, "gui_grid", this->gui_grid ));
+ if( wf->PropertyExists( wf_entity, "gui_grid" ))
+ this->SetGuiGrid( wf->ReadInt(wf_entity, "gui_grid", this->gui_grid ));
- if( wf->PropertyExists( this->id, "gui_outline" ))
- this->SetGuiOutline( wf->ReadInt(this->id, "gui_outline", this->gui_outline ));
+ if( wf->PropertyExists( wf_entity, "gui_outline" ))
+ this->SetGuiOutline( wf->ReadInt(wf_entity, "gui_outline", this->gui_outline ));
- if( wf->PropertyExists( this->id, "gui_movemask" ))
- this->SetGuiMask( wf->ReadInt(this->id, "gui_movemask", this->gui_mask ));
+ if( wf->PropertyExists( wf_entity, "gui_movemask" ))
+ this->SetGuiMask( wf->ReadInt(wf_entity, "gui_movemask", this->gui_mask ));
- if( wf->PropertyExists( this->id, "map_resolution" ))
- this->SetMapResolution( wf->ReadFloat(this->id, "map_resolution", this->map_resolution ));
+ if( wf->PropertyExists( wf_entity, "map_resolution" ))
+ this->SetMapResolution( wf->ReadFloat(wf_entity, "map_resolution", this->map_resolution ));
- if( wf->PropertyExists( this->id, "ctrl" ))
+ if( wf->PropertyExists( wf_entity, "ctrl" ))
{
- char* lib = (char*)wf->ReadString(this->id, "ctrl", NULL );
+ char* lib = (char*)wf->ReadString(wf_entity, "ctrl", NULL );
if( !lib )
puts( "Error - NULL library name" );
@@ -344,16 +345,16 @@ void StgModel::Load( void )
LoadControllerModule( lib );
}
- if( wf->PropertyExists( this->id, "say" ))
- this->Say( wf->ReadString(this->id, "say", NULL ));
+ if( wf->PropertyExists( wf_entity, "say" ))
+ this->Say( wf->ReadString(wf_entity, "say", NULL ));
// call any type-specific load callbacks
- this->CallCallbacks( &this->load );
+ this->CallCallbacks( &this->load_hook );
// MUST BE THE LAST THING LOADED
- if( wf->PropertyExists( this->id, "alwayson" ))
+ if( wf->PropertyExists( wf_entity, "alwayson" ))
{
- if( wf->ReadInt( this->id, "alwayson", 0) > 0 )
+ if( wf->ReadInt( wf_entity, "alwayson", 0) > 0 )
Startup();
}
@@ -366,10 +367,11 @@ void StgModel::Load( void )
void StgModel::Save( void )
{
+ assert( wf );
+ assert( wf_entity );
+
PRINT_DEBUG1( "Model \"%s\" saving...", token );
- Worldfile* wf = world->wf;
-
PRINT_DEBUG4( "saving model %s pose %.2f %.2f %.2f",
this->token,
this->pose.x,
@@ -377,17 +379,17 @@ void StgModel::Save( void )
this->pose.a );
// right now we only save poses
- wf->WriteTupleLength( this->id, "pose", 0, this->pose.x);
- wf->WriteTupleLength( this->id, "pose", 1, this->pose.y);
- wf->WriteTupleAngle( this->id, "pose", 2, this->pose.a);
+ wf->WriteTupleLength( wf_entity, "pose", 0, this->pose.x);
+ wf->WriteTupleLength( wf_entity, "pose", 1, this->pose.y);
+ wf->WriteTupleAngle( wf_entity, "pose", 2, this->pose.a);
- wf->WriteTupleLength( this->id, "pose3", 0, this->pose.x);
- wf->WriteTupleLength( this->id, "pose3", 1, this->pose.y);
- wf->WriteTupleLength( this->id, "pose3", 2, this->pose.z);
- wf->WriteTupleAngle( this->id, "pose3", 3, this->pose.a);
+ wf->WriteTupleLength( wf_entity, "pose3", 0, this->pose.x);
+ wf->WriteTupleLength( wf_entity, "pose3", 1, this->pose.y);
+ wf->WriteTupleLength( wf_entity, "pose3", 2, this->pose.z);
+ wf->WriteTupleAngle( wf_entity, "pose3", 3, this->pose.a);
// call any type-specific save callbacks
- this->CallCallbacks( &this->save );
+ this->CallCallbacks( &this->save_hook );
PRINT_DEBUG1( "Model \"%s\" saving complete.", token );
}
View
34 libstage/model_position.cc
@@ -83,10 +83,8 @@ const stg_position_drive_mode_t POSITION_DRIVE_DEFAULT = STG_POSITION_DRIVE_DIF
StgModelPosition::StgModelPosition( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr )
-: StgModel( world, parent, id, typestr )
+ StgModel* parent )
+ : StgModel( world, parent, MODEL_TYPE_POSITION )
{
PRINT_DEBUG2( "Constructing StgModelPosition %d (%s)\n",
id, typestr );
@@ -137,13 +135,11 @@ void StgModelPosition::Load( void )
char* keyword = NULL;
- Worldfile* wf = world->GetWorldFile();
-
// load steering mode
- if( wf->PropertyExists( this->id, "drive" ) )
+ if( wf->PropertyExists( wf_entity, "drive" ) )
{
const char* mode_str =
- wf->ReadString( this->id, "drive", NULL );
+ wf->ReadString( wf_entity, "drive", NULL );
if( mode_str )
{
@@ -161,7 +157,7 @@ void StgModelPosition::Load( void )
}
// load odometry if specified
- if( wf->PropertyExists( this->id, "odom" ) )
+ if( wf->PropertyExists( wf_entity, "odom" ) )
{
PRINT_WARN1( "the odom property is specified for model \"%s\","
" but this property is no longer available."
@@ -176,11 +172,11 @@ void StgModelPosition::Load( void )
est_origin = this->GetGlobalPose();
keyword = "localization_origin";
- if( wf->PropertyExists( this->id, keyword ) )
+ if( wf->PropertyExists( wf_entity, keyword ) )
{
- est_origin.x = wf->ReadTupleLength( id, keyword, 0, est_origin.x );
- est_origin.y = wf->ReadTupleLength( id, keyword, 1, est_origin.y );
- est_origin.a = wf->ReadTupleAngle( id,keyword, 2, est_origin.a );
+ est_origin.x = wf->ReadTupleLength( wf_entity, keyword, 0, est_origin.x );
+ est_origin.y = wf->ReadTupleLength( wf_entity, keyword, 1, est_origin.y );
+ est_origin.a = wf->ReadTupleAngle( wf_entity,keyword, 2, est_origin.a );
// compute our localization pose based on the origin and true pose
stg_pose_t gpose = this->GetGlobalPose();
@@ -201,21 +197,21 @@ void StgModelPosition::Load( void )
// odometry model parameters
keyword = "odom_error";
- if( wf->PropertyExists( id, keyword ) )
+ if( wf->PropertyExists( wf_entity, keyword ) )
{
integration_error.x =
- wf->ReadTupleLength( id, keyword, 0, integration_error.x );
+ wf->ReadTupleLength( wf_entity, keyword, 0, integration_error.x );
integration_error.y =
- wf->ReadTupleLength( id, keyword, 1, integration_error.y );
+ wf->ReadTupleLength( wf_entity, keyword, 1, integration_error.y );
integration_error.a
- = wf->ReadTupleAngle( id, keyword, 2, integration_error.a );
+ = wf->ReadTupleAngle( wf_entity, keyword, 2, integration_error.a );
}
// choose a localization model
- if( wf->PropertyExists( this->id, "localization" ) )
+ if( wf->PropertyExists( wf_entity, "localization" ) )
{
const char* loc_str =
- wf->ReadString( id, "localization", NULL );
+ wf->ReadString( wf_entity, "localization", NULL );
if( loc_str )
{
View
38 libstage/model_ranger.cc
@@ -94,10 +94,8 @@ static const char RANGER_GEOM_COLOR[] = "orange";
StgModelRanger::StgModelRanger( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr )
-: StgModel( world, parent, id, typestr )
+ StgModel* parent )
+ : StgModel( world, parent, MODEL_TYPE_RANGER )
{
PRINT_DEBUG2( "Constructing StgModelRanger %d (%s)\n",
id, typestr );
@@ -172,14 +170,12 @@ void StgModelRanger::Load( void )
{
StgModel::Load();
- Worldfile* wf = world->GetWorldFile();
-
- if( wf->PropertyExists(id, "scount" ) )
+ if( wf->PropertyExists( wf_entity, "scount" ) )
{
PRINT_DEBUG( "Loading ranger array" );
// Load the geometry of a ranger array
- sensor_count = wf->ReadInt( id, "scount", 0);
+ sensor_count = wf->ReadInt( wf_entity, "scount", 0);
assert( sensor_count > 0 );
char key[256];
@@ -188,14 +184,14 @@ void StgModelRanger::Load( void )
sensors = new stg_ranger_sensor_t[sensor_count];
stg_size_t common_size;
- common_size.x = wf->ReadTupleLength( id, "ssize", 0, DEFAULT_RANGER_SIZEX );
- common_size.y = wf->ReadTupleLength( id, "ssize", 1, DEFAULT_RANGER_SIZEY );
+ common_size.x = wf->ReadTupleLength( wf_entity, "ssize", 0, DEFAULT_RANGER_SIZEX );
+ common_size.y = wf->ReadTupleLength( wf_entity, "ssize", 1, DEFAULT_RANGER_SIZEY );
- double common_min = wf->ReadTupleLength( id, "sview", 0, DEFAULT_RANGER_RANGEMIN );
- double common_max = wf->ReadTupleLength( id, "sview", 1, DEFAULT_RANGER_RANGEMAX );
- double common_fov = wf->ReadTupleAngle( id, "sview", 2, M_PI / (double)sensor_count );
+ double common_min = wf->ReadTupleLength( wf_entity, "sview", 0, DEFAULT_RANGER_RANGEMIN );
+ double common_max = wf->ReadTupleLength( wf_entity, "sview", 1, DEFAULT_RANGER_RANGEMAX );
+ double common_fov = wf->ReadTupleAngle( wf_entity, "sview", 2, M_PI / (double)sensor_count );
- int common_ray_count = wf->ReadInt( id, "sraycount", sensors[0].ray_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++)
@@ -213,16 +209,16 @@ void StgModelRanger::Load( void )
for( unsigned int i = 0; i < sensor_count; i++)
{
snprintf(key, sizeof(key), "spose[%d]", i);
- sensors[i].pose.x = wf->ReadTupleLength( id, key, 0, sensors[i].pose.x );
- sensors[i].pose.y = wf->ReadTupleLength( id, key, 1, sensors[i].pose.y );
+ 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( id, key, 2, sensors[i].pose.a );
+ 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( id, key, 0, sensors[i].pose.x );
- sensors[i].pose.y = wf->ReadTupleLength( id, key, 1, sensors[i].pose.y );
- sensors[i].pose.z = wf->ReadTupleLength( id, key, 2, sensors[i].pose.z );
- sensors[i].pose.a = wf->ReadTupleAngle( id, key, 3, sensors[i].pose.a );
+ 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); */
View
14 libstage/stage.cc
@@ -20,8 +20,8 @@
//#include "config.h" // results of autoconf's system configuration tests
static bool init_called = false;
-static GHashTable* typetable = NULL;
+stg_typetable_entry_t Stg::typetable[MODEL_TYPE_COUNT];
void Stg::Init( int* argc, char** argv[] )
{
@@ -32,8 +32,9 @@ void Stg::Init( int* argc, char** argv[] )
if(!setlocale(LC_ALL,"POSIX"))
PRINT_WARN("Failed to setlocale(); config file may not be parse correctly\n" );
+
+ RegisterModels();
- typetable = stg_create_typetable();
init_called = true;
// ask FLTK to load support for various image formats
@@ -45,10 +46,15 @@ bool Stg::InitDone()
return init_called;
}
-GHashTable* Stg::Typetable()
+
+void Stg::RegisterModel( stg_model_type_t type,
+ const char* name,
+ stg_creator_t creator )
{
- return typetable;
+ Stg::typetable[ type ].token = name;
+ Stg::typetable[ type ].creator = creator;
}
+
void Stg::stg_print_err( const char* err )
View
397 libstage/stage.hh
@@ -90,8 +90,21 @@ namespace Stg
/** returns true iff Stg::Init() has been called. */
bool InitDone();
- /** Returns a hash table of model creator functions indexed by worldfile model string */
- GHashTable* Typetable();
+
+ /** Create unique identifying numbers for each type of model, and a
+ count of the number of types. */
+ typedef enum {
+ MODEL_TYPE_PLAIN = 0,
+ MODEL_TYPE_LASER,
+ MODEL_TYPE_FIDUCIAL,
+ MODEL_TYPE_RANGER,
+ MODEL_TYPE_POSITION,
+ MODEL_TYPE_BLOBFINDER,
+ MODEL_TYPE_BLINKENLIGHT,
+ MODEL_TYPE_CAMERA,
+ MODEL_TYPE_COUNT // must be the last entry, to count the number of
+ // types
+ } stg_model_type_t;
// foreward declare
class StgCanvas;
@@ -134,7 +147,7 @@ namespace Stg
"\n" \
"The text of the license may also be available online at\n" \
"http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";
-
+
/** The maximum length of a Stage model identifier string
*/
const uint32_t TOKEN_MAX = 64;
@@ -536,17 +549,24 @@ namespace Stg
class StgWorld;
class StgModel;
- GHashTable* stg_create_typetable();
-
- /** A model creator function. Each model type must define a function of this type. */
- typedef StgModel* (*stg_creator_t)( StgWorld*, StgModel*, stg_id_t, char* );
+ /** A model creator function. Each model type must define a function of this type. */
+ typedef StgModel* (*stg_creator_t)( StgWorld*, StgModel* );
+
typedef struct
{
const char* token;
- stg_creator_t creator_fn;
+ stg_creator_t creator;
} stg_typetable_entry_t;
+
+ /** a global (to the namespace) table mapping names to model types */
+ extern stg_typetable_entry_t typetable[MODEL_TYPE_COUNT];
+
+ void RegisterModel( stg_model_type_t type,
+ const char* name,
+ stg_creator_t creator );
+ void RegisterModels();
void gl_draw_grid( stg_bounds3d_t vol );
void gl_pose_shift( stg_pose_t* pose );
@@ -845,16 +865,22 @@ namespace Stg
protected:
GList* children;
- GHashTable* child_types;
- char* token;
+ //GHashTable* child_types;
+
+
+ char* token;
bool debug;
public:
+
+ /** array contains the number of each type of child model */
+ unsigned int child_type_counts[MODEL_TYPE_COUNT];
+
StgAncestor();
virtual ~StgAncestor();
- unsigned int GetNumChildrenOfType( const char* typestr );
- void IncrementNumChildrenOfType( const char* typestr );
+ // unsigned int GetNumChildrenOfType( const char* typestr );
+ // void IncrementNumChildrenOfType( const char* typestr );
virtual void AddChild( StgModel* mod );
virtual void RemoveChild( StgModel* mod );
@@ -863,6 +889,11 @@ namespace Stg
const char* Token()
{ return this->token; }
+ void SetToken( const char* str )
+ {
+ this->token = strdup( str );
+ }
+
// PURE VIRTUAL - descendents must implement
virtual void PushColor( stg_color_t col ) = 0;
virtual void PushColor( double r, double g, double b, double a ) = 0;
@@ -972,7 +1003,7 @@ private:
bool destroy;
- stg_id_t id;
+ //stg_id_t id;
GHashTable* models_by_id; ///< the models that make up the world, indexed by id
GHashTable* models_by_name; ///< the models that make up the world, indexed by name
@@ -1050,13 +1081,16 @@ protected:
GList* GetRayList(){ return ray_list; };
void ClearRays();
-public:
- StgWorld();
- StgWorld( const char* token,
- stg_msec_t interval_sim,
- stg_msec_t interval_real,
- double ppm );
+public:
+ static const int DEFAULT_PPM = 50; // default resolution in pixels per meter
+ static const stg_msec_t DEFAULT_INTERVAL_REAL = 100; ///< real time between updates
+ static const stg_msec_t DEFAULT_INTERVAL_SIM = 100; ///< duration of sim timestep
+
+ StgWorld( const char* token = "MyWorld",
+ stg_msec_t interval_sim = DEFAULT_INTERVAL_SIM,
+ stg_msec_t interval_real = DEFAULT_INTERVAL_REAL,
+ double ppm = DEFAULT_PPM );
virtual ~StgWorld();
@@ -1137,9 +1171,38 @@ class StgModel : public StgAncestor
friend class StgWorldGui;
friend class StgCanvas;
- protected:
+protected:
+
+ // basic model
+ static const bool DEFAULT_BLOBRETURN;
+ static const bool DEFAULT_BOUNDARY;
+ static const stg_color_t DEFAULT_COLOR;
+ static const stg_joules_t DEFAULT_ENERGY_CAPACITY;
+ static const bool DEFAULT_ENERGY_CHARGEENABLE;
+ static const stg_watts_t DEFAULT_ENERGY_GIVERATE;
+ static const stg_meters_t DEFAULT_ENERGY_PROBERANGE;
+ static const stg_watts_t DEFAULT_ENERGY_TRICKLERATE;
+ static const stg_meters_t DEFAULT_GEOM_SIZEX;
+ static const stg_meters_t DEFAULT_GEOM_SIZEY;
+ static const stg_meters_t DEFAULT_GEOM_SIZEZ;
+ static const bool DEFAULT_GRID;
+ static const bool DEFAULT_GRIPPERRETURN;
+ static const stg_laser_return_t DEFAULT_LASERRETURN;
+ static const stg_meters_t DEFAULT_MAP_RESOLUTION;
+ static const stg_movemask_t DEFAULT_MASK;
+ static const stg_kg_t DEFAULT_MASS;
+ static const bool DEFAULT_NOSE;
+ static const bool DEFAULT_OBSTACLERETURN;
+ static const bool DEFAULT_OUTLINE;
+ static const bool DEFAULT_RANGERRETURN;
+
+ // speech bubble colors
+ static const stg_color_t BUBBLE_FILL;
+ static const stg_color_t BUBBLE_BORDER;
+ static const stg_color_t BUBBLE_TEXT;
- const char* typestr;
+
+ //const char* typestr;
stg_pose_t pose;
stg_velocity_t velocity;
stg_watts_t watts; //< power consumed by this model
@@ -1193,7 +1256,7 @@ class StgModel : public StgAncestor
// stg_trail_item_t* history;
- bool body_dirty; //< iff true, regenerate block display list before redraw
+ bool rebuild_displaylist; //< iff true, regenerate block display list before redraw
bool data_dirty; //< iff true, regenerate data display list before redraw
stg_pose_t global_pose;
@@ -1206,7 +1269,7 @@ class StgModel : public StgAncestor
///allows polling the model instead of adding a
///data callback.
- stg_id_t id; // globally unique ID used as hash table key and
+ //stg_id_t id; // globally unique ID used as hash table key and
// worldfile section identifier
StgWorld* world; // pointer to the world in which this model exists
@@ -1308,18 +1371,20 @@ protected:
void DrawTrailArrows();
void DrawGrid();
void UpdateIfDue();
-
- /* hooks for attaching special callback functions (not used as
- variables) */
- char startup, shutdown, load, save, update;
-
- ctrlinit_t* initfunc;
- //ctrlupdate_t* updatefunc;
-
- GList* flag_list;
-
- GPtrArray* blinkenlights;
- void DrawBlinkenlights();
+
+ /* hooks for attaching special callback functions (not used as
+ variables) */
+ char startup_hook, shutdown_hook, load_hook, save_hook, update_hook;
+
+ ctrlinit_t* initfunc;
+
+ GList* flag_list;
+
+ Worldfile* wf;
+ int wf_entity;
+
+ GPtrArray* blinkenlights;
+ void DrawBlinkenlights();
/** OpenGL display list identifier */
int displaylist;
@@ -1327,20 +1392,35 @@ protected:
/** Compile the display list for this model */
void BuildDisplayList( int flags );
+ stg_model_type_t type;
+
public:
+ static const char* typestr;
+
// constructor
- StgModel( StgWorld* world, StgModel* parent, stg_id_t id, char* typestr );
+ StgModel( StgWorld* world, StgModel* parent, stg_model_type_t type = MODEL_TYPE_PLAIN );
// destructor
virtual ~StgModel();
void Say( const char* str );
- stg_id_t Id(){ return id; };
-
- /** configure a model by reading from the current world file */
- virtual void Load();
+ /** Set the worldfile and worldfile entity ID - must be called
+ before Load() */
+ void Load( Worldfile* wf, int wf_entity )
+ {
+ SetWorldfile( wf, wf_entity );
+ Load(); // call virtual load
+ }
+
+ /** Set the worldfile and worldfile entity ID */
+ void SetWorldfile( Worldfile* wf, int wf_entity )
+ { this->wf = wf; this->wf_entity = wf_entity; }
+
+ /** configure a model by reading from the current world file */
+ virtual void Load();
+
/** save the state of the model to the current world file */
virtual void Save();
@@ -1398,7 +1478,7 @@ protected:
/** remove all blocks from this model, freeing their memory */
void ClearBlocks();
- const char* TypeStr(){ return this->typestr; }
+ //const char* TypeStr(){ return this->typestr; }
StgModel* Parent(){ return this->parent; }
StgModel* GetModel( const char* name );
bool Stall(){ return this->stall; }
@@ -1514,34 +1594,34 @@ protected:
some implementation detail */
void AddStartupCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &startup, cb, user ); };
+ { AddCallback( &startup_hook, cb, user ); };
void RemoveStartupCallback( stg_model_callback_t cb )
- { RemoveCallback( &startup, cb ); };
+ { RemoveCallback( &startup_hook, cb ); };
void AddShutdownCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &shutdown, cb, user ); };
+ { AddCallback( &shutdown_hook, cb, user ); };
void RemoveShutdownCallback( stg_model_callback_t cb )
- { RemoveCallback( &shutdown, cb ); }
+ { RemoveCallback( &shutdown_hook, cb ); }
void AddLoadCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &load, cb, user ); }
+ { AddCallback( &load_hook, cb, user ); }
void RemoveLoadCallback( stg_model_callback_t cb )
- { RemoveCallback( &load, cb ); }
+ { RemoveCallback( &load_hook, cb ); }
void AddSaveCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &save, cb, user ); }
+ { AddCallback( &save_hook, cb, user ); }
void RemoveSaveCallback( stg_model_callback_t cb )
- { RemoveCallback( &save, cb ); }
+ { RemoveCallback( &save_hook, cb ); }
void AddUpdateCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &update, cb, user ); }
+ { AddCallback( &update_hook, cb, user ); }
void RemoveUpdateCallback( stg_model_callback_t cb )
- { RemoveCallback( &update, cb ); }
+ { RemoveCallback( &update_hook, cb ); }
/** named-property interface
*/
@@ -1613,21 +1693,10 @@ protected:
/** returns the first descendent of this model that is unsubscribed
and has the type indicated by the string */
- StgModel* GetUnsubscribedModelOfType( char* modelstr );
-
- // 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 new StgModel( world, parent, id, typestr );
- }
+ StgModel* GetUnsubscribedModelOfType( stg_model_type_t type );
// iff true, model may output some debugging visualizations and other info
bool debug;
-
};
// BLOCKS
@@ -1998,17 +2067,16 @@ class StgModelBlobfinder : public StgModel
stg_radians_t fov;
stg_radians_t pan;
- // constructor
- StgModelBlobfinder( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr);
-
- // destructor
- ~StgModelBlobfinder();
+ static const char* typestr;
- virtual void Startup();
- virtual void Shutdown();
+ // constructor
+ StgModelBlobfinder( StgWorld* world,
+ StgModel* parent );
+ // destructor
+ ~StgModelBlobfinder();
+
+ virtual void Startup();
+ virtual void Shutdown();
virtual void Update();
virtual void Load();
virtual void DataVisualize();
@@ -2028,16 +2096,6 @@ class StgModelBlobfinder : public StgModel
/** 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 --------------------------------------------------------------
@@ -2101,54 +2159,42 @@ typedef struct
class StgModelLaser : public StgModel
{
- private:
- int dl_debug_laser;
-
- stg_laser_sample_t* samples;
- uint32_t sample_count;
- stg_meters_t range_min, range_max;
- stg_radians_t fov;
- uint32_t resolution;
-
- public:
- // constructor
- StgModelLaser( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr );
-
- // destructor
- ~StgModelLaser();
-
- virtual void Startup();
- virtual void Shutdown();
- virtual void Update();
- virtual void Load();
- virtual void Print( char* prefix );
- virtual void DataVisualize();
-
- uint32_t GetSampleCount(){ return sample_count; }
-
- stg_laser_sample_t* GetSamples( uint32_t* count=NULL);
-
- void SetSamples( stg_laser_sample_t* samples, uint32_t count);
-
- // 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 );
-
-
- // 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 StgModelLaser( world, parent, id, typestr );
- }
+private:
+ int dl_debug_laser;
+
+ stg_laser_sample_t* samples;
+ uint32_t sample_count;
+ stg_meters_t range_min, range_max;
+ stg_radians_t fov;
+ uint32_t resolution;
+
+public:
+ static const char* typestr;
+ // constructor
+ StgModelLaser( StgWorld* world,
+ StgModel* parent );
+
+ // destructor
+ ~StgModelLaser();
+
+ virtual void Startup();
+ virtual void Shutdown();
+ virtual void Update();
+ virtual void Load();
+ virtual void Print( char* prefix );
+ virtual void DataVisualize();
+
+ uint32_t GetSampleCount(){ return sample_count; }
+
+ stg_laser_sample_t* GetSamples( uint32_t* count=NULL);
+
+ void SetSamples( stg_laser_sample_t* samples, uint32_t count);
+
+ // 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 );
};
// \todo GRIPPER MODEL --------------------------------------------------------
@@ -2294,15 +2340,13 @@ class StgModelFiducial : public StgModel
GArray* data;
public:
- // constructor
- StgModelFiducial( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr );
-
- // destructor
- virtual ~StgModelFiducial();
-
+ static const char* typestr;
+ // constructor
+ StgModelFiducial( StgWorld* world,
+ StgModel* parent );
+ // destructor
+ virtual ~StgModelFiducial();
+
virtual void Load();
stg_meters_t max_range_anon; //< maximum detection range
@@ -2314,16 +2358,6 @@ class StgModelFiducial : public StgModel
stg_fiducial_t* fiducials; ///< array of detected fiducials
uint32_t fiducial_count; ///< the number of fiducials detected
-
- // 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 StgModelFiducial( world, parent, id, typestr );
- }
};
@@ -2348,12 +2382,10 @@ class StgModelRanger : public StgModel
virtual void DataVisualize();
public:
+ static const char* typestr;
// constructor
StgModelRanger( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr );
-
+ StgModel* parent );
// destructor
virtual ~StgModelRanger();
@@ -2363,16 +2395,6 @@ class StgModelRanger : public StgModel
uint32_t sensor_count;
stg_ranger_sensor_t* sensors;
stg_meters_t* samples;
-
- // 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 StgModelRanger( world, parent, id, typestr );
- }
};
// BLINKENLIGHT MODEL ----------------------------------------------------
@@ -2386,26 +2408,15 @@ class StgModelBlinkenlight : public StgModel
public:
+ static const char* typestr;
StgModelBlinkenlight( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr );
+ StgModel* parent );
~StgModelBlinkenlight();
virtual void Load();
virtual void Update();
virtual void Draw( uint32_t flags, StgCanvas* canvas );
-
- // 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 StgModelBlinkenlight( world, parent, id, typestr );
- }
};
// CAMERA MODEL ----------------------------------------------------
@@ -2423,11 +2434,8 @@ class StgModelCamera : public StgModel
StgPerspectiveCamera _camera;
public:
-
StgModelCamera( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr );
+ StgModel* parent );
~StgModelCamera();
@@ -2443,16 +2451,6 @@ class StgModelCamera : public StgModel
///Imiate laser scan
float* laser();
-
- // 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 StgModelCamera( world, parent, id, typestr );
- }
};
// POSITION MODEL --------------------------------------------------------
@@ -2487,12 +2485,10 @@ class StgModelPosition : public StgModel
stg_velocity_t integration_error; ///< errors to apply in simple odometry model
public:
+ static const char* typestr;
// constructor
StgModelPosition( StgWorld* world,
- StgModel* parent,
- stg_id_t id,
- char* typestr);
-
+ StgModel* parent );
// destructor
~StgModelPosition();
@@ -2522,17 +2518,6 @@ class StgModelPosition : public StgModel
stg_pose_t est_pose; ///< position estimate in local coordinates
stg_pose_t est_pose_error; ///< estimated error in position estimate
stg_pose_t est_origin; ///< global origin of the local coordinate system
-
- // 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 StgModelPosition( world, parent, id, typestr );
- }
-
};
View
69 libstage/test.cc
@@ -35,15 +35,17 @@ int main( int argc, char* argv[] )
Init( &argc, &argv);
StgWorldGui world( 400,400, "Test" );
+ world.Start();
- StgModel mod( &world, NULL, 0, "model" );
-
+ stg_geom_t geom;
+ bzero( &geom, sizeof(geom) );
- // returned pose must be the same as the set pose
- stg_pose_t pose;
-
- world.Start();
-
+ {
+ StgModel mod( &world, NULL );
+
+ // returned pose must be the same as the set pose
+ stg_pose_t pose;
+
for( stg_meters_t x=0; x<5; x+=0.1 )
{
pose = new_pose( x, 0, 0, 0 );
@@ -87,8 +89,6 @@ int main( int argc, char* argv[] )
mod.SetPose( new_pose( 0,0,0,0 ));
- stg_geom_t geom;
- bzero( &geom, sizeof(geom) );
for( stg_meters_t x=0.01; x<5; x+=0.1 )
{
@@ -126,28 +126,59 @@ int main( int argc, char* argv[] )
mod.SetGeom( geom );
+ } // mod goes out of scope
+
-#define POP 100
+ #define POP 100
StgModel* m[POP];
for( int i=0; i<POP; i++ )
{
- m[i] = new StgModel( &world, NULL, 0, "model" );
+ m[i] = new StgModel( &world, NULL );
+
+ //m[i]->Say( "Hello" );
m[i]->SetGeom( geom );
- //m[i]->SetPose( random_pose( -10,10, -10,10 ) );
- m[i]->PlaceInFreeSpace( -10, 10, -10, 10 );
+ m[i]->SetPose( random_pose( -10,10, -10,10 ) );
+ //m[i]->PlaceInFreeSpace( -10, 10, -10, 10 );
m[i]->SetColor( lrand48() | 0xFF000000 );
interact( &world );
}
+
+ geom.size.x = 0.2;
+ geom.size.y = 0.2;
+ geom.size.z = 0.2;
+
+ for( int i=0; i<POP; i++ )
+ {
+ StgModel* top = new StgModel( &world, m[i] );
+ top->SetGeom( geom );
+ //top->SetPose( new_pose( 0,0,0,0 ) );
+ //m[i]->SetPose( random_pose( -10,10, -10,10 ) );
+ //m[i]->PlaceInFreeSpace( -10, 10, -10, 10 );
+ top->SetColor( lrand48() | 0xFF000000 );
+
+ interact( &world );
+ }
+
+ for( int i=0; i<POP; i++ )
+ {
+// m[i]->PlaceInFreeSpace( -10, 10, -10, 10 );
+ //m[i]->SetColor( 0xFF00FF00 );
+