Revision: 8064
http://playerstage.svn.sourceforge.net/playerstage/?rev=8064&view=rev
Author: rtv
Date: 2009-07-21 07:46:10 +0000 (Tue, 21 Jul 2009)
Log Message:
-----------
more STLing, and viz fixes
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/libstage/canvas.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_callbacks.cc
code/stage/trunk/libstage/model_draw.cc
code/stage/trunk/libstage/model_load.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/libstage/worldgui.cc
code/stage/trunk/worlds/fasr.world
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2009-07-21 05:03:39 UTC (rev
8063)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2009-07-21 07:46:10 UTC (rev
8064)
@@ -494,7 +494,6 @@
}
};
-
// Stage calls this when the model starts up
extern "C" int Init( Model* mod )
{
@@ -515,7 +514,6 @@
putchar( '\n' );
}
delete data;
-
}
new Robot( (ModelPosition*)mod,
Modified: code/stage/trunk/libstage/canvas.cc
===================================================================
--- code/stage/trunk/libstage/canvas.cc 2009-07-21 05:03:39 UTC (rev 8063)
+++ code/stage/trunk/libstage/canvas.cc 2009-07-21 07:46:10 UTC (rev 8064)
@@ -770,164 +770,166 @@
if( showTree || showOccupancy )
{
glPushMatrix();
-
- GLfloat scale = 1.0/world->Resolution();
+
+ GLfloat scale = 1.0/world->Resolution();
glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly
-
// out for Z. look into it.
-
+ // out for Z. look into it.
+
if( showOccupancy )
- ((WorldGui*)world)->DrawTree( false );
-
+ ((WorldGui*)world)->DrawTree( false );
+
if( showTree )
- ((WorldGui*)world)->DrawTree( true );
-
+ ((WorldGui*)world)->DrawTree( true );
+
glPopMatrix();
}
if( ! world->rt_cells.empty() )
- {
- glPushMatrix();
- GLfloat scale = 1.0/world->Resolution();
+ {
+ glPushMatrix();
+ GLfloat scale = 1.0/world->Resolution();
glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly
-
- world->PushColor( Color( 0,0,1,0.5) );
-
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
-
-
- glPointSize( 2 );
- glBegin( GL_POINTS );
-
- for( unsigned int i=0;
- i < world->rt_cells.size();
- i++ )
- {
- char str[128];
- snprintf( str, 128, "(%d,%d)",
- world->rt_cells[i].x,
- world->rt_cells[i].y );
-
- Gl::draw_string( world->rt_cells[i].x+1,
-
world->rt_cells[i].y+1, 0.1, str );
-
- //printf( "x: %d y: %d\n", world->rt_regions[i].x,
world->rt_regions[i].y );
- //glRectf( world->rt_cells[i].x+0.3,
world->rt_cells[i].y+0.3,
- // world->rt_cells[i].x+0.7,
world->rt_cells[i].y+0.7 );
-
- glVertex2f( world->rt_cells[i].x,
world->rt_cells[i].y );
-
- }
-
- glEnd();
-
+
+ world->PushColor( Color( 0,0,1,0.5) );
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+
+ glPointSize( 2 );
+ glBegin( GL_POINTS );
+
+ for( unsigned int i=0;
+ i < world->rt_cells.size();
+ i++ )
+ {
+ char str[128];
+ snprintf( str, 128, "(%d,%d)",
+
world->rt_cells[i].x,
+
world->rt_cells[i].y );
+
+ Gl::draw_string(
world->rt_cells[i].x+1,
+
world->rt_cells[i].y+1, 0.1, str );
+
+ //printf( "x: %d y: %d\n",
world->rt_regions[i].x, world->rt_regions[i].y );
+ //glRectf( world->rt_cells[i].x+0.3,
world->rt_cells[i].y+0.3,
+ // world->rt_cells[i].x+0.7,
world->rt_cells[i].y+0.7 );
+
+ glVertex2f( world->rt_cells[i].x,
world->rt_cells[i].y );
+
+ }
+
+ glEnd();
+
#if 1
world->PushColor( Color( 0,1,0,0.2) );
glBegin( GL_LINE_STRIP );
for( unsigned int i=0;
- i < world->rt_cells.size();
- i++ )
+ i < world->rt_cells.size();
+ i++ )
{
- glVertex2f( world->rt_cells[i].x+0.5,
world->rt_cells[i].y+0.5 );
+ glVertex2f( world->rt_cells[i].x+0.5,
world->rt_cells[i].y+0.5 );
}
glEnd();
world->PopColor();
#endif
-
+
glPopMatrix();
- world->PopColor();
+ world->PopColor();
}
-
+
if( ! world->rt_candidate_cells.empty() )
- {
- glPushMatrix();
- GLfloat scale = 1.0/world->Resolution();
+ {
+ glPushMatrix();
+ GLfloat scale = 1.0/world->Resolution();
glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly
-
- world->PushColor( Color( 1,0,0,0.5) );
-
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
-
- for( unsigned int i=0;
- i < world->rt_candidate_cells.size();
- i++ )
- {
-// char str[128];
-// snprintf( str, 128, "(%d,%d)",
-//
world->rt_candidate_cells[i].x,
-//
world->rt_candidate_cells[i].y );
-
-// Gl::draw_string( world->rt_candidate_cells[i].x+1,
-//
world->rt_candidate_cells[i].y+1, 0.1, str );
-
- //printf( "x: %d y: %d\n", world->rt_regions[i].x,
world->rt_regions[i].y );
- glRectf( world->rt_candidate_cells[i].x,
world->rt_candidate_cells[i].y,
-
world->rt_candidate_cells[i].x+1, world->rt_candidate_cells[i].y+1 );
- }
-
- world->PushColor( Color( 0,1,0,0.2) );
- glBegin( GL_LINE_STRIP );
- for( unsigned int i=0;
- i < world->rt_candidate_cells.size();
- i++ )
- {
- glVertex2f( world->rt_candidate_cells[i].x+0.5,
world->rt_candidate_cells[i].y+0.5 );
- }
- glEnd();
- world->PopColor();
-
+
+ world->PushColor( Color( 1,0,0,0.5) );
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+
+ for( unsigned int i=0;
+ i < world->rt_candidate_cells.size();
+ i++ )
+ {
+ // char str[128];
+ // snprintf( str,
128, "(%d,%d)",
+ //
world->rt_candidate_cells[i].x,
+ //
world->rt_candidate_cells[i].y );
+
+ //
Gl::draw_string( world->rt_candidate_cells[i].x+1,
+ //
world->rt_candidate_cells[i].y+1, 0.1, str );
+
+ //printf( "x: %d y: %d\n",
world->rt_regions[i].x, world->rt_regions[i].y );
+ glRectf(
world->rt_candidate_cells[i].x, world->rt_candidate_cells[i].y,
+
world->rt_candidate_cells[i].x+1, world->rt_candidate_cells[i].y+1 );
+ }
+
+ world->PushColor( Color( 0,1,0,0.2) );
+ glBegin( GL_LINE_STRIP );
+ for( unsigned int i=0;
+ i < world->rt_candidate_cells.size();
+ i++ )
+ {
+ glVertex2f(
world->rt_candidate_cells[i].x+0.5, world->rt_candidate_cells[i].y+0.5 );
+ }
+ glEnd();
+ world->PopColor();
+
glPopMatrix();
- world->PopColor();
-
- //world->rt_cells.clear();
+ world->PopColor();
+
+ //world->rt_cells.clear();
}
-
-
- if( showFootprints )
- {
- glDisable( GL_DEPTH_TEST );
-
- FOR_EACH( it, models_sorted )
- (*it)->DrawTrailFootprint();
-
- glEnable( GL_DEPTH_TEST );
- }
+
+
if( showGrid )
- DrawGlobalGrid();
+ DrawGlobalGrid();
else
- DrawFloor();
+ DrawFloor();
+ if( showFootprints )
+ {
+ glDisable( GL_DEPTH_TEST );
+
+ FOR_EACH( it, models_sorted )
+ (*it)->DrawTrailFootprint();
+
+ glEnable( GL_DEPTH_TEST );
+ }
+
if( showBlocks )
- DrawBlocks();
-
+ DrawBlocks();
+
if( showBBoxes )
- DrawBoundingBoxes();
-
+ DrawBoundingBoxes();
+
// TODO - finish this properly
//LISTMETHOD( models_sorted, Model*, DrawWaypoints );
+
+
-// MOTION BLUR
+ // MOTION BLUR
if( 0 )//showBlur )
- {
- DrawBlocks();
-
- //static float count = 0;
-
- if( ! blur )
- {
- blur = true;
- glClear( GL_ACCUM_BUFFER_BIT | GL_COLOR_BUFFER_BIT |
GL_DEPTH_BUFFER_BIT );
- glAccum( GL_LOAD, 1.0 );
- }
- else
- {
- glAccum( GL_MULT, 0.9 );
- glAccum( GL_ACCUM, 0.1 );
-
- glAccum( GL_RETURN, 1.1 );
-
- DrawBlocks(); // outline at current location
- }
+ {
+ DrawBlocks();
+
+ //static float count = 0;
+
+ if( ! blur )
+ {
+ blur = true;
+ glClear( GL_ACCUM_BUFFER_BIT |
GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
+ glAccum( GL_LOAD, 1.0 );
+ }
+ else
+ {
+ glAccum( GL_MULT, 0.9 );
+ glAccum( GL_ACCUM, 0.1 );
+
+ glAccum( GL_RETURN, 1.1 );
+
+ DrawBlocks(); // outline at current
location
+ }
}
// GRAY TRAILS
@@ -1137,7 +1139,7 @@
void Canvas::EnterScreenCS()
{
- //use orthogonal projeciton without any zoom
+ //use orthogonal projection without any zoom
glMatrixMode (GL_PROJECTION);
glPushMatrix(); //save old projection
glLoadIdentity ();
@@ -1170,13 +1172,13 @@
// pixels avoid a nasty word-alignment problem when indexing into
// the pixel array.
- // might save a bit of time as the image size rarely changes
- static uint8_t* pixels = NULL;
- pixels = g_renew( uint8_t, pixels, width * height * depth );
+ // might save a bit of time with a static var as the image size rarely
changes
+ static std::vector<uint8_t> pixels;
+ pixels.resize( width * height * depth );
glFlush(); // make sure the drawing is done
// read the pixels from the screen
- glReadPixels( 0,0, width, height, GL_RGBA, GL_UNSIGNED_BYTE, pixels );
+ glReadPixels( 0,0, width, height, GL_RGBA, GL_UNSIGNED_BYTE, &pixels[0] );
static uint32_t count = 0;
char filename[64];
@@ -1185,7 +1187,7 @@
FILE *fp = fopen( filename, "wb" );
if( fp == NULL )
{
- PRINT_ERR1( "Unable to open %s", filename );
+ PRINT_ERR1( "Unable to open %s", filename );
}
// create PNG data
@@ -1253,9 +1255,9 @@
showOccupancy.createMenuItem( menu, path );
//showTrailArrows.createMenuItem( menu, path ); // broken
showTrails.createMenuItem( menu, path );
- // showTrailRise.createMenuItem( menu, path ); // broken
+ //showTrailRise.createMenuItem( menu, path ); // broken
showBBoxes.createMenuItem( menu, path );
- showBlur.createMenuItem( menu, path );
+ //showBlur.createMenuItem( menu, path );
showTree.createMenuItem( menu, path );
showScreenshots.createMenuItem( menu, path );
}
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-07-21 05:03:39 UTC (rev 8063)
+++ code/stage/trunk/libstage/model.cc 2009-07-21 07:46:10 UTC (rev 8064)
@@ -196,11 +196,11 @@
blockgroup(),
blocks_dl(0),
boundary(false),
- callbacks( g_hash_table_new( g_direct_hash, g_direct_equal ) ),
+ callbacks(),// g_hash_table_new( g_direct_hash, g_direct_equal ) ),
color( 1,0,0 ), // red
data_fresh(false),
disabled(false),
- cv_list(),
+ cv_list(),
flag_list(),
geom(),
has_default_block( true ),
@@ -208,33 +208,35 @@
initfunc(NULL),
interval((stg_usec_t)1e4), // 10msec
last_update(0),
- log_state(false),
+ log_state(false),
map_resolution(0.1),
mass(0),
on_velocity_list( false ),
parent(parent),
pose(),
- power_pack( NULL ),
- pps_charging(),
+ power_pack( NULL ),
+ pps_charging(),
props(NULL),
- rastervis(),
+ rastervis(),
rebuild_displaylist(true),
say_string(NULL),
stall(false),
subs(0),
thread_safe( false ),
- trail( g_array_new( false, false, sizeof(stg_trail_item_t) )),
+ trail(),
+ trail_length(25),
+ trail_interval(5),
type(type),
- update_list_num( -1 ),
+ update_list_num( -1 ),
used(false),
velocity(),
watts(0.0),
- watts_give(0.0),
- watts_take(0.0),
+ watts_give(0.0),
+ watts_take(0.0),
wf(NULL),
wf_entity(0),
world(world),
- world_gui( dynamic_cast<WorldGui*>( world ) )
+ world_gui( dynamic_cast<WorldGui*>( world ) )
{
//assert( modelsbyid );
assert( world );
@@ -281,7 +283,7 @@
ModelPtrVec& vec = parent ? parent->children : world->children;
vec.erase( std::remove( vec.begin(), vec.end(), this ));
- if( callbacks ) g_hash_table_destroy( callbacks );
+ //if( callbacks ) g_hash_table_destroy( callbacks );
g_datalist_clear( &props );
@@ -864,21 +866,14 @@
if( disabled )
return;
- // TODO - control this properly, and maybe do it faster
- //if( 0 )
- //if( (world->updates % 10 == 0) )
- // {
- // stg_trail_item_t checkpoint;
- // memcpy( &checkpoint.pose, &pose, sizeof(pose));
- // checkpoint.color = color;
- // checkpoint.time = world->sim_time;
-
- // if( trail->len > 100 )
- // g_array_remove_index( trail, 0 );
-
- // g_array_append_val( this->trail, checkpoint );
- // }
-
+ if( world->updates % trail_interval == 0 )
+ {
+ trail.push_back( TrailItem( world->sim_time,
GetGlobalPose(), color ) );
+
+ if( trail.size() > trail_length )
+ trail.pop_front();
+ }
+
// convert usec to sec
double interval( (double)world->interval_sim / 1e6 );
Modified: code/stage/trunk/libstage/model_callbacks.cc
===================================================================
--- code/stage/trunk/libstage/model_callbacks.cc 2009-07-21 05:03:39 UTC
(rev 8063)
+++ code/stage/trunk/libstage/model_callbacks.cc 2009-07-21 07:46:10 UTC
(rev 8064)
@@ -1,141 +1,50 @@
#include "stage.hh"
using namespace Stg;
+using namespace std;
-
void Model::AddCallback( void* address,
stg_model_callback_t cb,
void* user )
{
-// // treat update callbacks specially as they are so frequently called
-// if( address == &hooks.update )
-// {
-// // a dedicated vector avoids a hash table lookup
-// update_callbacks.push_back( std::pair<stg_model_callback_t,void*>(
cb, user ) );
-// }
-// else
- {
- GList* cb_list = (GList*)g_hash_table_lookup( callbacks, address );
-
- //printf( "installing callback in model %s with key %p\n",
- // token, address );
-
- // add the callback & argument to the list
- cb_list = g_list_prepend( cb_list, new stg_cb_t( cb, user ) );
-
- // and replace the list in the hash table
- g_hash_table_insert( callbacks, address, cb_list );
- }
+ callbacks[address].insert( stg_cb_t( cb, user ));
}
-int Model::RemoveCallback( void* member,
- stg_model_callback_t callback )
+int Model::RemoveCallback( void* address,
+
stg_model_callback_t callback )
{
-
- // XX TODO
- // if( address == &hooks.update )
- // {
- // std::erase( std::remove( update_callbacks.begin();
update_callbacks.end(),
-
- GList* cb_list = (GList*)g_hash_table_lookup( callbacks, member );
-
- // find our callback in the list of stg_cbarg_t
- GList* el = NULL;
-
- // scan the list for the first matching callback
- for( el = g_list_first( cb_list );
- el;
- el = el->next )
- {
- if( ((stg_cb_t*)el->data)->callback == callback )
- break;
- }
-
- if( el ) // if we found the matching callback, remove it
- {
- //puts( "removed callback" );
-
- cb_list = g_list_remove( cb_list, el->data);
-
- // store the new, shorter, list of callbacks
- g_hash_table_insert( callbacks, member, cb_list );
-
- // we're done with the stored data
- delete (stg_cb_t*)(el->data);
- }
- else
- {
- //puts( "callback was not installed" );
- }
+ set<stg_cb_t>& callset = callbacks[address];
- // return the number of callbacks now in the list. Useful for
- // detecting when the list is empty.
- return g_list_length( cb_list );
+ callset.erase( stg_cb_t( callback, NULL) );
+
+ // return the number of callbacks remaining for this address. Useful
+ // for detecting when there are none.
+ return callset.size();
}
-void Model::CallCallbacks( void* address )
+int Model::CallCallbacks( void* address )
{
-
assert( address );
-// // avoid a hash table lookup for
-// if( address == &hooks.update )
-// {
-// FOR_EACH( it, update_callbacks )
-// (it->first)( this, it->second );
-
-// return;
-// }
-
-
- //int key = key_gen( this, address );
+ // maintain a list of callbacks that should be cancelled
+ vector<stg_cb_t> doomed;
- //printf( "CallCallbacks for model %s %p key %p\n", this->Token(),
this, address );
-
- //printf( "Model %s has %d callbacks. Checking key %d\n",
- // this->token, g_hash_table_size( this->callbacks ),
key );
+ set<stg_cb_t>& callset = callbacks[address];
- GList* cbs = (GList*)g_hash_table_lookup( callbacks, address );
+ FOR_EACH( it, callset )
+ {
+ const stg_cb_t& cba = *it;
+ if( (cba.callback)( this, cba.arg ) )
+ doomed.push_back( cba );
+ }
- //printf( "key %p has %d callbacks registered\n",
- // address, g_list_length( cbs ) );
-
- // maintain a list of callbacks that should be cancelled
- GList* doomed = NULL;
+ FOR_EACH( it, doomed )
+ callset.erase( *it );
- // for each callback in the list
- while( cbs )
- {
- //printf( "cbs %p data %p cvs->next %p\n", cbs, cbs->data, cbs->next
);
-
- stg_cb_t* cba = (stg_cb_t*)cbs->data;
- assert( cba );
-
- if( (cba->callback)( this, cba->arg ) )
- {
- //printf( "callback returned TRUE - schedule removal from
list\n" );
- doomed = g_list_prepend( doomed, (void*)cba->callback );
- }
- else
- {
- //printf( "callback returned FALSE - keep in list\n" );
- }
-
- cbs = cbs->next;
- }
-
- if( doomed ) // delete all the callbacks that returned TRUE
- {
- //printf( "removing %d doomed callbacks\n", g_list_length(
doomed ) );
-
- for( ; doomed ; doomed = doomed->next )
- this->RemoveCallback( address,
(stg_model_callback_t)doomed->data );
-
- g_list_free( doomed );
- }
-
- //puts( "Callbacks done" );
+ // return the number of callbacks remaining for this address. Useful
+ // for detecting when there are none.
+ return callset.size();
}
Modified: code/stage/trunk/libstage/model_draw.cc
===================================================================
--- code/stage/trunk/libstage/model_draw.cc 2009-07-21 05:03:39 UTC (rev
8063)
+++ code/stage/trunk/libstage/model_draw.cc 2009-07-21 07:46:10 UTC (rev
8064)
@@ -50,26 +50,33 @@
void Model::DrawTrailFootprint()
{
- for( int i=trail->len-1; i>=0; i-- )
+ double darkness = 0;
+ double fade = 0.5 / (double)(trail_length+1);
+
+ FOR_EACH( it, trail )
{
- stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t,
i );
-
+ TrailItem& checkpoint = *it;
+
glPushMatrix();
- Gl::pose_shift( checkpoint->pose );
- Gl::pose_shift( geom.pose );
+ Pose pz = checkpoint.pose;
+ pz.z += 0.02;
+ Gl::pose_shift( pz );
+ Gl::pose_shift( geom.pose );
+
+ darkness += fade;
- //stg_color_unpack( checkpoint->color, &r, &g, &b, &a );
- Color c = checkpoint->color;
- c.a = 0.1;
- PushColor( c );
+ Color c = checkpoint.color;
+ c.a = darkness;
+ PushColor( c );
+
blockgroup.DrawFootPrint( geom );
-
+
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
- PushColor( c.r/2, c.g/2, c.b/2, 0.1 );
-
+ PushColor( c.r/2, c.g/2, c.b/2, darkness );
+
blockgroup.DrawFootPrint( geom );
-
+
PopColor();
PopColor();
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
@@ -81,17 +88,14 @@
{
double timescale = 0.0000001;
- for( int i=trail->len-1; i>=0; i-- )
+ FOR_EACH( it, trail )
{
- stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t,
i );
+ TrailItem& checkpoint = *it;
- Pose pose;
- memcpy( &pose, &checkpoint->pose, sizeof(pose));
- pose.z = (world->sim_time - checkpoint->time) * timescale;
+ Pose pose = checkpoint.pose;
+ pose.z = (world->sim_time - checkpoint.time) * timescale;
PushLocalCoords();
- //blockgroup.Draw( this );
-
DrawBlocksTree();
PopCoords();
}
@@ -103,19 +107,18 @@
double dy = 0.07;
double timescale = 0.0000001;
- for( unsigned int i=0; i<trail->len; i++ )
+ FOR_EACH( it, trail )
{
- stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t,
i );
+ TrailItem& checkpoint = *it;
- Pose pose;
- memcpy( &pose, &checkpoint->pose, sizeof(pose));
- pose.z = (world->sim_time - checkpoint->time) * timescale;
+ Pose pose = checkpoint.pose;
+ pose.z = (world->sim_time - checkpoint.time) * timescale;
PushLocalCoords();
// set the height proportional to age
- PushColor( checkpoint->color );
+ PushColor( checkpoint.color );
glEnable(GL_POLYGON_OFFSET_FILL);
glPolygonOffset(1.0, 1.0);
@@ -128,13 +131,13 @@
glDisable(GL_POLYGON_OFFSET_FILL);
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+
+ Color c = checkpoint.color;
+ c.r /= 2.0;
+ c.g /= 2.0;
+ c.b /= 2.0;
+ PushColor( c ); // darker color
- Color c = checkpoint->color;
- c.r /= 2.0;
- c.g /= 2.0;
- c.b /= 2.0;
- PushColor( c ); // darker color
-
glDepthMask(GL_FALSE);
glBegin( GL_TRIANGLES );
glVertex3f( 0, -dy, 0);
Modified: code/stage/trunk/libstage/model_load.cc
===================================================================
--- code/stage/trunk/libstage/model_load.cc 2009-07-21 05:03:39 UTC (rev
8063)
+++ code/stage/trunk/libstage/model_load.cc 2009-07-21 07:46:10 UTC (rev
8064)
@@ -226,6 +226,8 @@
if( wf->PropertyExists( wf_entity, "say" ))
this->Say( wf->ReadString(wf_entity, "say", NULL ));
+ trail_length = wf->ReadInt( wf_entity, "trail_length", trail_length );
+ trail_interval = wf->ReadInt( wf_entity, "trail_interval",
trail_interval );
// call any type-specific load callbacks
this->CallCallbacks( &hooks.load );
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-07-21 05:03:39 UTC (rev 8063)
+++ code/stage/trunk/libstage/stage.hh 2009-07-21 07:46:10 UTC (rev 8064)
@@ -49,6 +49,7 @@
#include <ext/hash_map>
//#include <unordered_map>
#include <set>
+#include <queue>
#include <algorithm>
// we use GLib's data structures extensively. Gradually we're moving
@@ -118,14 +119,13 @@
MODEL_TYPE_BLOBFINDER,
MODEL_TYPE_BLINKENLIGHT,
MODEL_TYPE_CAMERA,
- MODEL_TYPE_GRIPPER,
- MODEL_TYPE_ACTUATOR,
- MODEL_TYPE_LOADCELL,
- MODEL_TYPE_LIGHTINDICATOR,
- MODEL_TYPE_COUNT // must be the last entry, to count the number of
- // types
+ MODEL_TYPE_GRIPPER,
+ MODEL_TYPE_ACTUATOR,
+ MODEL_TYPE_LOADCELL,
+ MODEL_TYPE_LIGHTINDICATOR,
+ MODEL_TYPE_COUNT // must be the last entry, to count the number of types
} stg_model_type_t;
-
+
/// Copyright string
const char COPYRIGHT[] =
"Copyright Richard Vaughan and contributors 2000-2009";
@@ -144,7 +144,7 @@
/// Project distribution license string
const char LICENSE[] =
"Stage robot simulation library\n" \
- "Copyright (C) 2000-2008 Richard Vaughan and contributors\n" \
+ "Copyright (C) 2000-2009 Richard Vaughan and contributors\n" \
"Part of the Player Project [http://playerstage.org]\n"; \
"\n" \
"This program is free software; you can redistribute it and/or\n" \
@@ -195,12 +195,6 @@
/** take binary sign of a, either -1, or 1 if >= 0. */
inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); }
-
- /** Describe the image format used for saving screenshots. */
- typedef enum {
- STG_IMAGE_FORMAT_PNG,
- STG_IMAGE_FORMAT_JPG
- } stg_image_format_t;
/** any integer value other than this is a valid fiducial ID */
enum { FiducialNone = 0 };
@@ -300,9 +294,9 @@
stg_meters_t ymin, stg_meters_t
ymax )
{
return Pose( xmin + drand48() * (xmax-xmin),
- ymin + drand48() * (ymax-ymin),
- 0,
- normalize( drand48() * (2.0 * M_PI) ));
+ ymin +
drand48() * (ymax-ymin),
+ 0,
+
normalize( drand48() * (2.0 * M_PI) ));
}
/** Print pose in human-readable format on stdout
@@ -605,30 +599,17 @@
double duty_cycle; ///< mark/space ratio
} stg_blinkenlight_t;
- typedef struct {
+ class TrailItem
+ {
+ public:
+ stg_usec_t time;
Pose pose;
Color color;
- stg_usec_t time;
- } stg_trail_item_t;
-
- /** container for a callback function and a single argument, so
- they can be stored together in a list with a single pointer. */
- class stg_cb_t
- {
- public:
- stg_model_callback_t callback;
- void* arg;
-
- stg_cb_t( stg_model_callback_t cb, void* arg )
- : callback(cb), arg(arg) {}
- stg_cb_t( stg_world_callback_t cb, void* arg )
- : callback(NULL), arg(arg) {}
-
- stg_cb_t() : callback(NULL), arg(NULL) {}
+ TrailItem( stg_usec_t time, Pose pose, Color color )
+ : time(time), pose(pose), color(color){}
};
-
/** Defines a rectangle of [size] located at [pose] */
typedef struct
{
@@ -853,10 +834,10 @@
{ return( strcmp( a, b ) == 0 ); }
};
- __gnu_cxx::hash_map<const char*, Model*, __gnu_cxx::hash<const char*>,
eqstr > models_by_name; ///< the models that make up the world, indexed by name.
- //tr1::unordered_map<const char*, Model*, __gnu_cxx::hash<const
char*>, eqstr > models_by_name; ///< the models that make up the world, indexed
by name.
+ __gnu_cxx::hash_map<const char*, Model*, __gnu_cxx::hash<const
char*>, eqstr > models_by_name; ///< the models that make up the world, indexed
by name.
+
+ std::map<int,Model*> models_by_wfentity;
-
/** Keep a list of all models with detectable fiducials. This
avoids searching the whole world for fiducials. */
ModelPtrSet models_with_fiducials;
@@ -884,7 +865,6 @@
stg_bounds3d_t extent; ///< Describes the 3D volume of the world
bool graphics;///< true iff we have a GUI
stg_usec_t interval_sim; ///< temporal resolution: microseconds that
elapse between simulated time steps
- //GHashTable* option_table; ///< GUI options (toggles) registered by
models
std::set<Option*> option_table; ///< GUI options (toggles) registered
by models
std::list<PowerPack*> powerpack_list; ///< List of all the powerpacks
attached to models in the world
std::list<float*> ray_list;///< List of rays traced for debug
visualization
@@ -930,110 +910,109 @@
/** hint that the world needs to be redrawn if a GUI is attached */
void NeedRedraw(){ dirty = true; };
-
- /** Special model for the floor of the world */
- Model* ground;
-
+
+ /** Special model for the floor of the world */
+ Model* ground;
+
/** Get human readable string that describes the current simulation
- time. */
+ time. */
virtual std::string ClockString( void );
+
+ Model* CreateModel( Model* parent, const char* typestr );
+ void LoadModel( Worldfile* wf, int entity );
+ void LoadBlock( Worldfile* wf, int entity );
+ void LoadBlockGroup( Worldfile* wf, int entity );
- Model* CreateModel( Model* parent, const char* typestr );
- 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
);
-
- virtual Model* RecentlySelectedModel(){ return NULL; }
-
+ virtual Model* RecentlySelectedModel(){ return NULL; }
+
SuperRegion* AddSuperRegion( const stg_point_int_t& coord );
SuperRegion* GetSuperRegion( const stg_point_int_t& coord );
SuperRegion* GetSuperRegionCached( const stg_point_int_t& coord);
SuperRegion* GetSuperRegionCached( int32_t x, int32_t y );
void ExpireSuperRegion( SuperRegion* sr );
-
+
inline Cell* GetCell( const stg_point_int_t& glob );
-
- /** add a Cell pointer to the vector for each cell on the line from
- pt1 to pt2 inclusive */
+
+ /** add a Cell pointer to the vector for each cell on the line
from
+ pt1 to pt2 inclusive */
void ForEachCellInLine( const stg_point_int_t& pt1,
- const
stg_point_int_t& pt2,
-
CellPtrVec& cells );
-
+
const stg_point_int_t& pt2,
+
CellPtrVec& cells );
+
/** convert a distance in meters to a distance in world occupancy
- grid pixels */
+ grid pixels */
int32_t MetersToPixels( stg_meters_t x )
{ return (int32_t)floor(x * ppm); };
-
+
stg_point_int_t MetersToPixels( const stg_point_t& pt )
{ return stg_point_int_t( MetersToPixels(pt.x), MetersToPixels(pt.y)); };
-
+
// dummy implementations to be overloaded by GUI subclasses
virtual void PushColor( Color col ) { /* do nothing */ };
virtual void PushColor( double r, double g, double b, double a ) { /* do
nothing */ };
virtual void PopColor(){ /* do nothing */ };
-
+
SuperRegion* CreateSuperRegion( stg_point_int_t origin );
void DestroySuperRegion( SuperRegion* sr );
-
- /** trace a ray. */
- stg_raytrace_result_t Raytrace( const Ray& ray );
+
+ /** trace a ray. */
+ stg_raytrace_result_t Raytrace( const Ray& ray );
stg_raytrace_result_t Raytrace( const Pose& pose,
-
const stg_meters_t range,
-
const stg_ray_test_func_t func,
-
const Model* finder,
-
const void* arg,
-
const bool ztest );
-
+
const
stg_meters_t range,
+
const
stg_ray_test_func_t func,
+
const Model*
finder,
+
const void* arg,
+
const bool
ztest );
+
void Raytrace( const Pose &pose,
- const stg_meters_t range,
- const stg_radians_t fov,
- const stg_ray_test_func_t func,
- const Model* finder,
- const void* arg,
- stg_raytrace_result_t* samples,
- const uint32_t sample_count,
- const bool ztest );
-
-
+ const
stg_meters_t range,
+ const
stg_radians_t fov,
+ const
stg_ray_test_func_t func,
+ const
Model* finder,
+ const
void* arg,
+
stg_raytrace_result_t* samples,
+ const
uint32_t sample_count,
+ const
bool ztest );
+
+
/** Enlarge the bounding volume to include this point */
void Extend( stg_point3_t pt );
virtual void AddModel( Model* mod );
virtual void RemoveModel( Model* mod );
-
+
void AddPowerPack( PowerPack* pp );
void RemovePowerPack( PowerPack* pp );
-
+
void ClearRays();
/** store rays traced for debugging purposes */
void RecordRay( double x1, double y1, double x2, double y2 );
-
+
/** Returns true iff the current time is greater than the time we
- should quit */
+ should quit */
bool PastQuitTime();
-
+
int UpdateListAdd( Model* mod );
- void UpdateListRemove( Model* mod );
+ void UpdateListRemove( Model* mod );
- void VelocityListAdd( Model* mod );
- void VelocityListRemove( Model* mod );
-
- void ChargeListAdd( Model* mod );
- void ChargeListRemove( Model* mod );
-
+ void VelocityListAdd( Model* mod );
+ void VelocityListRemove( Model* mod );
+
+ void ChargeListAdd( Model* mod );
+ void ChargeListRemove( Model* mod );
+
static gpointer update_thread_entry( std::pair<World*,int>* info );
public:
/** returns true when time to quit, false otherwise */
static bool UpdateAll();
-
+
World( const char* token = "MyWorld",
- stg_msec_t interval_sim = DEFAULT_INTERVAL_SIM,
- double ppm = DEFAULT_PPM );
-
+ stg_msec_t interval_sim =
DEFAULT_INTERVAL_SIM,
+ double ppm = DEFAULT_PPM );
+
virtual ~World();
stg_usec_t SimTimeNow(void);
@@ -1718,18 +1697,43 @@
forming a solid boundary around the bounding box of the
model. */
int boundary;
-
- /** callback functions can be attached to any field in this
- structure. When the internal function
model_change(<mod>,<field>)
- is called, the callback is triggered */
- GHashTable* callbacks;
- /** Dedicated storage for update callbacks - avoids looking them up in
a hash table */
- //std::vector<std::pair<stg_model_callback_t,void*> > update_callbacks;
-
- /** Default color of the model's blocks.*/
- Color color;
- double friction;
-
+
+ /** container for a callback function and a single argument, so
+ they can be stored together in a list with a
single pointer. */
+ public:
+ class stg_cb_t
+ {
+ public:
+ stg_model_callback_t callback;
+ void* arg;
+
+ stg_cb_t( stg_model_callback_t cb, void* arg )
+ : callback(cb), arg(arg) {}
+
+ stg_cb_t( stg_world_callback_t cb, void* arg )
+ : callback(NULL), arg(arg) {}
+
+ stg_cb_t() : callback(NULL), arg(NULL) {}
+
+ /** for placing in a sorted container */
+ bool operator<( const stg_cb_t& other ) const
+ { return ((void*)(callback)) <
((void*)(other.callback)); }
+
+ /** for searching in a sorted container */
+ bool operator==( const stg_cb_t& other ) const
+ { return( callback == other.callback); }
+ };
+
+ protected:
+ /** A list of callback functions can be attached to any
+ address. When Model::CallCallbacks( void*) is
called, the
+ callbacks are called.*/
+ std::map<void*, std::set<stg_cb_t> > callbacks;
+
+ /** Default color of the model's blocks.*/
+ Color color;
+ double friction;
+
/** This can be set to indicate that the model has new data that
may be of interest to users. This allows polling the model
instead of adding a data callback. */
@@ -1786,7 +1790,6 @@
uint8_t* data;
unsigned int width, height;
stg_meters_t cellwidth, cellheight;
- //GList* pts;
std::vector<stg_point_t> pts;
public:
@@ -1807,19 +1810,30 @@
void ClearPts();
} rastervis;
-
- bool rebuild_displaylist; ///< iff true, regenerate block display list
before redraw
- char* say_string; ///< if non-null, this string is displayed in the
GUI
-
- stg_bool_t stall;
- /** Thread safety flag. Iff true, Update() may be called in
- parallel with other models. Defaults to false for safety */
- int subs; ///< the number of subscriptions to this model
- bool thread_safe;
- GArray* trail;
- stg_model_type_t type;
- /** The index into the world's vector of update lists. Initially
- -1, to indicate that it is not on a list yet. */
+
+ bool rebuild_displaylist; ///< iff true, regenerate block
display list before redraw
+ char* say_string; ///< if non-null, this string is displayed
in the GUI
+
+ stg_bool_t stall;
+ /** Thread safety flag. Iff true, Update() may be called in
+ parallel with other models. Defaults to false
for safety */
+ int subs; ///< the number of subscriptions to this model
+ bool thread_safe;
+
+ /** Cache of recent poses, used to draw the trail. */
+ std::list<TrailItem> trail;
+
+ /** The maxiumum length of the trail drawn. Default is 20, but
can
+ be set in the world file using the tail_length
model
+ property. */
+ unsigned int trail_length;
+
+ /** Number of world updates between trail records. */
+ long unsigned int trail_interval;
+
+ stg_model_type_t type;
+ /** The index into the world's vector of update lists. Initially
+ -1, to indicate that it is not on a list yet. */
int update_list_num;
bool used; ///< TRUE iff this model has been returned by
GetUnusedModelOfType()
Velocity velocity;
@@ -2213,23 +2227,23 @@
void SetGuiGrid( int val );
void SetGuiOutline( int val );
void SetWatts( stg_watts_t watts );
- void SetMapResolution( stg_meters_t res );
- void SetFriction( double friction );
+ void SetMapResolution( stg_meters_t res );
+ void SetFriction( double friction );
bool DataIsFresh() const { return this->data_fresh; }
/* attach callback functions to data members. The function gets
called when the member is changed using SetX() accessor method
*/
-
- void AddCallback( void* address,
- stg_model_callback_t
cb,
- void* user );
-
- int RemoveCallback( void* member,
-
stg_model_callback_t callback );
-
- void CallCallbacks( void* address );
-
+
+ void AddCallback( void* address,
+
stg_model_callback_t cb,
+
void* user );
+
+ int RemoveCallback( void* member,
+
stg_model_callback_t callback );
+
+ int CallCallbacks( void* address );
+
/* wrappers for the generic callback add & remove functions that hide
some implementation detail */
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2009-07-21 05:03:39 UTC (rev 8063)
+++ code/stage/trunk/libstage/world.cc 2009-07-21 07:46:10 UTC (rev 8064)
@@ -222,11 +222,10 @@
mod->Init();
}
-void World::LoadBlock( Worldfile* wf, int entity, GHashTable* entitytable )
+void World::LoadBlock( Worldfile* wf, int entity )
{
// lookup the group in which this was defined
- Model* mod = (Model*)g_hash_table_lookup( entitytable,
-
(gpointer)wf->GetEntityParent( entity ) );
+ Model* mod = models_by_wfentity[ wf->GetEntityParent( entity )];
if( ! mod )
PRINT_ERR( "block has no model for a parent" );
@@ -276,15 +275,14 @@
}
-void World::LoadModel( Worldfile* wf, int entity, GHashTable* entitytable )
+void World::LoadModel( Worldfile* wf, int entity )
{
int parent_entity = wf->GetEntityParent( entity );
PRINT_DEBUG2( "wf entity %d parent entity %d\n",
entity, parent_entity );
- Model* parent = (Model*)g_hash_table_lookup( entitytable,
-
(gpointer)parent_entity );
+ Model* parent = models_by_wfentity[ parent_entity ];
char *typestr = (char*)wf->GetEntityType(entity);
assert(typestr);
@@ -295,7 +293,7 @@
mod->Load(wf, entity );
// record the model we created for this worlfile entry
- g_hash_table_insert( entitytable, (gpointer)entity, mod );
+ models_by_wfentity[entity] = mod;
}
void World::Load( const char* worldfile_path )
@@ -303,8 +301,6 @@
// note: must call Unload() before calling Load() if a world already
// exists TODO: unload doesn't clean up enough right now
- GHashTable* entitytable = g_hash_table_new( g_direct_hash, g_direct_equal );
-
printf( " [Loading %s]", worldfile_path );
fflush(stdout);
@@ -366,30 +362,26 @@
// don't load window entries here
if( strcmp( typestr, "window" ) == 0 )
- {
- /* do nothing here */
- }
+ {
+ /* do nothing here */
+ }
else if( strcmp( typestr, "block" ) == 0 )
- LoadBlock( wf, entity, entitytable );
- else
- LoadModel( wf, entity, entitytable );
+ LoadBlock( wf, entity );
+ else
+ LoadModel( wf, entity );
}
-
-
+
// warn about unused WF lines
wf->WarnUnused();
- // now we're done with the worldfile entry lookup
- g_hash_table_destroy( entitytable );
-
FOR_EACH( it, children )
- (*it)->InitRecursive();
+ (*it)->InitRecursive();
stg_usec_t load_end_time = RealTimeNow();
-
+
if( debug )
printf( "[Load time %.3fsec]\n",
- (load_end_time - load_start_time) / 1000000.0 );
+ (load_end_time -
load_start_time) / 1e6 );
else
putchar( '\n' );
}
@@ -404,7 +396,7 @@
//g_hash_table_remove_all( models_by_name );
models_by_name.clear();
-
+ models_by_wfentity.clear();
update_lists.resize(1);
ray_list.clear();
Modified: code/stage/trunk/libstage/worldgui.cc
===================================================================
--- code/stage/trunk/libstage/worldgui.cc 2009-07-21 05:03:39 UTC (rev
8063)
+++ code/stage/trunk/libstage/worldgui.cc 2009-07-21 07:46:10 UTC (rev
8064)
@@ -224,8 +224,8 @@
mbar->add( "Run/Pause", 'p', (Fl_Callback*) WorldGui::pauseCb, this );
mbar->add( "Run/One step", '.', (Fl_Callback*) WorldGui::onceCb, this,
FL_MENU_DIVIDER );
mbar->add( "Run/Faster", ']', (Fl_Callback*) WorldGui::fasterCb, this );
- mbar->add( "Run/Slower", '[', (Fl_Callback*) WorldGui::slowerCb, this );
- mbar->add( "Run/Realtime", '{', (Fl_Callback*) WorldGui::realtimeCb, this,
FL_MENU_DIVIDER );
+ mbar->add( "Run/Slower", '[', (Fl_Callback*) WorldGui::slowerCb, this,
FL_MENU_DIVIDER );
+ mbar->add( "Run/Realtime", '{', (Fl_Callback*) WorldGui::realtimeCb, this );
mbar->add( "Run/Fast", '}', (Fl_Callback*) WorldGui::fasttimeCb, this );
mbar->add( "&Help", 0, 0, 0, FL_SUBMENU );
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2009-07-21 05:03:39 UTC (rev 8063)
+++ code/stage/trunk/worlds/fasr.world 2009-07-21 07:46:10 UTC (rev 8064)
@@ -87,10 +87,6 @@
)
-# rtv - test ones
-charge_station( pose [ -7.940 2.000 0 0 ] )
-charge_station( pose [ -7.940 3.000 0 0 ] )
-charge_station( pose [ -7.940 4.000 0 0 ] )
charge_station( pose [ 7.940 -2.000 0 0 ] )
charge_station( pose [ 7.940 -3.000 0 0 ] )
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Enter the BlackBerry Developer Challenge
This is your chance to win up to $100,000 in prizes! For a limited time,
vendors submitting new applications to BlackBerry App World(TM) will have
the opportunity to enter the BlackBerry Developer Challenge. See full prize
details at: http://p.sf.net/sfu/Challenge
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
