Permalink
Browse files

more efficient real-time tracking

  • Loading branch information...
1 parent 850944c commit c69a1c9a4ef6dbffd796b211ac112cb26a5fb52b rtv committed Aug 15, 2009
Showing with 99 additions and 160 deletions.
  1. +37 −36 libstage/canvas.cc
  2. +27 −4 libstage/model_draw.cc
  3. +19 −9 libstage/stage.hh
  4. +13 −21 libstage/worldgui.cc
  5. +1 −1 worlds/benchmark/hospital.world
  6. +2 −2 worlds/fasr.world
  7. +0 −58 worlds/puck.world
  8. +0 −29 worlds/pucktarget.inc
View
@@ -30,11 +30,13 @@ using namespace Stg;
static const int checkImageWidth = 2;
static const int checkImageHeight = 2;
static GLubyte checkImage[checkImageHeight][checkImageWidth][4];
-static GLuint texName;
static bool blur = true;
static bool init_done = false;
+//GLuint glowTex;
+GLuint checkTex;
+
void Canvas::TimerCallback( Canvas* c )
{
if( c->world->dirty )
@@ -149,36 +151,53 @@ void Canvas::InitGl()
GLuint mains_id = TextureManager::getInstance().loadTexture( fullpath.c_str() );
TextureManager::getInstance()._mains_texture_id = mains_id;
- //TODO merge this code into the textureManager?
+// // generate a small glow texture
+// GLubyte* pixels = new GLubyte[ 4 * 128 * 128 ];
+
+// for( int x=0; x<128; x++ )
+// for( int y=0; y<128; y++ )
+// {
+// GLubyte* p = &pixels[ 4 * (128*y + x)];
+// p[0] = (GLubyte)255; // red
+// p[1] = (GLubyte)0; // green
+// p[2] = (GLubyte)0; // blue
+// p[3] = (GLubyte)128; // alpha
+// }
+
+
+// glGenTextures(1, &glowTex );
+// glBindTexture( GL_TEXTURE_2D, glowTex );
+
+// glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, 128, 128, 0,
+// GL_RGBA, GL_UNSIGNED_BYTE, pixels );
+
+// delete[] pixels;
+
+ // draw a check into a bitmap, then load that into a texture
int i, j;
for (i = 0; i < checkImageHeight; i++)
for (j = 0; j < checkImageWidth; j++)
{
- int even = (i+j)%2;
- checkImage[i][j][0] = (GLubyte) 255 - 10*even;
- checkImage[i][j][1] = (GLubyte) 255 - 10*even;
- checkImage[i][j][2] = (GLubyte) 255;// - 5*even;
- checkImage[i][j][3] = 255;
+ int even = (i+j)%2;
+ checkImage[i][j][0] = (GLubyte) 255 - 10*even;
+ checkImage[i][j][1] = (GLubyte) 255 - 10*even;
+ checkImage[i][j][2] = (GLubyte) 255;// - 5*even;
+ checkImage[i][j][3] = 255;
}
-
- glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
- glGenTextures(1, &texName);
- glBindTexture(GL_TEXTURE_2D, texName);
- glEnable(GL_TEXTURE_2D);
+ glGenTextures(1, &checkTex );
+ glBindTexture(GL_TEXTURE_2D, checkTex);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, checkImageWidth, checkImageHeight,
- 0, GL_RGBA, GL_UNSIGNED_BYTE, checkImage);
+ 0, GL_RGBA, GL_UNSIGNED_BYTE, checkImage);
glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
-
- // fl_font( FL_HELVETICA, 16 );
-
+
init_done = true;
}
@@ -574,7 +593,7 @@ void Canvas::DrawGlobalGrid()
*/
char str[64];
- PushColor( 0.15, 0.15, 0.15, 1.0 ); // pale gray
+ PushColor( 0.2, 0.2, 0.2, 1.0 ); // pale gray
for( double i = ceil(bounds.x.min); i < bounds.x.max; i++)
{
snprintf( str, 16, "%d", (int)i );
@@ -595,7 +614,7 @@ void Canvas::DrawGlobalGrid()
glDisable(GL_BLEND);
glEnable(GL_TEXTURE_2D);
- glBindTexture(GL_TEXTURE_2D, texName);
+ glBindTexture(GL_TEXTURE_2D, checkTex );
glColor3f( 1.0, 1.0, 1.0 );
glBegin(GL_QUADS);
@@ -612,24 +631,6 @@ void Canvas::DrawGlobalGrid()
glDisable(GL_TEXTURE_2D);
glEnable(GL_BLEND);
- // glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
- // glTranslatef( 0,0,0.1 );
- // glColor3f( 0, 1.0, 0 );
-
- // glBegin( GL_LINES );
- // for( float x=bounds.x.min; x<=bounds.x.max; x++ )
- // {
- // glVertex2f( ceil(x), bounds.y.min );
- // glVertex2f( ceil(x), bounds.y.max );
- // }
-
- // for( float y=bounds.y.min; y<=bounds.y.max; y++ )
- // {
- // glVertex2f( bounds.x.min, ceil(y) );
- // glVertex2f( bounds.x.max, ceil(y) );
- // }
- // glEnd();
-
glDisable(GL_POLYGON_OFFSET_FILL );
}
@@ -384,24 +384,37 @@ void Model::DrawStatus( Camera* cam )
{
DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 );
}
+
+// extern GLuint glowTex;
+// extern GLuint checkTex;
+
+// if( parent == NULL )
+// {
+// glBlendFunc(GL_SRC_COLOR, GL_ONE );
+// DrawImage( glowTex, cam, 1.0 );
+// glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
+// }
}
-void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width, double height )
+void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha,
+ double width, double height )
{
float yaw, pitch;
pitch = - cam->pitch();
yaw = - cam->yaw();
- float robotAngle = -rtod(pose.a);
+
+ float robotAngle = -rtod( GetGlobalPose().a);
glPolygonMode( GL_FRONT, GL_FILL );
glEnable(GL_TEXTURE_2D);
glBindTexture( GL_TEXTURE_2D, texture_id );
- glColor4f( 1.0, 1.0, 1.0, alpha );
+ glColor4f( 1.0, 1.0, 1.0, 1.0 );
glPushMatrix();
//position image above the robot
+ // TODO
glTranslatef( 0.0, 0.0, ModelHeight() + 0.3 );
// rotate to face screen
@@ -416,9 +429,19 @@ void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double wid
glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0, 0.25f );
glEnd();
- glPopMatrix();
glBindTexture( GL_TEXTURE_2D, 0 );
glDisable(GL_TEXTURE_2D);
+
+// glPolygonMode( GL_FRONT, GL_LINE );
+// glColor3f( 0,0,1 );
+// glBegin(GL_QUADS);
+// glVertex3f(-0.25f, 0, -0.25f );
+// glVertex3f( 0.25f, 0, -0.25f );
+// glVertex3f( 0.25f, 0, 0.25f );
+// glVertex3f(-0.25f, 0, 0.25f );
+// glEnd();
+
+ glPopMatrix();
}
View
@@ -873,7 +873,7 @@ namespace Stg
// todo - test performance of std::set
std::vector<ModelPtrVec > update_lists;
- long unsigned int updates; ///< the number of simulated time steps executed so far
+ uint64_t updates; ///< the number of simulated time steps executed so far
Worldfile* wf; ///< If set, points to the worldfile used to create this world
void CallUpdateCallbacks(); ///< Call all calbacks in cb_list, removing any that return true;
@@ -1070,7 +1070,7 @@ namespace Stg
stg_bounds3d_t GetExtent(){ return extent; };
/** Return the number of times the world has been updated. */
- long unsigned int GetUpdateCount() { return updates; }
+ uint64_t GetUpdateCount() { return updates; }
/// Register an Option for pickup by the GUI
void RegisterOption( Option* opt );
@@ -1427,8 +1427,18 @@ namespace Stg
Fl_Menu_Bar* mbar;
OptionsDlg* oDlg;
bool pause_time;
- stg_usec_t real_time_of_last_update;
-
+
+ /** The amount of real time elapsed between $timing_interval
+ timesteps. */
+ stg_usec_t real_time_interval;
+
+ /** The last recorded real time, sampled every $timing_interval
+ updates. */
+ stg_usec_t real_time_recorded;
+
+ /** Number of updates between measuring elapsed real time. */
+ uint64_t timing_interval;
+
// static callback functions
static void UpdateCallback( WorldGui* wg );
static void windowCb( Fl_Widget* w, WorldGui* wg );
@@ -1829,11 +1839,11 @@ namespace Stg
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;
+
+ /** Number of world updates between trail records. */
+ uint64_t trail_interval;
+
+ stg_model_type_t type;
/** The index into the world's vector of event queues. Initially
-1, to indicate that it is not on a list yet. */
int event_queue_num;
View
@@ -189,17 +189,14 @@ WorldGui::WorldGui(int W,int H,const char* L) :
canvas( new Canvas( this,0,30,W,H-30 ) ),
drawOptions(),
fileMan( new FileManager() ),
- interval_log(),
speedup(1.0), // real time
mbar( new Fl_Menu_Bar(0,0, W, 30)),
oDlg( NULL ),
pause_time( false ),
- real_time_of_last_update( RealTimeNow() )
+ real_time_interval( sim_interval ),
+ real_time_recorded( RealTimeNow() ),
+ timing_interval( 20 )
{
- interval_log.resize(16);
- for( unsigned int i=0; i<interval_log.size(); i++ )
- interval_log[i] = sim_interval;
-
Fl::scheme( "gtk+" );
resizable(canvas);
@@ -216,9 +213,6 @@ WorldGui::WorldGui(int W,int H,const char* L) :
mbar->add( "File/&Save World", FL_CTRL + 's', (Fl_Callback*)fileSaveCb, this );
mbar->add( "File/Save World &As...", FL_CTRL + FL_SHIFT + 's', (Fl_Callback*)WorldGui::fileSaveAsCb, this, FL_MENU_DIVIDER );
- //mbar->add( "File/Screenshots", 0,0,0, FL_SUBMENU );
- //mbar->add( "File/Screenshots/Save Frames, (Fl_Callback*)fileScreenshotSaveCb, this,FL_MENU_TOGGLE );
-
mbar->add( "File/E&xit", FL_CTRL+'q', (Fl_Callback*)fileExitCb, this );
mbar->add( "&View", 0, 0, 0, FL_SUBMENU );
@@ -345,11 +339,15 @@ bool WorldGui::Update()
//printf( "speedup %.2f timeout %.6f\n", speedup, timeout );
- stg_usec_t timenow = RealTimeNow();
- stg_usec_t interval = timenow - real_time_of_last_update;
- interval_log[updates%interval_log.size()] = interval;
- real_time_of_last_update = timenow;
-
+ // occasionally we measure the real time elapsing, for reporting the
+ // run speed
+ if( updates % timing_interval == 0 )
+ {
+ stg_usec_t timenow = RealTimeNow();
+ real_time_interval = timenow - real_time_recorded;
+ real_time_recorded = timenow;
+ }
+
// inherit
bool done = World::Update();
@@ -366,13 +364,7 @@ std::string WorldGui::ClockString()
{
std::string str = World::ClockString();
- // find the average length of the last few realtime intervals;
- stg_usec_t average_real_interval = 0;
- for( uint32_t i=0; i<interval_log.size(); i++ )
- average_real_interval += interval_log[i];
- average_real_interval /= interval_log.size();
-
- double localratio = (double)sim_interval / (double)average_real_interval;
+ double localratio = (double)sim_interval / (double)(real_time_interval/timing_interval);
char buf[32];
snprintf( buf, 32, " [%.1f]", localratio );
@@ -13,7 +13,7 @@ speedup -1 # as fast as possible
paused 1
# threads may help or hurt performance depending on your worldfile, machine and load
-# threads 7
+ threads 7
# quit_time 60
View
@@ -11,8 +11,8 @@ paused 1
# time to pause (in GUI mode) or quit (in headless mode) the simulation
-# quit_time 3600 # 1 hour of simulated time
-quit_time 360 # 6 minutes of simulated time
+quit_time 3600 # 1 hour of simulated time
+
resolution 0.02
View
@@ -1,58 +0,0 @@
-#
-# Four vision-based puck-foraging robots
-# Works nicely with Player's examples/c++/forage
-#
-# $Id: puck.world,v 1.6 2002-11-11 08:30:20 rtv Exp $
-#
-
-#resolution 0.025
-
-gui
-(
- size [561.000 573.000 ]
- origin [5.092 5.057]
- scale 0.018
- grid [0.2 1.0]
- showgrid 1
-)
-
-bitmap
-(
- file "rink.pnm.gz"
- resolution 0.0125
-)
-
-define redpuck puck ( color "red" friction 0.035 )
-
-define forager position
-(
- size [0.4 0.4]
- shape "circle"
- gps()
- gripper( consume "true" pose [0.16 0 0])
- sonar()
- ptz( vision() )
-)
-
-redpuck ( pose [7.27665042946 2.72334957055 0] )
-redpuck ( pose [7.27665042946 4.24111652352 0] )
-redpuck ( pose [2.72334957055 7.27665042946 0] )
-redpuck ( pose [7.27665042946 5.75888347649 0] )
-redpuck ( pose [2.72334957055 4.24111652352 0] )
-redpuck ( pose [2.72334957055 5.75888347649 0] )
-redpuck ( pose [4.24111652352 7.27665042946 0] )
-redpuck ( pose [2.72334957055 2.72334957055 0] )
-redpuck ( pose [4.24111652352 4.24111652352 0] )
-redpuck ( pose [4.24111652352 5.75888347649 0] )
-redpuck ( pose [5.75888347649 7.27665042946 0] )
-redpuck ( pose [5.75888347649 2.72334957055 0] )
-redpuck ( pose [4.24111652352 2.72334957055 0] )
-redpuck ( pose [5.75888347649 4.24111652352 0] )
-redpuck ( pose [5.75888347649 5.75888347649 0] )
-redpuck ( pose [7.27665042946 7.27665042946 0] )
-
-forager( pose [3.5 7.0 0] port 6665 )
-forager( pose [7.0 3.5 0] port 6666 )
-forager( pose [3.5 3.5 0] port 6667 )
-forager( pose [7.0 7.0 0] port 6668 )
-
Oops, something went wrong.

0 comments on commit c69a1c9

Please sign in to comment.