Skip to content
This repository
Browse code

more efficient real-time tracking

  • Loading branch information...
commit c69a1c9a4ef6dbffd796b211ac112cb26a5fb52b 1 parent 850944c
authored
73  libstage/canvas.cc
@@ -30,11 +30,13 @@ using namespace Stg;
30 30
 static  const int checkImageWidth = 2;
31 31
 static  const int	checkImageHeight = 2;
32 32
 static  GLubyte checkImage[checkImageHeight][checkImageWidth][4];
33  
-static  GLuint texName;
34 33
 static bool blur = true;
35 34
 
36 35
 static bool init_done = false;
37 36
 
  37
+//GLuint glowTex;
  38
+GLuint checkTex;
  39
+
38 40
 void Canvas::TimerCallback( Canvas* c )
39 41
 {
40 42
   if( c->world->dirty )
@@ -149,23 +151,42 @@ void Canvas::InitGl()
149 151
   GLuint mains_id = TextureManager::getInstance().loadTexture( fullpath.c_str() );
150 152
   TextureManager::getInstance()._mains_texture_id = mains_id;
151 153
   
152  
-  //TODO merge this code into the textureManager?
  154
+//   // generate a small glow texture
  155
+//   GLubyte* pixels = new GLubyte[ 4 * 128 * 128 ];
  156
+
  157
+//   for( int x=0; x<128; x++ )
  158
+//  	 for( int y=0; y<128; y++ )
  159
+//  		{		  
  160
+//  		  GLubyte* p = &pixels[ 4 * (128*y + x)];
  161
+//  		  p[0] = (GLubyte)255; // red
  162
+//  		  p[1] = (GLubyte)0; // green
  163
+//  		  p[2] = (GLubyte)0; // blue
  164
+//  		  p[3] = (GLubyte)128; // alpha
  165
+//  		}
  166
+
  167
+
  168
+//   glGenTextures(1, &glowTex );
  169
+//   glBindTexture( GL_TEXTURE_2D, glowTex );
  170
+  
  171
+//   glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, 128, 128, 0, 
  172
+// 					 GL_RGBA, GL_UNSIGNED_BYTE, pixels );
  173
+
  174
+//   delete[] pixels;
  175
+
  176
+  // draw a check into a bitmap, then load that into a texture
153 177
   int i, j;
154 178
   for (i = 0; i < checkImageHeight; i++) 
155 179
     for (j = 0; j < checkImageWidth; j++) 
156 180
       {			
157  
-	int even = (i+j)%2;
158  
-	checkImage[i][j][0] = (GLubyte) 255 - 10*even;
159  
-	checkImage[i][j][1] = (GLubyte) 255 - 10*even;
160  
-	checkImage[i][j][2] = (GLubyte) 255;// - 5*even;
161  
-	checkImage[i][j][3] = 255;
  181
+		  int even = (i+j)%2;
  182
+		  checkImage[i][j][0] = (GLubyte) 255 - 10*even;
  183
+		  checkImage[i][j][1] = (GLubyte) 255 - 10*even;
  184
+		  checkImage[i][j][2] = (GLubyte) 255;// - 5*even;
  185
+		  checkImage[i][j][3] = 255;
162 186
       }
163 187
   
164  
-  
165  
-  glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
166  
-  glGenTextures(1, &texName);		 
167  
-  glBindTexture(GL_TEXTURE_2D, texName);
168  
-  glEnable(GL_TEXTURE_2D);
  188
+  glGenTextures(1, &checkTex );		 
  189
+  glBindTexture(GL_TEXTURE_2D, checkTex);
169 190
   
170 191
   glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
171 192
   glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
@@ -173,12 +194,10 @@ void Canvas::InitGl()
173 194
   glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
174 195
   
175 196
   glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, checkImageWidth, checkImageHeight, 
176  
-	       0, GL_RGBA, GL_UNSIGNED_BYTE, checkImage);
  197
+					0, GL_RGBA, GL_UNSIGNED_BYTE, checkImage);
177 198
   
178 199
   glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
179  
-
180  
-  // fl_font( FL_HELVETICA, 16 );
181  
-
  200
+    
182 201
   init_done = true; 
183 202
 }
184 203
 
@@ -574,7 +593,7 @@ void Canvas::DrawGlobalGrid()
574 593
   */
575 594
   
576 595
   char str[64];	
577  
-  PushColor( 0.15, 0.15, 0.15, 1.0 ); // pale gray
  596
+  PushColor( 0.2, 0.2, 0.2, 1.0 ); // pale gray
578 597
   for( double i = ceil(bounds.x.min); i < bounds.x.max; i++)
579 598
     {
580 599
       snprintf( str, 16, "%d", (int)i );
@@ -595,7 +614,7 @@ void Canvas::DrawGlobalGrid()
595 614
   glDisable(GL_BLEND);
596 615
 
597 616
   glEnable(GL_TEXTURE_2D);
598  
-  glBindTexture(GL_TEXTURE_2D, texName);
  617
+  glBindTexture(GL_TEXTURE_2D, checkTex );
599 618
   glColor3f( 1.0, 1.0, 1.0 );
600 619
 
601 620
   glBegin(GL_QUADS);
@@ -612,24 +631,6 @@ void Canvas::DrawGlobalGrid()
612 631
   glDisable(GL_TEXTURE_2D);
613 632
   glEnable(GL_BLEND);
614 633
   
615  
-  //   glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
616  
-  //   glTranslatef( 0,0,0.1 );
617  
-  //   glColor3f( 0, 1.0, 0 );
618  
-
619  
-  //   glBegin( GL_LINES );
620  
-  //   for( float x=bounds.x.min; x<=bounds.x.max; x++ )
621  
-  // 	 {
622  
-  // 		glVertex2f( ceil(x), bounds.y.min );
623  
-  // 		glVertex2f( ceil(x), bounds.y.max );
624  
-  // 	 }
625  
-  
626  
-  //   	 for( float y=bounds.y.min; y<=bounds.y.max; y++ )
627  
-  // 	 {
628  
-  // 		glVertex2f(  bounds.x.min, ceil(y) );
629  
-  // 		glVertex2f(  bounds.x.max, ceil(y) );
630  
-  // 	 }
631  
-  //   glEnd();
632  
-
633 634
   glDisable(GL_POLYGON_OFFSET_FILL );
634 635
 }
635 636
 
31  libstage/model_draw.cc
@@ -384,24 +384,37 @@ void Model::DrawStatus( Camera* cam )
384 384
     {
385 385
       DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 );
386 386
     }
  387
+  
  388
+//   extern GLuint glowTex;
  389
+//   extern GLuint checkTex;
  390
+  
  391
+//   if( parent == NULL )
  392
+//  	 {
  393
+//  		glBlendFunc(GL_SRC_COLOR, GL_ONE );	
  394
+//  		DrawImage( glowTex, cam, 1.0 );
  395
+//  		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
  396
+//  	 }
387 397
 }
388 398
 
389  
-void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width, double height )
  399
+void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, 
  400
+							  double width, double height )
390 401
 {
391 402
   float yaw, pitch;
392 403
   pitch = - cam->pitch();
393 404
   yaw = - cam->yaw();
394  
-  float robotAngle = -rtod(pose.a);
  405
+
  406
+  float robotAngle = -rtod( GetGlobalPose().a);
395 407
 
396 408
   glPolygonMode( GL_FRONT, GL_FILL );
397 409
 
398 410
   glEnable(GL_TEXTURE_2D);
399 411
   glBindTexture( GL_TEXTURE_2D, texture_id );
400 412
 
401  
-  glColor4f( 1.0, 1.0, 1.0, alpha );
  413
+  glColor4f( 1.0, 1.0, 1.0, 1.0 );
402 414
   glPushMatrix();
403 415
 
404 416
   //position image above the robot
  417
+  // TODO 
405 418
   glTranslatef( 0.0, 0.0, ModelHeight() + 0.3 );
406 419
 
407 420
   // rotate to face screen
@@ -416,9 +429,19 @@ void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double wid
416 429
   glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0,  0.25f );
417 430
   glEnd();
418 431
 
419  
-  glPopMatrix();
420 432
   glBindTexture( GL_TEXTURE_2D, 0 );
421 433
   glDisable(GL_TEXTURE_2D);
  434
+
  435
+//   glPolygonMode( GL_FRONT, GL_LINE );
  436
+//   glColor3f( 0,0,1 );
  437
+//   glBegin(GL_QUADS);
  438
+//   glVertex3f(-0.25f, 0, -0.25f );
  439
+//   glVertex3f( 0.25f, 0, -0.25f );
  440
+//   glVertex3f( 0.25f, 0,  0.25f );
  441
+//   glVertex3f(-0.25f, 0,  0.25f );
  442
+//   glEnd();
  443
+
  444
+  glPopMatrix();
422 445
 }
423 446
 
424 447
 
28  libstage/stage.hh
@@ -873,7 +873,7 @@ namespace Stg
873 873
 	// todo - test performance of std::set
874 874
 	std::vector<ModelPtrVec > update_lists;  
875 875
 	 
876  
-    long unsigned int updates; ///< the number of simulated time steps executed so far
  876
+    uint64_t updates; ///< the number of simulated time steps executed so far
877 877
     Worldfile* wf; ///< If set, points to the worldfile used to create this world
878 878
 
879 879
 	 void CallUpdateCallbacks(); ///< Call all calbacks in cb_list, removing any that return true;
@@ -1070,7 +1070,7 @@ namespace Stg
1070 1070
     stg_bounds3d_t GetExtent(){ return extent; };
1071 1071
   
1072 1072
     /** Return the number of times the world has been updated. */
1073  
-    long unsigned int GetUpdateCount() { return updates; }
  1073
+    uint64_t GetUpdateCount() { return updates; }
1074 1074
 
1075 1075
 	 /// Register an Option for pickup by the GUI
1076 1076
 	 void RegisterOption( Option* opt );	
@@ -1427,8 +1427,18 @@ namespace Stg
1427 1427
     Fl_Menu_Bar* mbar;
1428 1428
     OptionsDlg* oDlg;
1429 1429
     bool pause_time;
1430  
-    stg_usec_t real_time_of_last_update;
1431  
-    
  1430
+
  1431
+	 /** The amount of real time elapsed between $timing_interval
  1432
+		  timesteps. */
  1433
+    stg_usec_t real_time_interval;
  1434
+	 
  1435
+	 /** The last recorded real time, sampled every $timing_interval
  1436
+		  updates. */
  1437
+    stg_usec_t real_time_recorded;
  1438
+	 
  1439
+	 /** Number of updates between measuring elapsed real time. */
  1440
+	 uint64_t timing_interval;
  1441
+
1432 1442
     // static callback functions
1433 1443
     static void UpdateCallback( WorldGui* wg );
1434 1444
     static void windowCb( Fl_Widget* w, WorldGui* wg );	
@@ -1829,11 +1839,11 @@ namespace Stg
1829 1839
 				be set in the world file using the tail_length model
1830 1840
 				property. */
1831 1841
 		unsigned int trail_length;
1832  
-
1833  
-		/** Number of world updates between trail records. */
1834  
-		long unsigned int trail_interval;
1835  
-
1836  
-		stg_model_type_t type;  
  1842
+	 
  1843
+	 /** Number of world updates between trail records. */
  1844
+	 uint64_t trail_interval;
  1845
+	 
  1846
+	 stg_model_type_t type;  
1837 1847
 	 /** The index into the world's vector of event queues. Initially
1838 1848
 		  -1, to indicate that it is not on a list yet. */
1839 1849
 	 int event_queue_num; 
34  libstage/worldgui.cc
@@ -189,17 +189,14 @@ WorldGui::WorldGui(int W,int H,const char* L) :
189 189
   canvas( new Canvas( this,0,30,W,H-30 ) ),
190 190
   drawOptions(),
191 191
   fileMan( new FileManager() ),
192  
-  interval_log(),
193 192
   speedup(1.0), // real time
194 193
   mbar( new Fl_Menu_Bar(0,0, W, 30)),
195 194
   oDlg( NULL ),
196 195
   pause_time( false ),	
197  
-  real_time_of_last_update( RealTimeNow() )
  196
+  real_time_interval( sim_interval ),
  197
+  real_time_recorded( RealTimeNow() ),
  198
+  timing_interval( 20 )
198 199
 {
199  
-  interval_log.resize(16);
200  
-  for( unsigned int i=0; i<interval_log.size(); i++ )
201  
-  	 interval_log[i] = sim_interval;
202  
-  
203 200
   Fl::scheme( "gtk+" );
204 201
   resizable(canvas);
205 202
   
@@ -216,9 +213,6 @@ WorldGui::WorldGui(int W,int H,const char* L) :
216 213
   mbar->add( "File/&Save World", FL_CTRL + 's', (Fl_Callback*)fileSaveCb, this );
217 214
   mbar->add( "File/Save World &As...", FL_CTRL + FL_SHIFT + 's', (Fl_Callback*)WorldGui::fileSaveAsCb, this, FL_MENU_DIVIDER );
218 215
   
219  
-  //mbar->add( "File/Screenshots", 0,0,0, FL_SUBMENU );
220  
-  //mbar->add( "File/Screenshots/Save Frames, (Fl_Callback*)fileScreenshotSaveCb, this,FL_MENU_TOGGLE );
221  
-  
222 216
   mbar->add( "File/E&xit", FL_CTRL+'q', (Fl_Callback*)fileExitCb, this );
223 217
   
224 218
   mbar->add( "&View", 0, 0, 0, FL_SUBMENU );
@@ -345,11 +339,15 @@ bool WorldGui::Update()
345 339
 
346 340
   //printf( "speedup %.2f timeout %.6f\n", speedup, timeout );
347 341
   
348  
-  stg_usec_t timenow = RealTimeNow();	 
349  
-  stg_usec_t interval = timenow - real_time_of_last_update; 
350  
-  interval_log[updates%interval_log.size()] =  interval;  
351  
-  real_time_of_last_update = timenow;
352  
-   
  342
+  // occasionally we measure the real time elapsing, for reporting the
  343
+  // run speed
  344
+  if( updates % timing_interval == 0 )
  345
+	 { 
  346
+		stg_usec_t timenow = RealTimeNow();	 
  347
+		real_time_interval = timenow - real_time_recorded; 
  348
+		real_time_recorded = timenow;
  349
+	 }   
  350
+
353 351
   // inherit
354 352
   bool done = World::Update();
355 353
   
@@ -366,13 +364,7 @@ std::string WorldGui::ClockString()
366 364
 {
367 365
   std::string str = World::ClockString();
368 366
   
369  
-  // find the average length of the last few realtime intervals;
370  
-  stg_usec_t average_real_interval = 0;
371  
-  for( uint32_t i=0; i<interval_log.size(); i++ )
372  
-    average_real_interval += interval_log[i];
373  
-  average_real_interval /= interval_log.size();
374  
-  
375  
-  double localratio = (double)sim_interval / (double)average_real_interval;
  367
+  double localratio = (double)sim_interval / (double)(real_time_interval/timing_interval);
376 368
   
377 369
   char buf[32];
378 370
   snprintf( buf, 32, " [%.1f]", localratio );
2  worlds/benchmark/hospital.world
@@ -13,7 +13,7 @@ speedup -1 # as fast as possible
13 13
 paused 1
14 14
 
15 15
 # threads may help or hurt performance depending on your worldfile, machine and load
16  
-# threads 7
  16
+ threads 7
17 17
 
18 18
 # quit_time 60
19 19
 
4  worlds/fasr.world
@@ -11,8 +11,8 @@ paused 1
11 11
 
12 12
 # time to pause (in GUI mode) or quit (in headless mode) the simulation
13 13
 
14  
-# quit_time 3600 # 1 hour of simulated time
15  
-quit_time 360 # 6 minutes of simulated time
  14
+quit_time 3600 # 1 hour of simulated time
  15
+
16 16
 
17 17
 resolution 0.02
18 18
 
58  worlds/puck.world
... ...
@@ -1,58 +0,0 @@
1  
-#
2  
-# Four vision-based puck-foraging robots 
3  
-# Works nicely with Player's examples/c++/forage 
4  
-#
5  
-# $Id: puck.world,v 1.6 2002-11-11 08:30:20 rtv Exp $
6  
-#
7  
-
8  
-#resolution 0.025
9  
-
10  
-gui
11  
-(
12  
-  size [561.000 573.000 ]
13  
-  origin [5.092 5.057]
14  
-  scale 0.018
15  
-  grid [0.2 1.0]
16  
-  showgrid 1
17  
-)
18  
-
19  
-bitmap
20  
-(
21  
-   file "rink.pnm.gz"
22  
-   resolution 0.0125
23  
-)
24  
-
25  
-define redpuck puck ( color "red" friction 0.035  )
26  
-
27  
-define forager position
28  
-(
29  
-  size [0.4 0.4] 
30  
-  shape "circle" 
31  
-  gps()
32  
-  gripper( consume "true" pose [0.16 0 0])
33  
-  sonar()
34  
-  ptz( vision() )
35  
-)
36  
-
37  
-redpuck ( pose [7.27665042946 2.72334957055 0] )
38  
-redpuck ( pose [7.27665042946 4.24111652352 0] )
39  
-redpuck ( pose [2.72334957055 7.27665042946 0] )
40  
-redpuck ( pose [7.27665042946 5.75888347649 0] )
41  
-redpuck ( pose [2.72334957055 4.24111652352 0] )
42  
-redpuck ( pose [2.72334957055 5.75888347649 0] )
43  
-redpuck ( pose [4.24111652352 7.27665042946 0] )
44  
-redpuck ( pose [2.72334957055 2.72334957055 0] )
45  
-redpuck ( pose [4.24111652352 4.24111652352 0] )
46  
-redpuck ( pose [4.24111652352 5.75888347649 0] )
47  
-redpuck ( pose [5.75888347649 7.27665042946 0] )
48  
-redpuck ( pose [5.75888347649 2.72334957055 0] )
49  
-redpuck ( pose [4.24111652352 2.72334957055 0] )
50  
-redpuck ( pose [5.75888347649 4.24111652352 0] )
51  
-redpuck ( pose [5.75888347649 5.75888347649 0] )
52  
-redpuck ( pose [7.27665042946 7.27665042946 0] )
53  
-
54  
-forager( pose [3.5 7.0 0] port 6665 )
55  
-forager( pose [7.0 3.5 0] port 6666 )
56  
-forager( pose [3.5 3.5 0] port 6667 )
57  
-forager( pose [7.0 7.0 0] port 6668 )
58  
-
29  worlds/pucktarget.inc
... ...
@@ -1,29 +0,0 @@
1  
-
2  
-define pucktarget fiducialfinder
3  
-(
4  
-	range_min 0.0
5  
-	range_max 1.5
6  
-	fov 360.0
7  
-	samples 2
8  
-	size [0.25 0.25] 
9  
-
10  
-	color "purple"
11  
-	#laser_return "bright"
12  
-
13  
-	# cos(18) = 0.9510565	sin(18) = 0.309017
14  
-	# cos(54) = 0.587785	sin(54) = 0.809017
15  
-	polygons 1
16  
-	polygon[0].points 10
17  
-	polygon[0].point[0] [  0.238  0.077 ]
18  
-	polygon[0].point[1] [  0.1    0.1 ]
19  
-	polygon[0].point[2] [  0      0.250 ]
20  
-	polygon[0].point[3] [ -0.1    0.1 ]
21  
-	polygon[0].point[4] [ -0.238  0.077 ]
22  
-	polygon[0].point[5] [ -0.1   -0.1 ]
23  
-	polygon[0].point[6] [ -0.147 -0.202 ]
24  
-	polygon[0].point[7] [  0.0   -0.1 ]
25  
-	polygon[0].point[8] [  0.147 -0.202 ]
26  
-	polygon[0].point[9] [  0.1   -0.1 ]
27  
-
28  
-)
29  
-

0 notes on commit c69a1c9

Please sign in to comment.
Something went wrong with that request. Please try again.