stage.hh
Go to the documentation of this file.
1 
2 #ifndef STG_H
3 #define STG_H
4 /*
5  * Stage : a multi-robot simulator. Part of the Player Project.
6  *
7  * Copyright (C) 2001-2009 Richard Vaughan, Brian Gerkey, Andrew
8  * Howard, Toby Collett, Reed Hedges, Alex Couture-Beil, Jeremy
9  * Asher, Pooya Karimian
10  *
11  * This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program; if not, write to the Free Software
23  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
24  *
25  */
26 
34 // C libs
35 #include <unistd.h>
36 #include <stdint.h> // for portable int types eg. uint32_t
37 #include <assert.h>
38 #include <stdlib.h>
39 #include <stdio.h>
40 #include <libgen.h>
41 #include <string.h>
42 #include <sys/types.h>
43 #include <sys/time.h>
44 #include <pthread.h>
45 
46 // C++ libs
47 #include <cmath>
48 #include <iostream>
49 #include <vector>
50 #include <list>
51 #include <map>
52 #include <set>
53 #include <queue>
54 #include <algorithm>
55 
56 // FLTK Gui includes
57 #include <FL/Fl.H>
58 #include <FL/Fl_Box.H>
59 #include <FL/Fl_Gl_Window.H>
60 #include <FL/Fl_Menu_Bar.H>
61 #include <FL/Fl_Window.H>
62 #include <FL/fl_draw.H>
63 #include <FL/gl.h> // FLTK takes care of platform-specific GL stuff
64 // except GLU
65 #ifdef __APPLE__
66 #include <OpenGL/glu.h>
67 #else
68 #include <GL/glu.h>
69 #endif
70 
72 namespace Stg
73 {
74  // forward declare
75  class Block;
76  class Canvas;
77  class Cell;
78  class Worldfile;
79  class World;
80  class WorldGui;
81  class Model;
82  class OptionsDlg;
83  class Camera;
84  class FileManager;
85  class Option;
86 
87  typedef Model* (*creator_t)( World*, Model*, const std::string& type );
88 
91  void Init( int* argc, char** argv[] );
92 
94  bool InitDone();
95 
98  const char* Version();
99 
101  const char COPYRIGHT[] =
102  "Copyright Richard Vaughan and contributors 2000-2009";
103 
105  const char AUTHORS[] =
106  "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, Toby Collett, Jeremy Asher, Alex Couture-Beil and contributors.";
107 
109  const char WEBSITE[] = "http://playerstage.org";
110 
112  const char DESCRIPTION[] =
113  "Robot simulation library\nPart of the Player Project";
114 
116  const char LICENSE[] =
117  "Stage robot simulation library\n" \
118  "Copyright (C) 2000-2009 Richard Vaughan and contributors\n" \
119  "Part of the Player Project [http://playerstage.org]\n" \
120  "\n" \
121  "This program is free software; you can redistribute it and/or\n" \
122  "modify it under the terms of the GNU General Public License\n" \
123  "as published by the Free Software Foundation; either version 2\n" \
124  "of the License, or (at your option) any later version.\n" \
125  "\n" \
126  "This program is distributed in the hope that it will be useful,\n" \
127  "but WITHOUT ANY WARRANTY; without even the implied warranty of\n" \
128  "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n" \
129  "GNU General Public License for more details.\n" \
130  "\n" \
131  "You should have received a copy of the GNU General Public License\n" \
132  "along with this program; if not, write to the Free Software\n" \
133  "Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.\n" \
134  "\n" \
135  "The text of the license may also be available online at\n" \
136  "http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";
137 
139  const double thousand = 1e3;
140 
142  const double million = 1e6;
143 
145  const double billion = 1e9;
146 
148  inline double rtod( double r ){ return( r*180.0/M_PI ); }
149 
151  inline double dtor( double d ){ return( d*M_PI/180.0 ); }
152 
154  inline double normalize( double a )
155  {
156  while( a < -M_PI ) a += 2.0*M_PI;
157  while( a > M_PI ) a -= 2.0*M_PI;
158  return a;
159  };
160 
162  inline int sgn( int a){ return( a<0 ? -1 : 1); }
163 
165  inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); }
166 
168  enum { FiducialNone = 0 };
169 
171  typedef uint32_t id_t;
172 
174  typedef double meters_t;
175 
177  typedef double radians_t;
178 
180  typedef struct timeval time_t;
181 
183  typedef unsigned long msec_t;
184 
186  typedef uint64_t usec_t;
187 
189  typedef double kg_t; // Kilograms (mass)
190 
192  typedef double joules_t;
193 
195  typedef double watts_t;
196 
197  class Color
198  {
199  public:
200  double r,g,b,a;
201 
202  Color( double r, double g, double b, double a=1.0 );
203 
207  Color( const std::string& name );
208 
209  Color();
210 
211  bool operator!=( const Color& other ) const;
212  bool operator==( const Color& other ) const;
213  static Color RandomColor();
214  void Print( const char* prefix ) const;
215 
217  static const Color blue, red, green, yellow, magenta, cyan;
218 
219  const Color& Load( Worldfile* wf, int entity );
220 
221  void GLSet( void ) { glColor4f( r,g,b,a ); }
222  };
223 
225  class Size
226  {
227  public:
228  meters_t x, y, z;
229 
230  Size( meters_t x,
231  meters_t y,
232  meters_t z )
233  : x(x), y(y), z(z)
234  {/*empty*/}
235 
237  Size() : x( 0.4 ), y( 0.4 ), z( 1.0 )
238  {/*empty*/}
239 
240  Size& Load( Worldfile* wf, int section, const char* keyword );
241  void Save( Worldfile* wf, int section, const char* keyword ) const;
242 
243  void Zero()
244  { x=y=z=0.0; }
245  };
246 
248  class Pose
249  {
250  public:
251  meters_t x, y, z;
252  radians_t a;
253 
254  Pose( meters_t x,
255  meters_t y,
256  meters_t z,
257  radians_t a )
258  : x(x), y(y), z(z), a(a)
259  { /*empty*/ }
260 
261  Pose() : x(0.0), y(0.0), z(0.0), a(0.0)
262  { /*empty*/ }
263 
264  virtual ~Pose(){};
265 
268  static Pose Random( meters_t xmin, meters_t xmax,
269  meters_t ymin, meters_t ymax )
270  {
271  return Pose( xmin + drand48() * (xmax-xmin),
272  ymin + drand48() * (ymax-ymin),
273  0,
274  normalize( drand48() * (2.0 * M_PI) ));
275  }
276 
280  virtual void Print( const char* prefix ) const
281  {
282  printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
283  prefix, x,y,z,a );
284  }
285 
286  std::string String() const
287  {
288  char buf[256];
289  snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]",
290  x,y,z,a );
291  return std::string(buf);
292  }
293 
294  /* returns true iff all components of the velocity are zero. */
295  bool IsZero() const
296  { return( !(x || y || z || a )); };
297 
299  void Zero()
300  { x=y=z=a=0.0; }
301 
302  Pose& Load( Worldfile* wf, int section, const char* keyword );
303  void Save( Worldfile* wf, int section, const char* keyword );
304 
305  inline Pose operator+( const Pose& p ) const
306  {
307  const double cosa = cos(a);
308  const double sina = sin(a);
309 
310  return Pose( x + p.x * cosa - p.y * sina,
311  y + p.x * sina + p.y * cosa,
312  z + p.z,
313  normalize(a + p.a) );
314  }
315 
316  // a < b iff a is closer to the origin than b
317  bool operator<( const Pose& other ) const
318  {
319  return( hypot( y, x ) < hypot( other.y, other.x ));
320  }
321 
322  bool operator==( const Pose& other ) const
323  {
324  return( x==other.x &&
325  y==other.y &&
326  z==other.z &&
327  a==other.a );
328  }
329 
330  bool operator!=( const Pose& other ) const
331  {
332  return( x!=other.x ||
333  y!=other.y ||
334  z!=other.z ||
335  a!=other.a );
336  }
337 
338  meters_t Distance2D( const Pose& other ) const
339  {
340  return hypot( x-other.x, y-other.y );
341  }
342 
343  };
344 
345 
348  class Velocity : public Pose
349  {
350  public:
356  Velocity( double x,
357  double y,
358  double z,
359  double a ) :
360  Pose( x, y, z, a )
361  { /*empty*/ }
362 
364  { /*empty*/ }
365 
372  Velocity& Load( Worldfile* wf, int section, const char* keyword )
373  {
374  Pose::Load( wf, section, keyword );
375  return *this;
376  }
377 
378  virtual void Print( const char* prefix ) const
379  {
380  if( prefix )
381  printf( "%s", prefix );
382 
383  printf( "velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
384  x,y,z,a );
385  }
386  };
387 
388 
391  class Geom
392  {
393  public:
396 
402  void Print( const char* prefix ) const
403  {
404  if( prefix )
405  printf( "%s", prefix );
406 
407  printf( "geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n",
408  pose.x,
409  pose.y,
410  pose.a,
411  size.x,
412  size.y );
413  }
414 
416  Geom() : pose(), size() {}
417 
419  Geom( const Pose& p, const Size& s ) : pose(p), size(s) {}
420 
421  void Zero()
422  {
423  pose.Zero();
424  size.Zero();
425  }
426  };
427 
429  class Bounds
430  {
431  public:
433  double min;
435  double max;
436 
437  Bounds() : min(0), max(0) { /* empty*/ }
438  Bounds( double min, double max ) : min(min), max(max) { /* empty*/ }
439 
440  Bounds& Load( Worldfile* wf, int section, const char* keyword );
441 
442  // returns value, but no smaller than min and no larger than max.
443  double Constrain( double value );
444  };
445 
448  {
449  public:
456 
457  bounds3d_t() : x(), y(), z() {}
458  bounds3d_t( const Bounds& x, const Bounds& y, const Bounds& z)
459  : x(x), y(y), z(z) {}
460  };
461 
463  typedef struct
464  {
466  radians_t angle;
467  } fov_t;
468 
470  class point_t
471  {
472  public:
473  meters_t x, y;
474  point_t( meters_t x, meters_t y ) : x(x), y(y){}
475  point_t() : x(0.0), y(0.0){}
476 
477  bool operator+=( const point_t& other )
478  { return ((x += other.x) && (y += other.y) ); }
479  };
480 
482  class point3_t
483  {
484  public:
485  meters_t x,y,z;
486  point3_t( meters_t x, meters_t y, meters_t z )
487  : x(x), y(y), z(z) {}
488 
489  point3_t() : x(0.0), y(0.0), z(0.0) {}
490  };
491 
494  {
495  public:
496  int x,y;
497  point_int_t( int x, int y ) : x(x), y(y){}
498  point_int_t() : x(0), y(0){}
499 
501  bool operator<( const point_int_t& other ) const
502  {
503  if( x < other.x ) return true;
504  if( other.x < x ) return false;
505  return y < other.y;
506  }
507 
508  bool operator==( const point_int_t& other ) const
509  { return ((x == other.x) && (y == other.y) ); }
510  };
511 
514  point_t* unit_square_points_create();
515 
518  namespace Gl
519  {
520  void pose_shift( const Pose &pose );
521  void pose_inverse_shift( const Pose &pose );
522  void coord_shift( double x, double y, double z, double a );
523  void draw_grid( bounds3d_t vol );
525  void draw_string( float x, float y, float z, const char *string);
526  void draw_string_multiline( float x, float y, float w, float h,
527  const char *string, Fl_Align align );
528  void draw_speech_bubble( float x, float y, float z, const char* str );
529  void draw_octagon( float w, float h, float m );
530  void draw_octagon( float x, float y, float w, float h, float m );
531  void draw_vector( double x, double y, double z );
532  void draw_origin( double len );
533  void draw_array( float x, float y, float w, float h,
534  float* data, size_t len, size_t offset,
535  float min, float max );
536  void draw_array( float x, float y, float w, float h,
537  float* data, size_t len, size_t offset );
539  void draw_centered_rect( float x, float y, float dx, float dy );
540  } // namespace Gl
541 
542  void RegisterModels();
543 
544 
546  class Visualizer {
547  private:
548  const std::string menu_name;
549  const std::string worldfile_name;
550 
551  public:
552  Visualizer( const std::string& menu_name,
553  const std::string& worldfile_name )
554  : menu_name( menu_name ),
555  worldfile_name( worldfile_name )
556  { }
557 
558  virtual ~Visualizer( void ) { }
559  virtual void Visualize( Model* mod, Camera* cam ) = 0;
560 
561  const std::string& GetMenuName() { return menu_name; }
562  const std::string& GetWorldfileName() { return worldfile_name; }
563  };
564 
565 
568  typedef int(*model_callback_t)(Model* mod, void* user );
569 
570  typedef int(*world_callback_t)(World* world, void* user );
571 
572  // return val, or minval if val < minval, or maxval if val > maxval
573  double constrain( double val, double minval, double maxval );
574 
575  typedef struct
576  {
577  int enabled;
579  meters_t size;
581  msec_t period;
582  double duty_cycle;
583  } blinkenlight_t;
584 
585 
587  typedef struct
588  {
591  } rotrect_t; // rotated rectangle
592 
597  int rotrects_from_image_file( const std::string& filename,
598  std::vector<rotrect_t>& rects );
599 
603  typedef bool (*ray_test_func_t)(Model* candidate,
604  Model* finder,
605  const void* arg );
606 
607  // STL container iterator macros - __typeof is a gcc extension, so
608  // this could be an issue one day.
609 #define VAR(V,init) __typeof(init) V=(init)
610 
611  //#define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();++I)
612 
613  // NOTE:
614  // this version assumes the container is not modified in the loop,
615  // which I think is true everywhere it is used in Stage
616 #define FOR_EACH(I,C) for(VAR(I,(C).begin()),ite=(C).end();(I)!=ite;++(I))
617 
620  template <class T, class C>
621  void EraseAll( T thing, C& cont )
622  { cont.erase( std::remove( cont.begin(), cont.end(), thing ), cont.end() ); }
623 
624  // Error macros - output goes to stderr
625 #define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
626 #define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
627 #define PRINT_ERR2(m,a,b) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
628 #define PRINT_ERR3(m,a,b,c) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
629 #define PRINT_ERR4(m,a,b,c,d) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
630 #define PRINT_ERR5(m,a,b,c,d,e) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)
631 
632  // Warning macros
633 #define PRINT_WARN(m) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
634 #define PRINT_WARN1(m,a) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
635 #define PRINT_WARN2(m,a,b) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
636 #define PRINT_WARN3(m,a,b,c) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
637 #define PRINT_WARN4(m,a,b,c,d) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
638 #define PRINT_WARN5(m,a,b,c,d,e) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)
639 
640  // Message macros
641 #ifdef DEBUG
642 #define PRINT_MSG(m) printf( "Stage: "m" (%s %s)\n", __FILE__, __FUNCTION__)
643 #define PRINT_MSG1(m,a) printf( "Stage: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
644 #define PRINT_MSG2(m,a,b) printf( "Stage: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
645 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
646 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
647 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m" (%s %s)\n", a, b, c, d, e,__FILE__, __FUNCTION__)
648 #else
649 #define PRINT_MSG(m) printf( "Stage: "m"\n" )
650 #define PRINT_MSG1(m,a) printf( "Stage: "m"\n", a)
651 #define PRINT_MSG2(m,a,b) printf( "Stage: "m"\n,", a, b )
652 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m"\n", a, b, c )
653 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m"\n", a, b, c, d )
654 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m"\n", a, b, c, d, e )
655 #endif
656 
657  // DEBUG macros
658 #ifdef DEBUG
659 #define PRINT_DEBUG(m) printf( "debug: "m" (%s %s)\n", __FILE__, __FUNCTION__)
660 #define PRINT_DEBUG1(m,a) printf( "debug: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
661 #define PRINT_DEBUG2(m,a,b) printf( "debug: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
662 #define PRINT_DEBUG3(m,a,b,c) printf( "debug: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
663 #define PRINT_DEBUG4(m,a,b,c,d) printf( "debug: "m" (%s %s)\n", a, b, c ,d, __FILE__, __FUNCTION__)
664 #define PRINT_DEBUG5(m,a,b,c,d,e) printf( "debug: "m" (%s %s)\n", a, b, c ,d, e, __FILE__, __FUNCTION__)
665 #else
666 #define PRINT_DEBUG(m)
667 #define PRINT_DEBUG1(m,a)
668 #define PRINT_DEBUG2(m,a,b)
669 #define PRINT_DEBUG3(m,a,b,c)
670 #define PRINT_DEBUG4(m,a,b,c,d)
671 #define PRINT_DEBUG5(m,a,b,c,d,e)
672 #endif
673 
674  class Block;
675  class Model;
676 
677 
678  // ANCESTOR CLASS
680  class Ancestor
681  {
682  friend class Canvas; // allow Canvas access to our private members
683 
684  protected:
685 
687  std::map<std::string,unsigned int> child_type_counts;
688 
689  std::vector<Model*> children;
690 
691  bool debug;
692 
694  std::map<std::string,void*> props;
695 
696  std::string token;
697 
698  Ancestor& Load( Worldfile* wf, int section );
699  void Save( Worldfile* wf, int section );
700 
701  public:
702  Ancestor();
703  virtual ~Ancestor();
704 
706  std::vector<Model*>& GetChildren(){ return children;}
707 
709  void ForEachDescendant( model_callback_t func, void* arg );
710 
711  virtual void AddChild( Model* mod );
712  virtual void RemoveChild( Model* mod );
713  virtual Pose GetGlobalPose() const;
714 
715  const char* Token() const { return token.c_str(); }
716  const std::string& TokenStr() const { return token; }
717 
718  virtual void SetToken( const std::string& str )
719  {
720  //printf( "Ancestor::SetToken( %s )\n", str.c_str() );
721 
722  if( str.size() > 0 )
723  token = str;
724  else
725  PRINT_ERR( "Ancestor::SetToken() called with zero length string. Ignored." );
726  }
727 
729  void SetProperty( std::string& key, void* value ){ props[ key ] = value; }
730 
732  void* GetProperty( std::string& key )
733  {
734  std::map<std::string,void*>::iterator it = props.find( key );
735  return( it == props.end() ? NULL : it->second );
736  }
737  };
738 
742  {
743  public:
745  meters_t range;
746  Model* mod;
748 
749  RaytraceResult() : pose(), range(0), mod(NULL), color() {}
750  RaytraceResult( const Pose& pose,
751  meters_t range )
752  : pose(pose), range(range), mod(NULL), color() {}
753  };
754 
755  class Ray
756  {
757  public:
758  Ray( const Model* mod, const Pose& origin, const meters_t range, const ray_test_func_t func, const void* arg, const bool ztest ) :
759  mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest)
760  {}
761 
762  Ray() : mod(NULL), origin(0,0,0,0), range(0), func(NULL), arg(NULL), ztest(true)
763  {}
764 
765  const Model* mod;
767  meters_t range;
769  const void* arg;
770  bool ztest;
771  };
772 
773 
774  // defined in stage_internal.hh
775  class Region;
776  class SuperRegion;
777  class BlockGroup;
778  class PowerPack;
779 
780  class LogEntry
781  {
782  usec_t timestamp;
783  Model* mod;
784  Pose pose;
785 
786  public:
787  LogEntry( usec_t timestamp, Model* mod );
788 
790  static std::vector<LogEntry> log;
791 
793  static size_t Count(){ return log.size(); }
794 
796  static void Clear(){ log.clear(); }
797 
799  static void Print();
800  };
801 
802  class CtrlArgs
803  {
804  public:
805  std::string worldfile;
806  std::string cmdline;
807 
808  CtrlArgs( std::string w, std::string c ) : worldfile(w), cmdline(c) {}
809  };
810 
811  class ModelPosition;
812 
814  class World : public Ancestor
815  {
816  public:
817  friend class Block;
818  friend class Model; // allow access to private members
819  friend class ModelFiducial;
820  friend class Canvas;
821  friend class WorkerThread;
822 
823  public:
826  static std::vector<std::string> args;
827  static std::string ctrlargs;
828 
829  private:
830 
831  static std::set<World*> world_set;
832  static bool quit_all;
833  static void UpdateCb( World* world);
834  static unsigned int next_id;
835 
836  bool destroy;
837  bool dirty;
838 
840  std::set<Model*> models;
841 
843  std::map<std::string, Model*> models_by_name;
844 
846  std::map<int,Model*> models_by_wfentity;
847 
850  std::vector<Model*> models_with_fiducials;
851 
852  struct ltx
853  {
854  bool operator()(const Model* a, const Model* b) const;
855  };
856 
857  struct lty
858  {
859  bool operator()(const Model* a, const Model* b) const;
860  };
861 
864  std::set<Model*,ltx> models_with_fiducials_byx;
865 
868  std::set<Model*,lty> models_with_fiducials_byy;
869 
871  void FiducialInsert( Model* mod )
872  {
873  FiducialErase( mod ); // make sure it's not there already
874  models_with_fiducials.push_back( mod );
875  }
876 
878  void FiducialErase( Model* mod )
879  {
880  EraseAll( mod, models_with_fiducials );
881  }
882 
883  double ppm;
884  bool quit;
885 
886  bool show_clock;
887  unsigned int show_clock_interval;
888 
889  pthread_mutex_t sync_mutex;
890  unsigned int threads_working;
891  pthread_cond_t threads_start_cond;
892  pthread_cond_t threads_done_cond;
893  int total_subs;
894  unsigned int worker_threads;
895 
896  protected:
897 
898  std::list<std::pair<world_callback_t,void*> > cb_list;
900  bool graphics;
901 
902  std::set<Option*> option_table;
903  std::list<PowerPack*> powerpack_list;
904 
905  usec_t quit_time;
906  std::list<float*> ray_list;
907  usec_t sim_time;
908  std::map<point_int_t,SuperRegion*> superregions;
909 
910  std::vector< std::vector<Model*> > update_lists;
911 
912  uint64_t updates;
914 
915  void CallUpdateCallbacks();
916 
917  public:
918 
919  uint64_t UpdateCount(){ return updates; }
920 
921  bool paused;
922 
923  virtual void Start(){ paused = false; };
924  virtual void Stop(){ paused = true; };
925  virtual void TogglePause(){ paused ? Start() : Stop(); };
926 
927  bool Paused() const { return( paused ); };
928 
932  virtual void Redraw( void ){ }; // does nothing
933 
934  std::vector<point_int_t> rt_cells;
935  std::vector<point_int_t> rt_candidate_cells;
936 
937  static const int DEFAULT_PPM = 50; // default resolution in pixels per meter
938 
941  void AddUpdateCallback( world_callback_t cb, void* user );
942 
945  int RemoveUpdateCallback( world_callback_t cb, void* user );
946 
948  void Log( Model* mod );
949 
951  void NeedRedraw(){ dirty = true; };
952 
954  Model* ground;
955 
958  virtual std::string ClockString( void ) const;
959 
960  Model* CreateModel( Model* parent, const std::string& typestr );
961  void LoadModel( Worldfile* wf, int entity );
962  void LoadBlock( Worldfile* wf, int entity );
963  void LoadBlockGroup( Worldfile* wf, int entity );
964 
965  void LoadSensor( Worldfile* wf, int entity );
966 
967  virtual Model* RecentlySelectedModel() const { return NULL; }
968 
970  void MapPoly( const std::vector<point_int_t>& poly,
971  Block* block,
972  unsigned int layer );
973 
974  SuperRegion* AddSuperRegion( const point_int_t& coord );
975  SuperRegion* GetSuperRegion( const point_int_t& org );
977  //void ExpireSuperRegion( SuperRegion* sr );
978 
981  int32_t MetersToPixels( meters_t x ) const
982  { return (int32_t)floor(x * ppm); };
983 
984  point_int_t MetersToPixels( const point_t& pt ) const
985  { return point_int_t( MetersToPixels(pt.x), MetersToPixels(pt.y)); };
986 
987  // dummy implementations to be overloaded by GUI subclasses
988  virtual void PushColor( Color col )
989  { /* do nothing */ (void)col; };
990  virtual void PushColor( double r, double g, double b, double a )
991  { /* do nothing */ (void)r; (void)g; (void)b; (void)a; };
992 
993  virtual void PopColor(){ /* do nothing */ };
994 
996  void DestroySuperRegion( SuperRegion* sr );
997 
999  RaytraceResult Raytrace( const Ray& ray );
1000 
1001  RaytraceResult Raytrace( const Pose& pose,
1002  const meters_t range,
1003  const ray_test_func_t func,
1004  const Model* finder,
1005  const void* arg,
1006  const bool ztest );
1007 
1008  void Raytrace( const Pose &pose,
1009  const meters_t range,
1010  const radians_t fov,
1011  const ray_test_func_t func,
1012  const Model* finder,
1013  const void* arg,
1014  RaytraceResult* samples,
1015  const uint32_t sample_count,
1016  const bool ztest );
1017 
1018 
1020  inline void Extend( point3_t pt );
1021 
1022  virtual void AddModel( Model* mod );
1023  virtual void RemoveModel( Model* mod );
1024 
1025  void AddModelName( Model* mod, const std::string& name );
1026 
1027  void AddPowerPack( PowerPack* pp );
1028  void RemovePowerPack( PowerPack* pp );
1029 
1030  void ClearRays();
1031 
1033  void RecordRay( double x1, double y1, double x2, double y2 );
1034 
1037  bool PastQuitTime();
1038 
1039  static void* update_thread_entry( std::pair<World*,int>* info );
1040 
1041  class Event
1042  {
1043  public:
1044 
1045  Event( usec_t time, Model* mod, model_callback_t cb, void* arg )
1046  : time(time), mod(mod), cb(cb), arg(arg) {}
1047 
1048  usec_t time;
1049  Model* mod;
1051  void* arg;
1052 
1055  bool operator<( const Event& other ) const;
1056  };
1057 
1059  std::vector<std::priority_queue<Event> > event_queues;
1060 
1062  std::vector<std::queue<Model*> > pending_update_callbacks;
1063 
1075  void Enqueue( unsigned int queue_num, usec_t delay, Model* mod, model_callback_t cb, void* arg )
1076  { event_queues[queue_num].push( Event( sim_time + delay, mod, cb, arg ) ); }
1077 
1079  std::set<Model*> active_energy;
1080  void EnableEnergy( Model* m ) { active_energy.insert( m ); };
1081  void DisableEnergy( Model* m ) { active_energy.erase( m ); };
1082 
1084  std::set<ModelPosition*> active_velocity;
1085 
1088 
1089  // debug instrumentation - making sure the number of update callbacks
1090  // in each thread is consistent with the number that have been
1091  // registered globally
1093 
1095  void ConsumeQueue( unsigned int queue_num );
1096 
1099  unsigned int GetEventQueue( Model* mod ) const;
1100 
1101  public:
1103  static bool UpdateAll();
1104 
1111  static void Run();
1112 
1113  World( const std::string& name = "MyWorld",
1114  double ppm = DEFAULT_PPM );
1115 
1116  virtual ~World();
1117 
1119  usec_t SimTimeNow(void) const { return sim_time; }
1120 
1123  Worldfile* GetWorldFile() { return wf; };
1124 
1128  virtual bool IsGUI() const { return false; }
1129 
1135  virtual void Load( const std::string& worldfile_path );
1136 
1137  virtual void UnLoad();
1138 
1139  virtual void Reload();
1140 
1143  virtual bool Save( const char* filename );
1144 
1148  virtual bool Update(void);
1149 
1153  bool TestQuit() const { return( quit || quit_all ); }
1154 
1156  void Quit(){ quit = true; }
1157 
1159  void QuitAll(){ quit_all = true; }
1160 
1162  void CancelQuit(){ quit = false; }
1163 
1165  void CancelQuitAll(){ quit_all = false; }
1166 
1167  void TryCharge( PowerPack* pp, const Pose& pose );
1168 
1171  double Resolution() const { return ppm; };
1172 
1175  Model* GetModel( const std::string& name ) const;
1176 
1178  const std::set<Model*> GetAllModels() const { return models; };
1179 
1181  const bounds3d_t& GetExtent() const { return extent; };
1182 
1184  uint64_t GetUpdateCount() const { return updates; }
1185 
1187  void RegisterOption( Option* opt );
1188 
1190  void ShowClock( bool enable ){ show_clock = enable; };
1191 
1193  Model* GetGround() {return ground;};
1194 
1195  };
1196 
1197  class Block
1198  {
1199  friend class BlockGroup;
1200  friend class Model;
1201  friend class SuperRegion;
1202  friend class World;
1203  friend class Canvas;
1204  friend class Cell;
1205  public:
1206 
1210  Block( Model* mod,
1211  const std::vector<point_t>& pts,
1212  meters_t zmin,
1213  meters_t zmax,
1214  Color color,
1215  bool inherit_color,
1216  bool wheel );
1217 
1219  Block( Model* mod, Worldfile* wf, int entity);
1220 
1221  ~Block();
1222 
1224  void Map( unsigned int layer );
1225 
1227  void UnMap( unsigned int layer );
1228 
1230  void DrawSolid(bool topview);
1231 
1233  void DrawFootPrint();
1234 
1236  void Translate( double x, double y );
1237 
1239  double CenterX();
1240 
1242  double CenterY();
1243 
1245  void SetCenterX( double y );
1246 
1248  void SetCenterY( double y );
1249 
1251  void SetCenter( double x, double y);
1252 
1254  void SetZ( double min, double max );
1255 
1256  void AppendTouchingModels( std::set<Model*>& touchers );
1257 
1259  Model* TestCollision();
1260 
1261  void Load( Worldfile* wf, int entity );
1262  Model* GetModel(){ return mod; };
1263  const Color& GetColor();
1264  void Rasterize( uint8_t* data,
1265  unsigned int width, unsigned int height,
1266  meters_t cellwidth, meters_t cellheight );
1267 
1268  private:
1269  Model* mod;
1270  std::vector<point_t> mpts;
1271  size_t pt_count;
1272  std::vector<point_t> pts;
1273  Size size;
1274  Bounds local_z;
1275  Color color;
1276  bool inherit_color;
1277  bool wheel;
1278 
1279  void DrawTop();
1280  void DrawSides();
1281 
1283  Bounds global_z;
1284  bool mapped;
1285 
1287  std::vector< std::list<Block*>::iterator > list_entries;
1288 
1291  std::vector<Cell*> rendered_cells[2];
1292 
1295  point_t BlockPointToModelMeters( const point_t& bpt );
1296 
1298  void InvalidateModelPointCache();
1299 
1300  };
1301 
1302 
1304  {
1305  friend class Model;
1306  friend class Block;
1307 
1308  private:
1309  int displaylist;
1310 
1311  void BuildDisplayList( Model* mod );
1312 
1313  std::vector<Block*> blocks;
1314  Size size;
1315  point3_t offset;
1316  meters_t minx, maxx, miny, maxy;
1317 
1318  public:
1319  BlockGroup();
1320  ~BlockGroup();
1321 
1322  uint32_t GetCount(){ return blocks.size(); };
1323  const Size& GetSize(){ return size; };
1324  const point3_t& GetOffset(){ return offset; };
1325 
1328  void CalcSize();
1329 
1330  void AppendBlock( Block* block );
1331  void CallDisplayList( Model* mod );
1332  void Clear() ;
1334  void AppendTouchingModels( std::set<Model*>& touchers );
1335 
1338  Model* TestCollision();
1339 
1340  void Map( unsigned int layer );
1341  void UnMap( unsigned int layer );
1342 
1344  void DrawSolid( const Geom &geom);
1345 
1347  void DrawFootPrint( const Geom &geom);
1348 
1349  void LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfile *wf );
1350  void LoadBlock( Model* mod, Worldfile* wf, int entity );
1351 
1352  void Rasterize( uint8_t* data,
1353  unsigned int width, unsigned int height,
1354  meters_t cellwidth, meters_t cellheight );
1355 
1357  {
1358  FOR_EACH( it, blocks )
1359  (*it)->InvalidateModelPointCache();
1360  }
1361 
1362  };
1363 
1364  class Camera
1365  {
1366  protected:
1367  double _pitch; //left-right (about y)
1368  double _yaw; //up-down (about x)
1369  double _x, _y, _z;
1370 
1371  public:
1372  Camera() : _pitch( 0 ), _yaw( 0 ), _x( 0 ), _y( 0 ), _z( 0 ) { }
1373  virtual ~Camera() { }
1374 
1375  virtual void Draw( void ) const = 0;
1376  virtual void SetProjection( void ) const = 0;
1377 
1378  double yaw( void ) const { return _yaw; }
1379  double pitch( void ) const { return _pitch; }
1380 
1381  double x( void ) const { return _x; }
1382  double y( void ) const { return _y; }
1383  double z( void ) const { return _z; }
1384 
1385  virtual void reset() = 0;
1386  virtual void Load( Worldfile* wf, int sec ) = 0;
1387 
1388  //TODO data should be passed in somehow else. (at least min/max stuff)
1389  //virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const = 0;
1390  };
1391 
1392  class PerspectiveCamera : public Camera
1393  {
1394  private:
1395  double _z_near;
1396  double _z_far;
1397  double _vert_fov;
1398  double _horiz_fov;
1399  double _aspect;
1400 
1401  public:
1402  PerspectiveCamera( void );
1403 
1404  virtual void Draw( void ) const;
1405  virtual void SetProjection( void ) const;
1406  //void SetProjection( double aspect ) const;
1407  void update( void );
1408 
1409  void strafe( double amount );
1410  void forward( double amount );
1411 
1412  void setPose( double x, double y, double z ) { _x = x; _y = y; _z = z; }
1413  void addPose( double x, double y, double z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; }
1414  void move( double x, double y, double z );
1415  void setFov( double horiz_fov, double vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; }
1417  void setAspect( double aspect ) { _aspect = aspect; }
1418  void setYaw( double yaw ) { _yaw = yaw; }
1419  double horizFov( void ) const { return _horiz_fov; }
1420  double vertFov( void ) const { return _vert_fov; }
1421  void addYaw( double yaw ) { _yaw += yaw; }
1422  void setPitch( double pitch ) { _pitch = pitch; }
1423  void addPitch( double pitch ) { _pitch += pitch; if( _pitch < 0 ) _pitch = 0; else if( _pitch > 180 ) _pitch = 180; }
1424 
1425  double realDistance( double z_buf_val ) const {
1426  return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) );
1427  }
1428  void scroll( double dy ) { _z += dy; }
1429  double nearClip( void ) const { return _z_near; }
1430  double farClip( void ) const { return _z_far; }
1431  void setClip( double near, double far ) { _z_far = far; _z_near = near; }
1432 
1433  void reset() { setPitch( 70 ); setYaw( 0 ); }
1434 
1435  void Load( Worldfile* wf, int sec );
1436  void Save( Worldfile* wf, int sec );
1437  };
1438 
1439  class OrthoCamera : public Camera
1440  {
1441  private:
1442  double _scale;
1443  double _pixels_width;
1444  double _pixels_height;
1445  double _y_min;
1446  double _y_max;
1447 
1448  public:
1449  OrthoCamera( void ) :
1450  _scale( 15 ),
1451  _pixels_width(0),
1452  _pixels_height(0),
1453  _y_min(0),
1454  _y_max(0)
1455  { }
1456 
1457  virtual void Draw() const;
1458 
1459  virtual void SetProjection( double pixels_width,
1460  double pixels_height,
1461  double y_min,
1462  double y_max );
1463 
1464  virtual void SetProjection( void ) const;
1465 
1466  void move( double x, double y );
1467 
1468  void setYaw( double yaw ) { _yaw = yaw; }
1469 
1470  void setPitch( double pitch ) { _pitch = pitch; }
1471 
1472  void addYaw( double yaw ) { _yaw += yaw; }
1473 
1474  void addPitch( double pitch ) {
1475  _pitch += pitch;
1476  if( _pitch > 90 )
1477  _pitch = 90;
1478  else if( _pitch < 0 )
1479  _pitch = 0;
1480  }
1481 
1482  void setScale( double scale ) { _scale = scale; }
1483  void setPose( double x, double y) { _x = x; _y = y; }
1484 
1485  void scale( double scale, double shift_x = 0, double h = 0, double shift_y = 0, double w = 0 );
1486  void reset( void ) { _pitch = _yaw = 0; }
1487 
1488  double scale() const { return _scale; }
1489 
1490  void Load( Worldfile* wf, int sec );
1491  void Save( Worldfile* wf, int sec );
1492  };
1493 
1494 
1498  class WorldGui : public World, public Fl_Window
1499  {
1500  friend class Canvas;
1501  friend class ModelCamera;
1502  friend class Model;
1503  friend class Option;
1504 
1505  private:
1506 
1507  Canvas* canvas;
1508  std::vector<Option*> drawOptions;
1509  FileManager* fileMan;
1510  std::vector<usec_t> interval_log;
1511 
1514  double speedup;
1515 
1516  Fl_Menu_Bar* mbar;
1517  OptionsDlg* oDlg;
1518  bool pause_time;
1519 
1522  usec_t real_time_interval;
1523 
1525  usec_t real_time_now;
1526 
1529  usec_t real_time_recorded;
1530 
1532  uint64_t timing_interval;
1533 
1534  // static callback functions
1535  static void windowCb( Fl_Widget* w, WorldGui* wg );
1536  static void fileLoadCb( Fl_Widget* w, WorldGui* wg );
1537  static void fileSaveCb( Fl_Widget* w, WorldGui* wg );
1538  static void fileSaveAsCb( Fl_Widget* w, WorldGui* wg );
1539  static void fileExitCb( Fl_Widget* w, WorldGui* wg );
1540  static void viewOptionsCb( OptionsDlg* oDlg, WorldGui* wg );
1541  static void optionsDlgCb( OptionsDlg* oDlg, WorldGui* wg );
1542  static void helpAboutCb( Fl_Widget* w, WorldGui* wg );
1543  static void pauseCb( Fl_Widget* w, WorldGui* wg );
1544  static void onceCb( Fl_Widget* w, WorldGui* wg );
1545  static void fasterCb( Fl_Widget* w, WorldGui* wg );
1546  static void slowerCb( Fl_Widget* w, WorldGui* wg );
1547  static void realtimeCb( Fl_Widget* w, WorldGui* wg );
1548  static void fasttimeCb( Fl_Widget* w, WorldGui* wg );
1549  static void resetViewCb( Fl_Widget* w, WorldGui* wg );
1550  static void moreHelptCb( Fl_Widget* w, WorldGui* wg );
1551 
1552  // GUI functions
1553  bool saveAsDialog();
1554  bool closeWindowQuery();
1555 
1556  virtual void AddModel( Model* mod );
1557 
1558  void SetTimeouts();
1559 
1560  protected:
1561 
1562  virtual void PushColor( Color col );
1563  virtual void PushColor( double r, double g, double b, double a );
1564  virtual void PopColor();
1565 
1566  void DrawOccupancy() const;
1567  void DrawVoxels() const;
1568 
1569  public:
1570 
1571  WorldGui(int W,int H,const char*L=0);
1572  ~WorldGui();
1573 
1575  virtual void Redraw( void );
1576 
1577  virtual std::string ClockString() const;
1578  virtual bool Update();
1579  virtual void Load( const std::string& filename );
1580  virtual void UnLoad();
1581  virtual bool Save( const char* filename );
1582  virtual bool IsGUI() const { return true; };
1583  virtual Model* RecentlySelectedModel() const;
1584 
1585  virtual void Start();
1586  virtual void Stop();
1587 
1588  usec_t RealTimeNow(void) const;
1589 
1590  void DrawBoundingBoxTree();
1591 
1592  Canvas* GetCanvas( void ) const { return canvas; }
1593 
1595  void Show();
1596 
1598  std::string EnergyString( void ) const;
1599  virtual void RemoveChild( Model* mod );
1600 
1601  bool IsTopView();
1602  };
1603 
1604 
1605  class StripPlotVis : public Visualizer
1606  {
1607  private:
1608 
1609  Model* mod;
1610  float* data;
1611  size_t len;
1612  size_t count;
1613  unsigned int index;
1614  float x,y,w,h,min,max;
1615  Color fgcolor, bgcolor;
1616 
1617  public:
1618  StripPlotVis( float x, float y, float w, float h,
1619  size_t len,
1620  Color fgcolor, Color bgcolor,
1621  const char* name, const char* wfname );
1622  virtual ~StripPlotVis();
1623  virtual void Visualize( Model* mod, Camera* cam );
1624  void AppendValue( float value );
1625  };
1626 
1627 
1629  {
1630  friend class WorldGui;
1631  friend class Canvas;
1632 
1633  protected:
1634 
1636  {
1637  private:
1638  unsigned int columns, rows;
1639  meters_t width, height;
1640 
1641  std::vector<joules_t> cells;
1642 
1643  joules_t peak_value;
1644  double cellsize;
1645 
1646  static joules_t global_peak_value;
1647 
1648  public:
1649  DissipationVis( meters_t width,
1650  meters_t height,
1651  meters_t cellsize );
1652 
1653  virtual ~DissipationVis();
1654  virtual void Visualize( Model* mod, Camera* cam );
1655 
1656  void Accumulate( meters_t x, meters_t y, joules_t amount );
1657  } event_vis;
1658 
1659 
1662 
1664  Model* mod;
1665 
1667  joules_t stored;
1668 
1670  joules_t capacity;
1671 
1673  bool charging;
1674 
1676  joules_t dissipated;
1677 
1678  // these are used to visualize the power draw
1679  usec_t last_time;
1680  joules_t last_joules;
1681  watts_t last_watts;
1682 
1683  public:
1684  static joules_t global_stored;
1685  static joules_t global_capacity;
1686  static joules_t global_dissipated;
1687  static joules_t global_input;
1688 
1689  public:
1690  PowerPack( Model* mod );
1691  ~PowerPack();
1692 
1694  void Visualize( Camera* cam );
1695 
1697  joules_t RemainingCapacity() const;
1698 
1700  void Add( joules_t j );
1701 
1703  void Subtract( joules_t j );
1704 
1706  void TransferTo( PowerPack* dest, joules_t amount );
1707 
1708  double ProportionRemaining() const
1709  { return( stored / capacity ); }
1710 
1713  void Print( const char* prefix ) const
1714  {
1715  if( prefix )
1716  printf( "%s", prefix );
1717 
1718  printf( "PowerPack %.2f/%.2f J\n", stored, capacity );
1719  }
1720 
1721  joules_t GetStored() const;
1722  joules_t GetCapacity() const;
1723  joules_t GetDissipated() const;
1724  void SetCapacity( joules_t j );
1725  void SetStored( joules_t j );
1726 
1728  bool GetCharging() const { return charging; }
1729 
1730  void ChargeStart(){ charging = true; }
1731  void ChargeStop(){ charging = false; }
1732 
1734  void Dissipate( joules_t j );
1735 
1737  void Dissipate( joules_t j, const Pose& p );
1738  };
1739 
1740 
1742  class Model : public Ancestor
1743  {
1744  friend class Ancestor;
1745  friend class World;
1746  friend class World::Event;
1747  friend class WorldGui;
1748  friend class Canvas;
1749  friend class Block;
1750  friend class Region;
1751  friend class BlockGroup;
1752  friend class PowerPack;
1753  friend class Ray;
1754  friend class ModelFiducial;
1755 
1756  private:
1758  static uint32_t count;
1759  static std::map<id_t,Model*> modelsbyid;
1760 
1762  bool mapped;
1763 
1764  std::vector<Option*> drawOptions;
1765  const std::vector<Option*>& getOptions() const { return drawOptions; }
1766 
1767  protected:
1768 
1771  bool alwayson;
1772 
1776 
1781 
1784  public:
1785  class cb_t
1786  {
1787  public:
1789  void* arg;
1790 
1791  cb_t( model_callback_t cb, void* arg )
1792  : callback(cb), arg(arg) {}
1793 
1794  cb_t( world_callback_t cb, void* arg )
1795  : callback(NULL), arg(arg) { (void)cb; }
1796 
1797  cb_t() : callback(NULL), arg(NULL) {}
1798 
1800  bool operator<( const cb_t& other ) const
1801  {
1802  if( callback == other.callback )
1803  return( arg < other.arg );
1804  //else
1805  return ((void*)(callback)) < ((void*)(other.callback));
1806  }
1807 
1809  bool operator==( const cb_t& other ) const
1810  { return( callback == other.callback); }
1811  };
1812 
1813  class Flag
1814  {
1815  private:
1816  Color color;
1817  double size;
1818  int displaylist;
1819 
1820  public:
1821  void SetColor( const Color& col );
1822  void SetSize( double sz );
1823 
1824  Color GetColor(){ return color; }
1825  double GetSize(){ return size; }
1826 
1827  Flag( const Color& color, double size );
1828  Flag* Nibble( double portion );
1829 
1832  void Draw( GLUquadric* quadric );
1833  };
1834 
1835  typedef enum {
1848  //CB_POSTUPDATE,
1849  __CB_TYPE_COUNT // must be the last entry: counts the number of types
1850  } callback_type_t;
1851 
1852  protected:
1856  std::vector<std::set<cb_t> > callbacks;
1857 
1860 
1865 
1869  bool disabled;
1870 
1872  std::list<Visualizer*> cv_list;
1873 
1875  std::list<Flag*> flag_list;
1876 
1879  double friction;
1880 
1884 
1886  class GuiState
1887  {
1888  public:
1889  bool grid;
1890  bool move;
1891  bool nose;
1892  bool outline;
1893 
1894  GuiState();
1895  GuiState& Load( Worldfile* wf, int wf_entity );
1896  } gui;
1897 
1899 
1901  uint32_t id;
1902  usec_t interval;
1904  // usec_t interval_pose; ///< time between updates of pose due to velocity in usec
1905 
1906  usec_t last_update;
1907  bool log_state;
1908  meters_t map_resolution;
1909  kg_t mass;
1910 
1912  Model* parent;
1913 
1917 
1920 
1923  std::list<PowerPack*> pps_charging;
1924 
1926  class RasterVis : public Visualizer
1927  {
1928  private:
1929  uint8_t* data;
1930  unsigned int width, height;
1931  meters_t cellwidth, cellheight;
1932  std::vector<point_t> pts;
1933 
1934  public:
1935  RasterVis();
1936  virtual ~RasterVis( void ){}
1937  virtual void Visualize( Model* mod, Camera* cam );
1938 
1939  void SetData( uint8_t* data,
1940  unsigned int width,
1941  unsigned int height,
1942  meters_t cellwidth,
1943  meters_t cellheight );
1944 
1945  int subs; //< the number of subscriptions to this model
1946  int used; //< the number of connections to this model
1947 
1948  void AddPoint( meters_t x, meters_t y );
1949  void ClearPts();
1950 
1951  } rastervis;
1952 
1954  std::string say_string;
1955 
1957 
1958  bool stall;
1959  int subs;
1960 
1965 
1967  class TrailItem
1968  {
1969  public:
1970  usec_t time;
1973 
1975  : time(0), pose(), color(){}
1976 
1977  //TrailItem( usec_t time, Pose pose, Color color )
1978  //: time(time), pose(pose), color(color){}
1979  };
1980 
1982  std::vector<TrailItem> trail;
1983 
1985  unsigned int trail_index;
1986 
1990  static unsigned int trail_length;
1991 
1993  static uint64_t trail_interval;
1994 
1996  void UpdateTrail();
1997 
1998  //model_type_t type;
1999  const std::string type;
2002  unsigned int event_queue_num;
2003  bool used;
2004 
2005  watts_t watts;
2006 
2009  watts_t watts_give;
2010 
2013  watts_t watts_take;
2014 
2017  World* world; // pointer to the world in which this model exists
2018  WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode
2019 
2020  public:
2021 
2022  virtual void SetToken( const std::string& str )
2023  {
2024  //printf( "Model::SetToken( %s )\n", str.c_str() );
2025 
2026  if( str.size() > 0 )
2027  {
2028  world->AddModelName( this, str );
2029  Ancestor::SetToken( str );
2030  }
2031  else
2032  PRINT_ERR( "Model::SetToken() called with zero length string. Ignored." );
2033  }
2034 
2035 
2036  const std::string& GetModelType() const {return type;}
2037  std::string GetSayString(){return std::string(say_string);}
2038 
2041  Model* GetChild( const std::string& name ) const;
2042 
2044  usec_t GetInterval(){ return interval; }
2045 
2047  {
2048  public:
2054  double ranger_return; // 0 - 1
2055 
2056  Visibility();
2057 
2058  Visibility& Load( Worldfile* wf, int wf_entity );
2059  void Save( Worldfile* wf, int wf_entity );
2060  } vis;
2061 
2062  usec_t GetUpdateInterval() const { return interval; }
2063  usec_t GetEnergyInterval() const { return interval_energy; }
2064  // usec_t GetPoseInterval() const { return interval_pose; }
2065 
2068  void Rasterize( uint8_t* data,
2069  unsigned int width, unsigned int height,
2070  meters_t cellwidth, meters_t cellheight );
2071 
2072  private:
2075  explicit Model(const Model& original);
2076 
2079  Model& operator=(const Model& original);
2080 
2081  protected:
2082 
2084  void RegisterOption( Option* opt );
2085 
2086  void AppendTouchingModels( std::set<Model*>& touchers );
2087 
2092  Model* TestCollision();
2093 
2094  void CommitTestedPose();
2095 
2096  void Map( unsigned int layer );
2097 
2099  inline void Map(){ Map(0); Map(1); }
2100 
2101  void UnMap( unsigned int layer );
2102 
2104  inline void UnMap(){ UnMap(0); UnMap(1); }
2105 
2106  void MapWithChildren( unsigned int layer );
2107  void UnMapWithChildren( unsigned int layer );
2108 
2109  // Find the root model, and map/unmap the whole tree.
2110  void MapFromRoot( unsigned int layer );
2111  void UnMapFromRoot( unsigned int layer );
2112 
2115  RaytraceResult Raytrace( const Pose &pose,
2116  const meters_t range,
2117  const ray_test_func_t func,
2118  const void* arg,
2119  const bool ztest = true );
2120 
2123  void Raytrace( const Pose &pose,
2124  const meters_t range,
2125  const radians_t fov,
2126  const ray_test_func_t func,
2127  const void* arg,
2128  RaytraceResult* samples,
2129  const uint32_t sample_count,
2130  const bool ztest = true );
2131 
2132  RaytraceResult Raytrace( const radians_t bearing,
2133  const meters_t range,
2134  const ray_test_func_t func,
2135  const void* arg,
2136  const bool ztest = true );
2137 
2138  void Raytrace( const radians_t bearing,
2139  const meters_t range,
2140  const radians_t fov,
2141  const ray_test_func_t func,
2142  const void* arg,
2143  RaytraceResult* samples,
2144  const uint32_t sample_count,
2145  const bool ztest = true );
2146 
2147  virtual void Startup();
2148  virtual void Shutdown();
2149  virtual void Update();
2150  virtual void UpdateCharge();
2151 
2152  static int UpdateWrapper( Model* mod, void* arg ){ mod->Update(); return 0; }
2153 
2155  void CallUpdateCallbacks( void );
2156 
2157  meters_t ModelHeight() const;
2158 
2159  void DrawBlocksTree();
2160  virtual void DrawBlocks();
2161  void DrawBoundingBox();
2162  void DrawBoundingBoxTree();
2163  virtual void DrawStatus( Camera* cam );
2164  void DrawStatusTree( Camera* cam );
2165 
2166  void DrawOriginTree();
2167  void DrawOrigin();
2168 
2169  void PushLocalCoords();
2170  void PopCoords();
2171 
2173  void DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width=1.0, double height=1.0 );
2174 
2175  virtual void DrawPicker();
2176  virtual void DataVisualize( Camera* cam );
2177  virtual void DrawSelected(void);
2178 
2179  void DrawTrailFootprint();
2180  void DrawTrailBlocks();
2181  void DrawTrailArrows();
2182  void DrawGrid();
2183  // void DrawBlinkenlights();
2184  void DataVisualizeTree( Camera* cam );
2185  void DrawFlagList();
2186  void DrawPose( Pose pose );
2187 
2188  public:
2189  virtual void PushColor( Color col ){ world->PushColor( col ); }
2190  virtual void PushColor( double r, double g, double b, double a ){ world->PushColor( r,g,b,a ); }
2191  virtual void PopColor() { world->PopColor(); }
2192 
2193  PowerPack* FindPowerPack() const;
2194 
2195  //void RecordRenderPoint( GSList** head, GSList* link,
2196  // unsigned int* c1, unsigned int* c2 );
2197 
2198  void PlaceInFreeSpace( meters_t xmin, meters_t xmax,
2199  meters_t ymin, meters_t ymax );
2200 
2202  std::string PoseString()
2203  { return pose.String(); }
2204 
2206  static Model* LookupId( uint32_t id )
2207  { return modelsbyid[id]; }
2208 
2210  Model( World* world,
2211  Model* parent = NULL,
2212  const std::string& type = "model",
2213  const std::string& name = "" );
2214 
2216  virtual ~Model();
2217 
2220  : mapped(false), alwayson(false), blocks_dl(0),
2221  boundary(false), data_fresh(false), disabled(true), friction(0), has_default_block(false), log_state(false), map_resolution(0), mass(0), parent(NULL), rebuild_displaylist(false), stack_children(true), stall(false), subs(0), thread_safe(false),trail_index(0), event_queue_num(0), used(false), watts(0), watts_give(0),watts_take(0),wf(NULL), wf_entity(0), world(NULL)
2222  {}
2223 
2224  void Say( const std::string& str );
2225 
2227  void AddVisualizer( Visualizer* custom_visual, bool on_by_default );
2228 
2230  void RemoveVisualizer( Visualizer* custom_visual );
2231 
2232  void BecomeParentOf( Model* child );
2233 
2234  void Load( Worldfile* wf, int wf_entity )
2235  {
2238  SetWorldfile( wf, wf_entity );
2239  Load(); // call virtual load
2240  }
2241 
2243  void SetWorldfile( Worldfile* wf, int wf_entity )
2244  { this->wf = wf; this->wf_entity = wf_entity; }
2245 
2247  virtual void Load();
2248 
2250  virtual void Save();
2251 
2253  void InitControllers();
2254 
2255  void AddFlag( Flag* flag );
2256  void RemoveFlag( Flag* flag );
2257 
2258  void PushFlag( Flag* flag );
2259  Flag* PopFlag();
2260 
2261  unsigned int GetFlagCount() const { return flag_list.size(); }
2262 
2267  void Disable(){ disabled = true; };
2268 
2271  void Enable(){ disabled = false; };
2272 
2275  void LoadControllerModule( const char* lib );
2276 
2279  void NeedRedraw();
2280 
2282  void Redraw();
2283 
2286  void LoadBlock( Worldfile* wf, int entity );
2287 
2290  Block* AddBlockRect( meters_t x, meters_t y,
2291  meters_t dx, meters_t dy,
2292  meters_t dz );
2293 
2295  void ClearBlocks();
2296 
2299  Model* Parent() const { return this->parent; }
2300 
2302  World* GetWorld() const { return this->world; }
2303 
2305  Model* Root(){ return( parent ? parent->Root() : this ); }
2306 
2307  bool IsAntecedent( const Model* testmod ) const;
2308 
2310  bool IsDescendent( const Model* testmod ) const;
2311 
2313  bool IsRelated( const Model* testmod ) const;
2314 
2316  Pose GetGlobalPose() const;
2317 
2319  Velocity GetGlobalVelocity() const;
2320 
2321  /* set the velocity of a model in the global coordinate system */
2322  void SetGlobalVelocity( const Velocity& gvel );
2323 
2325  void Subscribe();
2326 
2328  void Unsubscribe();
2329 
2331  void SetGlobalPose( const Pose& gpose );
2332 
2334  // void VelocityEnable();
2335 
2337  //void VelocityDisable();
2338 
2340  void SetPose( const Pose& pose );
2341 
2343  void AddToPose( const Pose& pose );
2344 
2346  void AddToPose( double dx, double dy, double dz, double da );
2347 
2349  void SetGeom( const Geom& src );
2350 
2353  void SetFiducialReturn( int fid );
2354 
2356  int GetFiducialReturn() const { return vis.fiducial_return; }
2357 
2360  void SetFiducialKey( int key );
2361 
2362  Color GetColor() const { return color; }
2363 
2365  uint32_t GetId() const { return id; }
2366 
2368  kg_t GetTotalMass() const;
2369 
2371  kg_t GetMassOfChildren() const;
2372 
2374  int SetParent( Model* newparent);
2375 
2378  Geom GetGeom() const { return geom; }
2379 
2382  Pose GetPose() const { return pose; }
2383 
2384 
2385  // guess what these do?
2386  void SetColor( Color col );
2387  void SetMass( kg_t mass );
2388  void SetStall( bool stall );
2389  void SetGravityReturn( bool val );
2390  void SetGripperReturn( bool val );
2391  void SetStickyReturn( bool val );
2392  void SetRangerReturn( double val );
2393  void SetObstacleReturn( bool val );
2394  void SetBlobReturn( bool val );
2395  void SetRangerReturn( bool val );
2396  void SetBoundary( bool val );
2397  void SetGuiNose( bool val );
2398  void SetGuiMove( bool val );
2399  void SetGuiGrid( bool val );
2400  void SetGuiOutline( bool val );
2401  void SetWatts( watts_t watts );
2402  void SetMapResolution( meters_t res );
2403  void SetFriction( double friction );
2404 
2405  bool DataIsFresh() const { return this->data_fresh; }
2406 
2407  /* attach callback functions to data members. The function gets
2408  called when the member is changed using SetX() accessor method */
2409 
2416  void AddCallback( callback_type_t type,
2417  model_callback_t cb,
2418  void* user );
2419 
2420  int RemoveCallback( callback_type_t type,
2421  model_callback_t callback );
2422 
2423  int CallCallbacks( callback_type_t type );
2424 
2425 
2426  virtual void Print( char* prefix ) const;
2427  virtual const char* PrintWithPose() const;
2428 
2431  Pose GlobalToLocal( const Pose& pose ) const;
2432 
2435  Pose LocalToGlobal( const Pose& pose ) const
2436  {
2437  return( ( GetGlobalPose() + geom.pose ) + pose );
2438  }
2439 
2441  std::vector<point_int_t> LocalToPixels( const std::vector<point_t>& local ) const;
2442 
2445  point_t LocalToGlobal( const point_t& pt) const;
2446 
2449  Model* GetUnsubscribedModelOfType( const std::string& type ) const;
2450 
2453  Model* GetUnusedModelOfType( const std::string& type );
2454 
2457  bool Stalled() const { return this->stall; }
2458 
2461  unsigned int GetSubscriptionCount() const { return subs; }
2462 
2464  bool HasSubscribers() const { return( subs > 0 ); }
2465 
2466  static std::map< std::string, creator_t> name_map;
2467 
2468  // class Neighbors
2469  // {
2470  // Model *left, *right, *up, *down;
2471  // public:
2472  // Neighbors() : left(NULL), right(NULL), up(NULL), down(NULL) {}
2473  // } nbors; // instance
2474 
2475 
2476  };
2477 
2478 
2479  // BLOBFINDER MODEL --------------------------------------------------------
2480 
2481 
2483  class ModelBlobfinder : public Model
2484  {
2485  public:
2487  class Blob
2488  {
2489  public:
2491  uint32_t left, top, right, bottom;
2492  meters_t range;
2493  };
2494 
2495  class Vis : public Visualizer
2496  {
2497  private:
2498  //static Option showArea;
2499  public:
2500  Vis( World* world );
2501  virtual ~Vis( void ){}
2502  virtual void Visualize( Model* mod, Camera* cam );
2503  } vis;
2504 
2505  private:
2506  std::vector<Blob> blobs;
2507  std::vector<Color> colors;
2508 
2509  // predicate for ray tracing
2510  static bool BlockMatcher( Block* testblock, Model* finder );
2511 
2512  public:
2513  radians_t fov;
2514  radians_t pan;
2515  meters_t range;
2516  unsigned int scan_height;
2517  unsigned int scan_width;
2518 
2519  // constructor
2520  ModelBlobfinder( World* world,
2521  Model* parent,
2522  const std::string& type );
2523  // destructor
2524  ~ModelBlobfinder();
2525 
2526  virtual void Startup();
2527  virtual void Shutdown();
2528  virtual void Update();
2529  virtual void Load();
2530 
2531  Blob* GetBlobs( unsigned int* count )
2532  {
2533  if( count ) *count = blobs.size();
2534  return &blobs[0];
2535  }
2536 
2537  std::vector<Blob> GetBlobs() const { return blobs; }
2538 
2540  void AddColor( Color col );
2541 
2543  void RemoveColor( Color col );
2544 
2547  void RemoveAllColors();
2548  };
2549 
2550 
2551 
2552 
2553 
2554  // Light indicator model
2555  class ModelLightIndicator : public Model
2556  {
2557  public:
2558  ModelLightIndicator( World* world,
2559  Model* parent,
2560  const std::string& type );
2562 
2563  void SetState(bool isOn);
2564 
2565  protected:
2566  virtual void DrawBlocks();
2567 
2568  private:
2569  bool m_IsOn;
2570  };
2571 
2572  // \todo GRIPPER MODEL --------------------------------------------------------
2573 
2574 
2575  class ModelGripper : public Model
2576  {
2577  public:
2578 
2580  PADDLE_OPEN = 0, // default state
2584  };
2585 
2587  LIFT_DOWN = 0, // default state
2589  LIFT_UPPING, // verbed these to match the paddle state
2591  };
2592 
2593  enum cmd_t {
2594  CMD_NOOP = 0, // default state
2599  };
2600 
2601 
2604  struct config_t
2605  {
2610  double lift_position;
2611  Model* gripped;
2612  bool paddles_stalled; // true iff some solid object stopped the paddles closing or opening
2613  double close_limit;
2614  bool autosnatch;
2615  double break_beam_inset[2];
2616  Model* beam[2];
2617  Model* contact[2];
2618  };
2619 
2620  private:
2621  virtual void Update();
2622  virtual void DataVisualize( Camera* cam );
2623 
2624  void FixBlocks();
2625  void PositionPaddles();
2626  void UpdateBreakBeams();
2627  void UpdateContacts();
2628 
2629  config_t cfg;
2630  cmd_t cmd;
2631 
2632  Block* paddle_left;
2633  Block* paddle_right;
2634 
2635  static Option showData;
2636 
2637  public:
2638  static const Size size;
2639 
2640  // constructor
2641  ModelGripper( World* world,
2642  Model* parent,
2643  const std::string& type );
2644  // destructor
2645  virtual ~ModelGripper();
2646 
2647  virtual void Load();
2648  virtual void Save();
2649 
2651  void SetConfig( config_t & newcfg ){ this->cfg = newcfg; FixBlocks(); }
2652 
2654  config_t GetConfig(){ return cfg; };
2655 
2657  void SetCommand( cmd_t cmd ) { this->cmd = cmd; }
2663  void CommandUp() { SetCommand( CMD_UP ); }
2666  };
2667 
2668 
2669  // \todo BUMPER MODEL --------------------------------------------------------
2670 
2671  // typedef struct
2672  // {
2673  // Pose pose;
2674  // meters_t length;
2675  // } bumper_config_t;
2676 
2677  // typedef struct
2678  // {
2679  // Model* hit;
2680  // point_t hit_point;
2681  // } bumper_sample_t;
2682 
2683 
2684  // FIDUCIAL MODEL --------------------------------------------------------
2685 
2687  class ModelFiducial : public Model
2688  {
2689  public:
2691  class Fiducial
2692  {
2693  public:
2694  meters_t range;
2695  radians_t bearing;
2697  //Pose pose_rel; /// relative pose of the target in local coordinates
2699  Model* mod;
2700  int id;
2701  };
2702 
2703  private:
2704  // if neighbor is visible, add him to the fiducial scan
2705  void AddModelIfVisible( Model* him );
2706 
2707  virtual void Update();
2708  virtual void DataVisualize( Camera* cam );
2709 
2710  static Option showData;
2711  static Option showFov;
2712 
2713  std::vector<Fiducial> fiducials;
2714 
2715  public:
2716  ModelFiducial( World* world,
2717  Model* parent,
2718  const std::string& type );
2719  virtual ~ModelFiducial();
2720 
2721  virtual void Load();
2722  void Shutdown( void );
2723 
2724  meters_t max_range_anon;
2725  meters_t max_range_id;
2726  meters_t min_range;
2727  radians_t fov;
2728  radians_t heading;
2729  int key;
2731 
2733  std::vector<Fiducial>& GetFiducials() { return fiducials; }
2734 
2736  Fiducial* GetFiducials( unsigned int* count )
2737  {
2738  if( count ) *count = fiducials.size();
2739  return &fiducials[0];
2740  }
2741  };
2742 
2743 
2744  // RANGER MODEL --------------------------------------------------------
2745 
2747  class ModelRanger : public Model
2748  {
2749  public:
2750  public:
2751  ModelRanger( World* world, Model* parent,
2752  const std::string& type );
2753  virtual ~ModelRanger();
2754 
2755  virtual void Load();
2756  virtual void Print( char* prefix ) const;
2757 
2758  class Vis : public Visualizer
2759  {
2760  public:
2763  static Option showFov;
2766 
2767  Vis( World* world );
2768  virtual ~Vis( void ){}
2769  virtual void Visualize( Model* mod, Camera* cam );
2770  } vis;
2771 
2772  class Sensor
2773  {
2774  public:
2778  radians_t fov;
2779  unsigned int sample_count;
2781 
2782  std::vector<meters_t> ranges;
2783  std::vector<double> intensities;
2784  std::vector<double> bearings;
2785 
2786  Sensor() : pose( 0,0,0,0 ),
2787  size( 0.02, 0.02, 0.02 ), // teeny transducer
2788  range( 0.0, 5.0 ),
2789  fov( 0.1 ),
2790  sample_count(1),
2791  col( 0,1,0,0.3 ),
2792  ranges(),
2793  intensities(),
2794  bearings()
2795  {}
2796 
2797  void Update( ModelRanger* rgr );
2798  void Visualize( Vis* vis, ModelRanger* rgr ) const;
2799  std::string String() const;
2800  void Load( Worldfile* wf, int entity );
2801  };
2802 
2804  const std::vector<Sensor>& GetSensors() const
2805  { return sensors; }
2806 
2808  std::vector<Sensor>& GetSensorsMutable()
2809  { return sensors; }
2810 
2811  void LoadSensor( Worldfile* wf, int entity );
2812 
2813  private:
2814  std::vector<Sensor> sensors;
2815 
2816  protected:
2817 
2818  virtual void Startup();
2819  virtual void Shutdown();
2820  virtual void Update();
2821  };
2822 
2823  // BLINKENLIGHT MODEL ----------------------------------------------------
2824  class ModelBlinkenlight : public Model
2825  {
2826  private:
2827  double dutycycle;
2828  bool enabled;
2829  msec_t period;
2830  bool on;
2831 
2832  static Option showBlinkenData;
2833  public:
2834  ModelBlinkenlight( World* world,
2835  Model* parent,
2836  const std::string& type );
2837 
2839 
2840  virtual void Load();
2841  virtual void Update();
2842  virtual void DataVisualize( Camera* cam );
2843  };
2844 
2845 
2846  // CAMERA MODEL ----------------------------------------------------
2847 
2849  class ModelCamera : public Model
2850  {
2851  public:
2852  typedef struct
2853  {
2854  // GL_V3F
2855  GLfloat x, y, z;
2856  } ColoredVertex;
2857 
2858  private:
2859  Canvas* _canvas;
2860 
2861  GLfloat* _frame_data; //opengl read buffer
2862  GLubyte* _frame_color_data; //opengl read buffer
2863 
2864  bool _valid_vertexbuf_cache;
2865  ColoredVertex* _vertexbuf_cache; //cached unit vectors with appropriate rotations (these must be scalled by z-buffer length)
2866 
2867  int _width; //width of buffer
2868  int _height; //height of buffer
2869  static const int _depth = 4;
2870 
2871  int _camera_quads_size;
2872  GLfloat* _camera_quads;
2873  GLubyte* _camera_colors;
2874 
2875  static Option showCameraData;
2876 
2877  PerspectiveCamera _camera;
2878  double _yaw_offset; //position camera is mounted at
2879  double _pitch_offset;
2880 
2882  bool GetFrame();
2883 
2884  public:
2885  ModelCamera( World* world,
2886  Model* parent,
2887  const std::string& type );
2888 
2889  ~ModelCamera();
2890 
2891  virtual void Load();
2892 
2894  virtual void Update();
2895 
2897  //virtual void Draw( uint32_t flags, Canvas* canvas );
2898 
2900  virtual void DataVisualize( Camera* cam );
2901 
2903  int getWidth( void ) const { return _width; }
2904 
2906  int getHeight( void ) const { return _height; }
2907 
2909  const PerspectiveCamera& getCamera( void ) const { return _camera; }
2910 
2912  const GLfloat* FrameDepth() const { return _frame_data; }
2913 
2915  const GLubyte* FrameColor() const { return _frame_color_data; }
2916 
2918  void setPitch( double pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; }
2919 
2921  void setYaw( double yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; }
2922  };
2923 
2924  // POSITION MODEL --------------------------------------------------------
2925 
2927  class ModelPosition : public Model
2928  {
2929  friend class Canvas;
2930 
2931  public:
2933  typedef enum
2937  } ControlMode;
2938 
2940  typedef enum
2943  } LocalizationMode;
2944 
2946  typedef enum
2950  } DriveMode;
2951 
2952  private:
2953  Velocity velocity;
2954  Pose goal;
2955  ControlMode control_mode;
2956  DriveMode drive_mode;
2957  LocalizationMode localization_mode;
2958  Velocity integration_error;
2959  double wheelbase;
2960 
2961  public:
2964 
2967 
2968  public:
2969  // constructor
2970  ModelPosition( World* world,
2971  Model* parent,
2972  const std::string& type );
2973  // destructor
2974  ~ModelPosition();
2975 
2976  virtual void Move();
2977  virtual void Startup();
2978  virtual void Shutdown();
2979  virtual void Update();
2980  virtual void Load();
2981 
2984  Velocity GetVelocity() const { return velocity; }
2985  void SetVelocity( const Velocity& val );
2986 
2989  class Waypoint
2990  {
2991  public:
2992  Waypoint( meters_t x, meters_t y, meters_t z, radians_t a, Color color ) ;
2993  Waypoint( const Pose& pose, Color color ) ;
2994  Waypoint();
2995  void Draw() const;
2996 
2999  };
3000 
3001  std::vector<Waypoint> waypoints;
3002 
3003  class WaypointVis : public Visualizer
3004  {
3005  public:
3006  WaypointVis();
3007  virtual ~WaypointVis( void ){}
3008  virtual void Visualize( Model* mod, Camera* cam );
3009  } wpvis;
3010 
3011  class PoseVis : public Visualizer
3012  {
3013  public:
3014  PoseVis();
3015  virtual ~PoseVis( void ){}
3016  virtual void Visualize( Model* mod, Camera* cam );
3017  } posevis;
3018 
3020  void SetOdom( Pose odom );
3021 
3024  void SetSpeed( double x, double y, double a );
3025  void SetXSpeed( double x );
3026  void SetYSpeed( double y );
3027  void SetZSpeed( double z );
3028  void SetTurnSpeed( double a );
3029  void SetSpeed( Velocity vel );
3031  void Stop();
3032 
3035  void GoTo( double x, double y, double a );
3036  void GoTo( Pose pose );
3037 
3041  void SetAcceleration( double x, double y, double a );
3042 
3043  // localization state
3047  };
3048 
3049 
3050  // ACTUATOR MODEL --------------------------------------------------------
3051 
3053  class ModelActuator : public Model
3054  {
3055  public:
3057  typedef enum
3060  } ControlMode;
3061 
3063  typedef enum
3066  } ActuatorType;
3067 
3068  private:
3069  double goal; //< the current velocity or pose to reach depending on the value of control_mode
3070  double pos;
3071  double max_speed;
3072  double min_position;
3073  double max_position;
3074  double start_position;
3075  double cosa;
3076  double sina;
3077  ControlMode control_mode;
3078  ActuatorType actuator_type;
3079  point3_t axis;
3080 
3081  Pose InitialPose;
3082  public:
3083  // constructor
3084  ModelActuator( World* world,
3085  Model* parent,
3086  const std::string& type );
3087  // destructor
3088  ~ModelActuator();
3089 
3090  virtual void Startup();
3091  virtual void Shutdown();
3092  virtual void Update();
3093  virtual void Load();
3094 
3097  void SetSpeed( double speed );
3098 
3099  double GetSpeed() const {return goal;}
3100 
3103  void GoTo( double pose );
3104 
3105  double GetPosition() const {return pos;};
3106  double GetMaxPosition() const {return max_position;};
3107  double GetMinPosition() const {return min_position;};
3108 
3109  ActuatorType GetType() const { return actuator_type; }
3110  point3_t GetAxis() const { return axis; }
3111  };
3112 
3113 
3114 }; // end namespace stg
3115 
3116 #endif
void SetTurnSpeed(double a)
Definition: model_position.cc:637
Bounds()
Definition: stage.hh:437
callback_type_t
Definition: stage.hh:1835
radians_t fov
field of view
Definition: stage.hh:2727
meters_t range
range to the target
Definition: stage.hh:2694
void SetBoundary(bool val)
Definition: model.cc:1313
Pose pose
location and direction of the ray origin
Definition: stage.hh:744
std::vector< point_int_t > rt_candidate_cells
Definition: stage.hh:935
unsigned int scan_height
Definition: stage.hh:2516
virtual void PushColor(Color col)
Definition: stage.hh:2189
ModelCamera(World *world, Model *parent, const std::string &type)
Definition: model_camera.cc:87
Definition: stage.hh:3011
Geom(const Pose &p, const Size &s)
Definition: stage.hh:419
Model class
Definition: stage.hh:1742
Definition: stage.hh:3059
meters_t y
Definition: stage.hh:485
Definition: stage.hh:1785
uint32_t right
Definition: stage.hh:2491
meters_t max_range_anon
maximum detection range
Definition: stage.hh:2724
Definition: stage.hh:1628
Model * Parent() const
Definition: stage.hh:2299
void coord_shift(double x, double y, double z, double a)
Definition: gl.cc:6
RaytraceResult(const Pose &pose, meters_t range)
Definition: stage.hh:750
virtual void Start()
Definition: stage.hh:923
void Shutdown(void)
Definition: model_fiducial.cc:383
msec_t period
duration of a complete cycle
Definition: stage.hh:581
void GoTo(double x, double y, double a)
Definition: model_position.cc:652
Worldfile * GetWorldFile()
Definition: stage.hh:1123
bool autosnatch
if true, cycle the gripper through open-close-up-down automatically
Definition: stage.hh:2614
point3_t(meters_t x, meters_t y, meters_t z)
Definition: stage.hh:486
virtual void Startup()
Definition: model_actuator.cc:293
ModelGripper(World *world, Model *parent, const std::string &type)
Definition: model_gripper.cc:62
Definition: stage.hh:780
void AddColor(Color col)
SuperRegion * CreateSuperRegion(point_int_t origin)
Definition: world.cc:193
void CommitTestedPose()
meters_t size
rendered as a sphere with this diameter
Definition: stage.hh:579
Sensor()
Definition: stage.hh:2786
int subs
the number of subscriptions to this model
Definition: stage.hh:1959
meters_t x
Definition: stage.hh:485
ActuatorType GetType() const
Definition: stage.hh:3109
virtual const char * PrintWithPose() const
Definition: model.cc:695
Event(usec_t time, Model *mod, model_callback_t cb, void *arg)
Definition: stage.hh:1045
static size_t Count()
Definition: stage.hh:793
void Show()
Definition: worldgui.cc:251
Canvas * GetCanvas(void) const
Definition: stage.hh:1592
Definition: stage.hh:493
const std::string & GetWorldfileName()
Definition: stage.hh:562
point_int_t(int x, int y)
Definition: stage.hh:497
bool ztest
Definition: stage.hh:770
cmd_t
Definition: stage.hh:2593
void LoadBlock(Worldfile *wf, int entity)
Definition: model.cc:430
virtual void Visualize(Model *mod, Camera *cam)
Definition: model_position.cc:699
void Enqueue(unsigned int queue_num, usec_t delay, Model *mod, model_callback_t cb, void *arg)
Definition: stage.hh:1075
joules_t stored
Definition: stage.hh:1667
Bounds y
volume extent along y axis, initially zero
Definition: stage.hh:453
virtual void DrawBlocks()
Definition: model_draw.cc:251
void CalcSize()
Definition: blockgroup.cc:60
radians_t heading
center of field of view
Definition: stage.hh:2728
virtual void Load()
Definition: model_position.cc:162
static bool UpdateAll()
Definition: world.cc:231
std::list< Visualizer * > cv_list
Definition: stage.hh:1872
void UnMapWithChildren(unsigned int layer)
Definition: model.cc:632
void Save(Worldfile *wf, int sec)
Definition: camera.cc:98
virtual void Start()
Definition: worldgui.cc:592
virtual void UnLoad()
Definition: world.cc:488
static void Print()
Definition: logentry.cc:18
StripPlotVis(float x, float y, float w, float h, size_t len, Color fgcolor, Color bgcolor, const char *name, const char *wfname)
Definition: vis_strip.cc:9
double CenterY()
Definition: block.cc:74
void CommandOpen()
Definition: stage.hh:2661
Flag * Nibble(double portion)
Definition: model.cc:1172
virtual void PushColor(Color col)
Definition: stage.hh:988
point3_t GetAxis() const
Definition: stage.hh:3110
point_int_t()
Definition: stage.hh:498
const Model * mod
Definition: stage.hh:765
virtual void Draw(void) const =0
int CallCallbacks(callback_type_t type)
Definition: model_callbacks.cc:41
static joules_t global_input
Definition: stage.hh:1687
const std::set< Model * > GetAllModels() const
Definition: stage.hh:1178
std::list< PowerPack * > pps_charging
Definition: stage.hh:1923
bool operator==(const point_int_t &other) const
Definition: stage.hh:508
Model * mod
Definition: stage.hh:1664
static const int DEFAULT_PPM
Definition: stage.hh:937
double a
Definition: stage.hh:200
void update(void)
Definition: camera.cc:75
std::string cmdline
Definition: stage.hh:806
uint64_t updates
the number of simulated time steps executed so far
Definition: stage.hh:912
unsigned int GetFlagCount() const
Definition: stage.hh:2261
ModelRanger(World *world, Model *parent, const std::string &type)
Definition: model_ranger.cc:87
virtual ~Vis(void)
Definition: stage.hh:2768
Definition: stage.hh:225
Definition: stage.hh:2580
void PlaceInFreeSpace(meters_t xmin, meters_t xmax, meters_t ymin, meters_t ymax)
Definition: model.cc:787
Velocity()
Definition: stage.hh:363
const std::string & GetMenuName()
Definition: stage.hh:561
~ModelCamera()
Definition: model_camera.cc:131
bool stack_children
whether child models should be stacked on top of this model or not
Definition: stage.hh:1956
bool graphics
true iff we have a GUI
Definition: stage.hh:900
Model * GetUnusedModelOfType(const std::string &type)
Definition: model.cc:914
Definition: stage.hh:348
const char LICENSE[]
Definition: stage.hh:116
void DrawOriginTree()
Definition: model_draw.cc:219
virtual bool IsGUI() const
Definition: stage.hh:1582
double max
largest value in range, initially zero
Definition: stage.hh:435
Definition: stage.hh:2824
void LoadBlock(Worldfile *wf, int entity)
Definition: world.cc:303
usec_t SimTimeNow(void) const
Definition: stage.hh:1119
World class
Definition: stage.hh:814
double dtor(double d)
Definition: stage.hh:151
void setYaw(double yaw)
Definition: stage.hh:1468
usec_t GetEnergyInterval() const
Definition: stage.hh:2063
std::string String() const
Definition: stage.hh:286
void Save(Worldfile *wf, int section, const char *keyword)
Definition: model.cc:187
Definition: stage.hh:2595
The Stage library uses its own namespace.
Definition: canvas.hh:8
Stg::ModelRanger::Vis vis
bool operator<(const Pose &other) const
Definition: stage.hh:317
void Print(const char *prefix) const
Definition: stage.hh:402
Block * AddBlockRect(meters_t x, meters_t y, meters_t dx, meters_t dy, meters_t dz)
Definition: model.cc:442
std::vector< Fiducial > & GetFiducials()
Definition: stage.hh:2733
Definition: stage.hh:447
static const Color red
Definition: stage.hh:217
void EnableEnergy(Model *m)
Definition: stage.hh:1080
PerspectiveCamera(void)
Definition: camera.cc:17
GLfloat z
Definition: stage.hh:2855
void setPose(double x, double y)
Definition: stage.hh:1483
Color color
Definition: stage.hh:580
void CallUpdateCallbacks(void)
Definition: model.cc:756
void CommandClose()
Definition: stage.hh:2659
std::list< std::pair< world_callback_t, void * > > cb_list
List of callback functions and arguments.
Definition: stage.hh:898
void SetPose(const Pose &pose)
Definition: model.cc:1396
int fiducial_return
Definition: stage.hh:2051
Definition: stage.hh:2487
radians_t fov
Definition: stage.hh:2778
RasterVis()
Definition: model.cc:1036
Definition: stage.hh:2581
virtual void Load()
Definition: model_fiducial.cc:299
static std::vector< std::string > args
Definition: stage.hh:826
bool InitDone()
Definition: stage.cc:41
static const Color magenta
Definition: stage.hh:217
ModelLightIndicator(World *world, Model *parent, const std::string &type)
Definition: model_lightindicator.cc:5
Definition: stage.hh:1849
ActuatorType
Definition: stage.hh:3063
std::string String() const
Definition: model_ranger.cc:229
void CallDisplayList(Model *mod)
Definition: blockgroup.cc:239
radians_t fov
Definition: stage.hh:2513
Definition: stage.hh:1839
void Extend(point3_t pt)
Definition: world.cc:1138
bool IsAntecedent(const Model *testmod) const
Definition: model.cc:545
bool ignore_zloc
Are we ignoring the Z-loc of the fiducials we detect compared to the fiducial detector?
Definition: stage.hh:2730
virtual void Reload()
Definition: world.cc:1015
virtual void Stop()
Definition: stage.hh:924
bool IsDescendent(const Model *testmod) const
Definition: model.cc:557
bool DataIsFresh() const
Definition: stage.hh:2405
meters_t x
Definition: stage.hh:228
void ChargeStop()
Definition: stage.hh:1731
void GLSet(void)
Definition: stage.hh:221
Model * GetModel()
Definition: stage.hh:1262
Size()
Definition: stage.hh:237
void Save(Worldfile *wf, int wf_entity)
Definition: model.cc:214
void DrawTrailArrows()
Definition: model_draw.cc:184
std::list< float * > ray_list
List of rays traced for debug visualization.
Definition: stage.hh:906
virtual void SetProjection(void) const
Definition: camera.cc:123
void RegisterOption(Option *opt)
Register an Option for pickup by the GUI.
Definition: world.cc:1160
uint32_t bottom
Definition: stage.hh:2491
~BlockGroup()
Definition: blockgroup.cc:22
Definition: stage.hh:1967
StripPlotVis output_vis
Definition: stage.hh:1660
double lift_position
0.0 = full down, 1.0 full up
Definition: stage.hh:2610
virtual void Shutdown()
Definition: model_position.cc:592
Model * TestCollision()
Definition: model.cc:800
int getHeight(void) const
height of captured image
Definition: stage.hh:2906
void LoadModel(Worldfile *wf, int entity)
Definition: world.cc:367
void Accumulate(meters_t x, meters_t y, joules_t amount)
Definition: powerpack.cc:316
Definition: stage.hh:1439
void RemovePowerPack(PowerPack *pp)
Definition: world.cc:1154
Definition: stage.hh:2949
Definition: stage.hh:2589
cb_t(model_callback_t cb, void *arg)
Definition: stage.hh:1791
Color GetColor()
Definition: stage.hh:1824
virtual void PushColor(double r, double g, double b, double a)
Definition: stage.hh:990
static joules_t global_capacity
Definition: stage.hh:1685
Visibility()
Definition: model.cc:192
uint32_t GetCount()
Definition: stage.hh:1322
virtual void DrawPicker()
Definition: model_draw.cc:612
void AppendBlock(Block *block)
Definition: blockgroup.cc:27
std::set< Option * > option_table
GUI options (toggles) registered by models.
Definition: stage.hh:902
const char AUTHORS[]
Definition: stage.hh:105
int key
/// only detect fiducials with a key that matches this one (defaults 0)
Definition: stage.hh:2729
virtual void RemoveChild(Model *mod)
Definition: worldgui.cc:414
Definition: stage.hh:2942
void DrawStatusTree(Camera *cam)
Definition: model_draw.cc:357
Definition: stage.hh:680
void Save(Worldfile *wf, int section)
Definition: ancestor.cc:61
void SetGlobalPose(const Pose &gpose)
Definition: model.cc:1349
Block(Model *mod, const std::vector< point_t > &pts, meters_t zmin, meters_t zmax, Color color, bool inherit_color, bool wheel)
Definition: block.cc:13
PowerPack * power_pack
Definition: stage.hh:1919
uint32_t left
Definition: stage.hh:2491
virtual void DrawSelected(void)
Definition: model_draw.cc:12
double _yaw
Definition: stage.hh:1368
const Color & GetColor()
Definition: block.cc:134
Definition: stage.hh:587
virtual void Shutdown()
Definition: model_actuator.cc:300
Definition: stage.hh:3064
CtrlArgs(std::string w, std::string c)
Definition: stage.hh:808
Definition: options_dlg.hh:20
virtual void PushColor(double r, double g, double b, double a)
Definition: stage.hh:2190
virtual void PopColor()
Definition: stage.hh:993
virtual void AddChild(Model *mod)
Definition: ancestor.cc:21
void SetSpeed(double x, double y, double a)
Definition: model_position.cc:610
virtual void Save()
Definition: model_gripper.cc:163
SuperRegion * AddSuperRegion(const point_int_t &coord)
Definition: world.cc:1095
std::string token
Definition: stage.hh:696
void DrawImage(uint32_t texture_id, Camera *cam, float alpha, double width=1.0, double height=1.0)
Definition: model_draw.cc:469
void Init(int *argc, char **argv[])
Definition: stage.cc:18
void DrawOccupancy() const
Definition: worldgui.cc:434
static const Size size
Definition: stage.hh:2638
std::vector< Sensor > & GetSensorsMutable()
Definition: stage.hh:2808
Definition: stage.hh:2772
virtual void UpdateCharge()
Definition: model.cc:816
Definition: region.hh:92
void addPose(double x, double y, double z)
Definition: stage.hh:1413
~ModelLightIndicator()
Definition: model_lightindicator.cc:14
virtual void Update()
Capture a new frame ( calls GetFrame )
Definition: model_camera.cc:164
static const Color blue
Definition: stage.hh:217
int x
Definition: stage.hh:496
virtual ~RasterVis(void)
Definition: stage.hh:1936
Definition: stage.hh:2575
ModelPosition(World *world, Model *parent, const std::string &type)
Definition: model_position.cc:98
Definition: stage.hh:1926
virtual void Load(Worldfile *wf, int sec)=0
Vis(World *world)
Definition: model_blobfinder.cc:301
void TryCharge(PowerPack *pp, const Pose &pose)
void Add(joules_t j)
Definition: powerpack.cc:162
Definition: stage.hh:755
static Color RandomColor()
Definition: color.cc:95
StripPlotVis stored_vis
Definition: stage.hh:1661
meters_t z
Definition: stage.hh:485
double scale() const
Definition: stage.hh:1488
cb_t(world_callback_t cb, void *arg)
Definition: stage.hh:1794
Vis(World *world)
Definition: model_ranger.cc:401
virtual void Update()
Definition: model_blobfinder.cc:179
void AddToPose(const Pose &pose)
Definition: model.cc:782
void SetStall(bool stall)
Definition: model.cc:1271
static joules_t global_stored
Definition: stage.hh:1684
point_t()
Definition: stage.hh:475
usec_t sim_interval
Definition: stage.hh:1087
double _z
Definition: stage.hh:1369
Geom geom
Definition: stage.hh:1883
double _x
Definition: stage.hh:1369
void Zero()
Definition: stage.hh:243
double CenterX()
Definition: block.cc:89
bool used
TRUE iff this model has been returned by GetUnusedModelOfType()
Definition: stage.hh:2003
bool operator==(const Pose &other) const
Definition: stage.hh:322
void Load(Worldfile *wf, int sec)
Definition: camera.cc:92
const std::string type
Definition: stage.hh:1999
static void Clear()
Definition: stage.hh:796
watts_t watts
power consumed by this model
Definition: stage.hh:2005
Definition: stage.hh:2583
void forward(double amount)
Definition: camera.cc:86
Definition: stage.hh:1392
void LoadSensor(Worldfile *wf, int entity)
Definition: world.cc:314
int(* model_callback_t)(Model *mod, void *user)
Definition: stage.hh:568
void EraseAll(T thing, C &cont)
Definition: stage.hh:621
double min
smallest value in range, initially zero
Definition: stage.hh:433
void Draw() const
Definition: model_position.cc:829
const double billion
Definition: stage.hh:145
const char * Version()
Definition: stage.cc:12
double nearClip(void) const
Definition: stage.hh:1429
Size & Load(Worldfile *wf, int section, const char *keyword)
Definition: model.cc:169
void setYaw(double yaw)
Definition: stage.hh:1418
void Map(unsigned int layer)
Definition: block.cc:197
void SetConfig(config_t &newcfg)
Definition: stage.hh:2651
PoseVis()
Definition: model_position.cc:695
Color GetColor() const
Definition: stage.hh:2362
Size size
extent
Definition: stage.hh:395
uint32_t id
Definition: stage.hh:1901
void draw_origin(double len)
Definition: gl.cc:134
ModelBlinkenlight(World *world, Model *parent, const std::string &type)
Definition: model_blinkenlight.cc:60
const double thousand
Definition: stage.hh:139
void Rasterize(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth, meters_t cellheight)
Definition: model.cc:1002
virtual void DrawBlocks()
Definition: model_lightindicator.cc:25
void RemoveAllColors()
Definition: model_blobfinder.cc:142
virtual void DataVisualize(Camera *cam)
Draw Camera Model - TODO.
Definition: model_camera.cc:244
bool operator<(const Event &other) const
Definition: world.cc:1173
void AddFlag(Flag *flag)
Definition: model.cc:381
double GetSpeed() const
Definition: stage.hh:3099
Definition: stage.hh:482
double y(void) const
Definition: stage.hh:1382
void Print(const char *prefix) const
Definition: stage.hh:1713
meters_t map_resolution
Definition: stage.hh:1908
Definition: stage.hh:1836
int update_cb_count
Definition: stage.hh:1092
virtual void Visualize(Model *mod, Camera *cam)
Definition: model_ranger.cc:411
virtual void Load()
Definition: model_actuator.cc:92
void AppendTouchingModels(std::set< Model * > &touchers)
Definition: model.cc:794
Pose & Load(Worldfile *wf, int section, const char *keyword)
Definition: model.cc:180
Bounds velocity_bounds[4]
Definition: stage.hh:2966
double yaw(void) const
Definition: stage.hh:1378
uint32_t top
Definition: stage.hh:2491
double duty_cycle
mark/space ratio
Definition: stage.hh:582
void SetZSpeed(double z)
Definition: model_position.cc:631
SuperRegion * GetSuperRegionCreate(const point_int_t &org)
Definition: world.cc:1125
void SetColor(const Color &col)
Definition: model.cc:1187
Flag(const Color &color, double size)
Definition: model.cc:1167
ray_test_func_t func
Definition: stage.hh:768
void Visualize(Vis *vis, ModelRanger *rgr) const
Definition: model_ranger.cc:237
uint64_t UpdateCount()
Definition: stage.hh:919
void draw_string_multiline(float x, float y, float w, float h, const char *string, Fl_Align align)
Definition: gl.cc:77
Ancestor & Load(Worldfile *wf, int section)
Definition: ancestor.cc:56
watts_t watts_take
Definition: stage.hh:2013
meters_t range
Definition: stage.hh:2515
void setPose(double x, double y, double z)
Definition: stage.hh:1412
void SetGuiGrid(bool val)
Definition: model.cc:1328
ModelCamera class
Definition: stage.hh:2849
void SetYSpeed(double y)
Definition: model_position.cc:625
double ProportionRemaining() const
Definition: stage.hh:1708
virtual void Redraw(void)
Definition: worldgui.cc:586
unsigned int event_queue_num
Definition: stage.hh:2002
usec_t interval_energy
time between updates of powerpack in usec
Definition: stage.hh:1903
void LoadBlockGroup(Worldfile *wf, int entity)
Definition: stage.hh:2590
const double million
Definition: stage.hh:142
double _pitch
Definition: stage.hh:1367
void GoTo(double pose)
Definition: model_actuator.cc:319
void DestroySuperRegion(SuperRegion *sr)
Definition: world.cc:201
usec_t time
time that event occurs
Definition: stage.hh:1048
void SetCenter(double x, double y)
Definition: block.cc:104
virtual void PopColor()
Definition: stage.hh:2191
void Load(Worldfile *wf, int wf_entity)
Definition: stage.hh:2234
model_callback_t cb
Definition: stage.hh:1050
void Visualize(Camera *cam)
Definition: powerpack.cc:52
void RemoveFlag(Flag *flag)
Definition: model.cc:390
virtual void RemoveChild(Model *mod)
Definition: ancestor.cc:35
Bounds(double min, double max)
Definition: stage.hh:438
RaytraceResult Raytrace(const Pose &pose, const meters_t range, const ray_test_func_t func, const void *arg, const bool ztest=true)
Definition: model.cc:475
std::vector< TrailItem > trail
Definition: stage.hh:1982
double ranger_return
Definition: stage.hh:2054
void setFov(double horiz_fov, double vert_fov)
Definition: stage.hh:1415
Model * GetUnsubscribedModelOfType(const std::string &type) const
Definition: model.cc:882
void Dissipate(joules_t j)
Definition: powerpack.cc:244
kg_t mass
Definition: stage.hh:1909
void SetMapResolution(meters_t res)
Definition: model.cc:1343
std::map< std::string, unsigned int > child_type_counts
Definition: stage.hh:687
void Say(const std::string &str)
Definition: model.cc:539
lift_state_t
Definition: stage.hh:2586
Stg::ModelPosition::PoseVis posevis
kg_t GetTotalMass() const
Definition: model.cc:937
usec_t last_time
Definition: stage.hh:1679
void DrawSolid(bool topview)
Definition: block.cc:368
void DataVisualizeTree(Camera *cam)
Definition: model_draw.cc:632
void ForEachDescendant(model_callback_t func, void *arg)
Definition: ancestor.cc:46
bool nose
Definition: stage.hh:1891
float s
Definition: glutgraphics.cc:58
virtual ~Pose()
Definition: stage.hh:264
void RegisterModels()
Definition: typetable.cc:19
void reset(void)
Definition: stage.hh:1486
void LoadControllerModule(const char *lib)
Definition: model.cc:1672
~ModelPosition()
Definition: model_position.cc:150
TrailItem()
Definition: stage.hh:1974
bool thread_safe
Definition: stage.hh:1964
virtual void Visualize(Model *mod, Camera *cam)=0
void DrawFootPrint(const Geom &geom)
Definition: blockgroup.cc:130
virtual ~PoseVis(void)
Definition: stage.hh:3015
Definition: stage.hh:2587
virtual ~WaypointVis(void)
Definition: stage.hh:3007
World * GetWorld() const
Definition: stage.hh:2302
void AddUpdateCallback(world_callback_t cb, void *user)
Definition: world.cc:538
meters_t range
Definition: stage.hh:2492
Color col
Definition: stage.hh:2780
void Save(Worldfile *wf, int sec)
Definition: camera.cc:213
usec_t time
Definition: stage.hh:1970
meters_t z
location in 3 axes
Definition: stage.hh:251
meters_t y
Definition: stage.hh:251
void DrawBoundingBoxTree()
Definition: model_draw.cc:256
Definition: file_manager.hh:9
virtual void Load(const std::string &filename)
Definition: worldgui.cc:256
Worldfile * wf
Definition: stage.hh:2015
bool charging
Definition: stage.hh:1673
ModelBlobfinder(World *world, Model *parent, const std::string &type)
Definition: model_blobfinder.cc:82
point_t(meters_t x, meters_t y)
Definition: stage.hh:474
Definition: stage.hh:2596
ModelPosition class
Definition: stage.hh:2927
Definition: stage.hh:429
void InvalidateModelPointCache()
Definition: stage.hh:1356
virtual void Update()
Definition: model_blinkenlight.cc:104
void MapFromRoot(unsigned int layer)
Definition: model.cc:627
void Clear()
Definition: blockgroup.cc:32
void SetStickyReturn(bool val)
void SetVelocity(const Velocity &val)
Definition: model_position.cc:155
void SetWorldfile(Worldfile *wf, int wf_entity)
Definition: stage.hh:2243
void Load(Worldfile *wf, int entity)
Definition: block.cc:396
double z(void) const
Definition: stage.hh:1383
static void * update_thread_entry(std::pair< World *, int > *info)
Definition: world.cc:246
void RemoveColor(Color col)
Definition: model_blobfinder.cc:131
OrthoCamera(void)
Definition: stage.hh:1449
Definition: stage.hh:575
bool disabled
Definition: stage.hh:1869
Color color
Definition: stage.hh:2998
Pose pose
Definition: stage.hh:589
double watts_t
Definition: stage.hh:195
Definition: stage.hh:2598
Definition: stage.hh:1813
int y
Definition: stage.hh:496
virtual void Shutdown()
Definition: model_blobfinder.cc:281
Model * mod
Pointer to the model (real fiducial detectors can't do this!)
Definition: stage.hh:2699
Definition: stage.hh:2936
void RemoveVisualizer(Visualizer *custom_visual)
Definition: model_draw.cc:348
void SetSpeed(double speed)
Definition: model_actuator.cc:312
bool IsRelated(const Model *testmod) const
Definition: model.cc:570
void DrawOrigin()
Bounds range
Definition: stage.hh:2777
joules_t last_joules
Definition: stage.hh:1680
void CancelQuit()
Definition: stage.hh:1162
Definition: stage.hh:1843
void pose_inverse_shift(const Pose &pose)
Definition: gl.cc:18
Pose origin
Definition: stage.hh:766
Definition: stage.hh:391
double normalize(double a)
Definition: stage.hh:154
virtual void Load()
Definition: model_ranger.cc:156
const Color & Load(Worldfile *wf, int entity)
Definition: color.cc:105
int wf_entity
Definition: stage.hh:2016
bool move
Definition: stage.hh:1890
Model * Root()
Definition: stage.hh:2305
std::vector< double > bearings
Definition: stage.hh:2784
bool operator<(const point_int_t &other) const
Definition: stage.hh:501
lift_state_t lift
Definition: stage.hh:2608
double GetMinPosition() const
Definition: stage.hh:3107
Velocity GetGlobalVelocity() const
WorldGui * world_gui
Definition: stage.hh:2018
void RegisterOption(Option *opt)
Definition: model.cc:996
Model * mod
model to pass into callback
Definition: stage.hh:1049
void RecordRay(double x1, double y1, double x2, double y2)
Definition: world.cc:722
static unsigned int trail_length
Definition: stage.hh:1990
static int UpdateWrapper(Model *mod, void *arg)
Definition: stage.hh:2152
virtual ~Visualizer(void)
Definition: stage.hh:558
static int argc
Definition: glutgraphics.cc:238
usec_t GetUpdateInterval() const
Definition: stage.hh:2062
Pose est_pose
position estimate in local coordinates
Definition: stage.hh:3044
usec_t sim_time
the current sim time in this world in microseconds
Definition: stage.hh:907
GuiState & Load(Worldfile *wf, int wf_entity)
Definition: model.cc:232
void draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset, float min, float max)
Definition: gl.cc:24
void draw_centered_rect(float x, float y, float dx, float dy)
Definition: gl.cc:121
void ChargeStart()
Definition: stage.hh:1730
void SetXSpeed(double x)
Definition: model_position.cc:619
unsigned int sample_count
Definition: stage.hh:2779
Definition: stage.hh:2941
Flag * PopFlag()
Definition: model.cc:408
void setClip(double near, double far)
Definition: stage.hh:1431
void AddCallback(callback_type_t type, model_callback_t cb, void *user)
Definition: model_callbacks.cc:6
void addYaw(double yaw)
Definition: stage.hh:1472
joules_t GetCapacity() const
Definition: powerpack.cc:222
Size paddle_size
paddle dimensions
Definition: stage.hh:2606
virtual void AddModel(Model *mod)
Definition: world.cc:284
void Subtract(joules_t j)
Definition: powerpack.cc:171
virtual void Print(char *prefix) const
Definition: model.cc:680
virtual bool Update(void)
Definition: world.cc:624
Definition: stage.hh:741
~PowerPack()
Definition: powerpack.cc:43
double r
Definition: stage.hh:200
virtual void TogglePause()
Definition: stage.hh:925
int rotrects_from_image_file(const std::string &filename, std::vector< rotrect_t > &rects)
Definition: stage.cc:102
Definition: stage.hh:1841
void DrawTrailFootprint()
Definition: model_draw.cc:126
static Option showArea
Definition: stage.hh:2761
std::string PoseString()
Definition: stage.hh:2202
int RemoveCallback(callback_type_t type, model_callback_t callback)
Definition: model_callbacks.cc:22
Pose GetGlobalPose() const
Definition: model.cc:1379
virtual void Draw() const
Definition: camera.cc:110
static Option showBeams
Definition: stage.hh:2764
Model()
Definition: stage.hh:2219
double rtod(double r)
Definition: stage.hh:148
Model * beam[2]
points to a model detected by the beams
Definition: stage.hh:2616
static Pose Random(meters_t xmin, meters_t xmax, meters_t ymin, meters_t ymax)
Definition: stage.hh:268
void SetData(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth, meters_t cellheight)
Definition: model.cc:1136
void ClearRays()
Definition: world.cc:732
std::vector< point_int_t > LocalToPixels(const std::vector< point_t > &local) const
Definition: model.cc:600
radians_t bearing
bearing to the target
Definition: stage.hh:2695
Stg::ModelPosition::WaypointVis wpvis
~ModelActuator()
Definition: model_actuator.cc:87
Definition: stage.hh:1837
void addPitch(double pitch)
Definition: stage.hh:1474
meters_t range
Definition: stage.hh:767
Definition: stage.hh:2588
Ray()
Definition: stage.hh:762
double GetSize()
Definition: stage.hh:1825
virtual void Visualize(Model *mod, Camera *cam)
Definition: model.cc:1048
double realDistance(double z_buf_val) const
Definition: stage.hh:1425
void Draw(GLUquadric *quadric)
Definition: model.cc:1212
void SetGravityReturn(bool val)
void LoadBitmap(Model *mod, const std::string &bitmapfile, Worldfile *wf)
Definition: blockgroup.cc:256
int used
Definition: stage.hh:1946
Pose pose
Definition: stage.hh:1971
Pose pose
position
Definition: stage.hh:394
void LoadSensor(Worldfile *wf, int entity)
Definition: model_ranger.cc:131
const bounds3d_t & GetExtent() const
Definition: stage.hh:1181
bool obstacle_return
Definition: stage.hh:2053
Camera()
Definition: stage.hh:1372
std::map< point_int_t, SuperRegion * > superregions
Definition: stage.hh:908
void PushFlag(Flag *flag)
Definition: model.cc:399
meters_t range
range to beam hit in meters
Definition: stage.hh:745
#define PRINT_ERR(m)
Definition: stage.hh:625
void DrawVoxels() const
Definition: worldgui.cc:454
void SetCenterY(double y)
Definition: block.cc:111
const Size & GetSize()
Definition: stage.hh:1323
int getWidth(void) const
width of captured image
Definition: stage.hh:2903
virtual void reset()=0
std::string EnergyString(void) const
Definition: worldgui.cc:421
model_callback_t callback
Definition: stage.hh:1788
Definition: stage.hh:2852
meters_t y
Definition: stage.hh:473
meters_t max_range_id
maximum range at which the ID can be read
Definition: stage.hh:2725
virtual ~StripPlotVis()
Definition: vis_strip.cc:30
bool Stalled() const
Definition: stage.hh:2457
std::string GetSayString()
Definition: stage.hh:2037
Definition: stage.hh:1498
Definition: stage.hh:1845
uint64_t usec_t
Definition: stage.hh:186
Definition: stage.hh:1635
bounds3d_t extent
Describes the 3D volume of the world.
Definition: stage.hh:899
void Zero()
Definition: stage.hh:299
void LoadBlock(Model *mod, Worldfile *wf, int entity)
Definition: blockgroup.cc:250
void draw_grid(bounds3d_t vol)
Definition: gl.cc:141
double meters_t
Definition: stage.hh:174
bool GetCharging() const
Definition: stage.hh:1728
virtual void Load()
Definition: model_gripper.cc:113
virtual Model * RecentlySelectedModel() const
Definition: stage.hh:967
void ClearPts()
Definition: model.cc:1161
joules_t dissipated
Definition: stage.hh:1676
Definition: stage.hh:1197
void Load(Worldfile *wf, int entity)
Definition: model_ranger.cc:141
void draw_vector(double x, double y, double z)
Definition: gl.cc:126
void PushLocalCoords()
Definition: model_draw.cc:307
void DrawTrailBlocks()
Definition: model_draw.cc:163
meters_t x
Definition: stage.hh:473
const char DESCRIPTION[]
Definition: stage.hh:112
usec_t interval
time between updates in usec
Definition: stage.hh:1902
radians_t pan
Definition: stage.hh:2514
RaytraceResult Raytrace(const Ray &ray)
Definition: world.cc:775
static const Color cyan
Definition: stage.hh:217
std::map< std::string, void * > props
Definition: stage.hh:694
Definition: stage.hh:248
meters_t y
Definition: stage.hh:228
int32_t MetersToPixels(meters_t x) const
Definition: stage.hh:981
Pose operator+(const Pose &p) const
Definition: stage.hh:305
Definition: stage.hh:1041
LogEntry(usec_t timestamp, Model *mod)
Definition: logentry.cc:7
Model * TestCollision()
Definition: block.cc:153
Model * ground
Definition: stage.hh:951
Pose pose
Definition: stage.hh:578
const std::vector< Sensor > & GetSensors() const
Definition: stage.hh:2804
void pose_shift(const Pose &pose)
Definition: gl.cc:13
Definition: stage.hh:1846
void Save(Worldfile *wf, int section, const char *keyword) const
Definition: model.cc:175
Definition: stage.hh:470
std::vector< Blob > GetBlobs() const
Definition: stage.hh:2537
std::vector< point_int_t > rt_cells
Definition: stage.hh:932
virtual void Save()
Definition: model.cc:1633
ControlMode
Definition: stage.hh:2933
bool operator<(const cb_t &other) const
Definition: stage.hh:1800
Color color
the color struck by this beam
Definition: stage.hh:747
void CommandUp()
Definition: stage.hh:2663
virtual void Visualize(Model *mod, Camera *cam)
Definition: powerpack.cc:284
virtual void Load()
Definition: model_blinkenlight.cc:94
void SetGlobalVelocity(const Velocity &gvel)
Visualizer(const std::string &menu_name, const std::string &worldfile_name)
Definition: stage.hh:552
double Resolution() const
Definition: stage.hh:1171
void SetProperty(std::string &key, void *value)
Definition: stage.hh:729
void SetCommand(cmd_t cmd)
Definition: stage.hh:2657
void CommandDown()
Definition: stage.hh:2665
void Stop()
Definition: model_position.cc:605
int SetParent(Model *newparent)
Definition: model.cc:1354
void SetZ(double min, double max)
Definition: block.cc:125
void SetBlobReturn(bool val)
Definition: model.cc:1303
void AppendTouchingModels(std::set< Model * > &touchers)
Definition: block.cc:139
std::vector< Waypoint > waypoints
Definition: stage.hh:3001
virtual void Print(char *prefix) const
Definition: model_ranger.cc:371
virtual void Load()
Definition: model_camera.cc:143
void AddVisualizer(Visualizer *custom_visual, bool on_by_default)
Definition: model_draw.cc:322
void Redraw()
Definition: model.cc:909
meters_t z
Definition: stage.hh:228
Pose pose
Absolute accurate position of the target in world coordinates (it's cheating to use this in robot con...
Definition: stage.hh:2698
unsigned int GetSubscriptionCount() const
Definition: stage.hh:2461
static std::vector< LogEntry > log
Definition: stage.hh:790
double GetMaxPosition() const
Definition: stage.hh:3106
virtual void Startup()
Definition: model.cc:707
radians_t angle
width of viewing angle of sensor
Definition: stage.hh:466
double _y
Definition: stage.hh:1369
virtual Model * RecentlySelectedModel() const
Definition: worldgui.cc:866
void addPitch(double pitch)
Definition: stage.hh:1423
watts_t watts_give
Definition: stage.hh:2009
const char COPYRIGHT[]
Definition: stage.hh:101
double b
Definition: stage.hh:200
Model * contact[2]
pointers to a model detected by the contacts
Definition: stage.hh:2617
virtual void SetToken(const std::string &str)
Definition: stage.hh:2022
double close_limit
How far the gripper can close. If < 1.0, the gripper has its mouth full.
Definition: stage.hh:2613
virtual void Visualize(Model *mod, Camera *cam)
Definition: model_position.cc:762
void Subscribe()
Definition: model.cc:646
void SetRangerReturn(double val)
Definition: model.cc:1308
virtual void Startup()
Definition: model_blobfinder.cc:271
WorldGui(int W, int H, const char *L=0)
Definition: worldgui.cc:187
void Enable()
Definition: stage.hh:2271
virtual void SetProjection(void) const =0
Definition: stage.hh:1844
Pose est_pose_error
estimated error in position estimate
Definition: stage.hh:3045
Velocity & Load(Worldfile *wf, int section, const char *keyword)
Definition: stage.hh:372
Worldfile * wf
If set, points to the worldfile used to create this world.
Definition: stage.hh:913
Definition: stage.hh:168
bool blob_return
Definition: stage.hh:2049
virtual ~World()
Definition: world.cc:185
Definition: option.hh:19
void SetFriction(double friction)
Definition: model.cc:1013
void AddPoint(meters_t x, meters_t y)
Definition: model.cc:1156
bool(* ray_test_func_t)(Model *candidate, Model *finder, const void *arg)
Definition: stage.hh:603
bool has_default_block
Definition: stage.hh:1898
void draw_string(float x, float y, float z, const char *string)
Definition: gl.cc:65
void SetSize(double sz)
Definition: model.cc:1199
bool stall
Set to true iff the model collided with something else.
Definition: stage.hh:1958
static char * argv
Definition: glutgraphics.cc:239
static Model * LookupId(uint32_t id)
Definition: stage.hh:2206
void SetFiducialReturn(int fid)
Definition: model.cc:1281
void Map(unsigned int layer)
Definition: blockgroup.cc:97
virtual void Update()
Definition: model.cc:735
double g
Definition: stage.hh:200
double horizFov(void) const
Definition: stage.hh:1419
void SetGripperReturn(bool val)
Definition: model.cc:1276
void Update(ModelRanger *rgr)
Definition: model_ranger.cc:185
void NeedRedraw()
Definition: model.cc:899
virtual ~Model()
Definition: model.cc:356
Definition: stage.hh:1842
Bounds x
volume extent along x axis, intially zero
Definition: stage.hh:451
void TransferTo(PowerPack *dest, joules_t amount)
Definition: powerpack.cc:185
virtual void Stop()
Definition: worldgui.cc:619
virtual void Visualize(Model *mod, Camera *cam)
Definition: model_blobfinder.cc:311
Bounds range
min and max range of sensor
Definition: stage.hh:465
void Map()
Definition: stage.hh:2099
usec_t GetInterval()
Definition: stage.hh:2044
virtual void DataVisualize(Camera *cam)
Definition: model_blinkenlight.cc:112
virtual ~Camera()
Definition: stage.hh:1373
void NeedRedraw()
Definition: stage.hh:951
unsigned int trail_index
Definition: stage.hh:1985
Pose(meters_t x, meters_t y, meters_t z, radians_t a)
Definition: stage.hh:254
void Disable()
Definition: stage.hh:2267
Bounds acceleration_bounds[4]
Definition: stage.hh:2963
std::vector< Model * > children
Definition: stage.hh:689
void Unsubscribe()
Definition: model.cc:659
bool alwayson
Definition: stage.hh:1771
virtual bool Update()
Definition: worldgui.cc:354
virtual bool IsGUI() const
Definition: stage.hh:1128
bool paused
if true, the simulation is stopped
Definition: stage.hh:921
virtual void PopColor()
Definition: worldgui.cc:863
unsigned long msec_t
Definition: stage.hh:183
double x(void) const
Definition: stage.hh:1381
void Rasterize(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth, meters_t cellheight)
Definition: blockgroup.cc:318
const GLfloat * FrameDepth() const
get a reference to camera depth buffer
Definition: stage.hh:2912
Definition: stage.hh:463
ModelFiducial class
Definition: stage.hh:2687
PowerPack(Model *mod)
Definition: powerpack.cc:18
joules_t GetStored() const
Definition: powerpack.cc:227
std::vector< std::set< cb_t > > callbacks
Definition: stage.hh:1856
Pose pose
Definition: stage.hh:2775
virtual void SetProjection(void) const
Definition: camera.cc:54
virtual void Startup()
Definition: model_ranger.cc:115
virtual void PushColor(Color col)
Definition: worldgui.cc:857
int blocks_dl
Definition: stage.hh:1775
std::vector< std::priority_queue< Event > > event_queues
Definition: stage.hh:1059
virtual ~DissipationVis()
Definition: powerpack.cc:280
virtual void Startup()
Definition: model_position.cc:583
virtual void Load()
Definition: model.cc:1422
class Stg::Model::Visibility vis
friend class WorkerThread
Definition: stage.hh:821
const PerspectiveCamera & getCamera(void) const
get reference to camera used
Definition: stage.hh:2909
virtual void Print(const char *prefix) const
Definition: stage.hh:280
bounds3d_t(const Bounds &x, const Bounds &y, const Bounds &z)
Definition: stage.hh:458
virtual void SetToken(const std::string &str)
Definition: stage.hh:718
Definition: stage.hh:197
void DrawGrid()
Definition: model_draw.cc:655
virtual void Update()
Definition: model_actuator.cc:184
virtual void RemoveModel(Model *mod)
Definition: world.cc:295
double pitch(void) const
Definition: stage.hh:1379
meters_t Distance2D(const Pose &other) const
Definition: stage.hh:338
ModelRanger class
Definition: stage.hh:2747
void SetState(bool isOn)
Definition: model_lightindicator.cc:19
Velocity GetVelocity() const
Definition: stage.hh:2984
void strafe(double amount)
Definition: camera.cc:80
void UnMapFromRoot(unsigned int layer)
Definition: model.cc:641
virtual ~ModelFiducial()
Definition: model_fiducial.cc:99
void Rasterize(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth, meters_t cellheight)
Definition: block.cc:262
BlockGroup blockgroup
Definition: stage.hh:1773
bool paddles_stalled
Definition: stage.hh:2612
LocalizationMode
Definition: stage.hh:2940
Bounds z
volume extent along z axis, initially zero
Definition: stage.hh:455
Stg::Model::RasterVis rastervis
void Zero()
Definition: stage.hh:421
Pose pose
Definition: stage.hh:1916
Definition: stage.hh:802
bool operator==(const cb_t &other) const
Definition: stage.hh:1809
Definition: stage.hh:1303
static void Run()
Definition: world.cc:207
~WorldGui()
Definition: worldgui.cc:243
std::string worldfile
Definition: stage.hh:805
void setScale(double scale)
Definition: stage.hh:1482
static std::map< std::string, creator_t > name_map
Definition: stage.hh:2466
virtual Pose GetGlobalPose() const
Definition: ancestor.cc:41
const char WEBSITE[]
Definition: stage.hh:109
void DrawFlagList()
Definition: model_draw.cc:522
bool debug
Definition: stage.hh:691
uint32_t id_t
Definition: stage.hh:171
Pose pose
Definition: stage.hh:2997
bool PastQuitTime()
Definition: world.cc:506
static Option showTransducers
Definition: stage.hh:2765
void InitControllers()
Definition: model.cc:375
void SetWatts(watts_t watts)
Definition: model.cc:1338
void ShowClock(bool enable)
Control printing time to stdout.
Definition: stage.hh:1190
ControlMode
Definition: stage.hh:3057
void SetGuiMove(bool val)
Definition: model.cc:1323
virtual void DataVisualize(Camera *cam)
Definition: model_draw.cc:627
Pose geom
size and relative angle of the target
Definition: stage.hh:2696
World * world
Definition: stage.hh:2017
bool rebuild_displaylist
iff true, regenerate block display list before redraw
Definition: stage.hh:1953
Color()
Definition: color.cc:13
bool operator!=(const Color &other) const
Definition: color.cc:17
void setPitch(double pitch)
Definition: stage.hh:1470
kg_t GetMassOfChildren() const
Definition: model.cc:947
bool IsZero() const
Definition: stage.hh:295
Definition: stage.hh:2758
Model * GetModel(const std::string &name) const
Definition: world.cc:707
std::set< Model * > active_energy
Definition: stage.hh:1079
usec_t RealTimeNow(void) const
Definition: worldgui.cc:869
void scroll(double dy)
Definition: stage.hh:1428
int GetFiducialReturn() const
Definition: stage.hh:2356
bool data_fresh
Definition: stage.hh:1864
void CancelQuitAll()
Definition: stage.hh:1165
std::string say_string
if non-empty, this string is displayed in the GUI
Definition: stage.hh:1954
int enabled
Definition: stage.hh:577
Definition: stage.hh:546
std::list< PowerPack * > powerpack_list
List of all the powerpacks attached to models in the world.
Definition: stage.hh:903
void addYaw(double yaw)
Definition: stage.hh:1421
Definition: stage.hh:3065
Definition: stage.hh:1364
void ConsumeQueue(unsigned int queue_num)
Definition: world.cc:599
std::vector< std::queue< Model * > > pending_update_callbacks
Definition: stage.hh:1062
void DrawBlocksTree()
Definition: model_draw.cc:228
void draw_speech_bubble(float x, float y, float z, const char *str)
Definition: gl.cc:83
void SetCapacity(joules_t j)
Definition: powerpack.cc:208
void UnMap(unsigned int layer)
Definition: blockgroup.cc:106
bool operator==(const Color &other) const
Definition: color.cc:90
Definition: stage.hh:2046
const void * arg
Definition: stage.hh:769
DriveMode
Definition: stage.hh:2946
void Load(Worldfile *wf, int sec)
Definition: camera.cc:206
double Constrain(double value)
Definition: model.cc:164
const std::string & TokenStr() const
Definition: stage.hh:716
std::list< Flag * > flag_list
Definition: stage.hh:1875
void DrawBoundingBoxTree()
Definition: worldgui.cc:851
joules_t RemainingCapacity() const
Definition: powerpack.cc:157
void Log(Model *mod)
Definition: world.cc:1166
void DrawBoundingBox()
Definition: model_draw.cc:267
static Option showFov
Definition: stage.hh:2763
Definition: stage.hh:3003
void Quit()
Definition: stage.hh:1156
Size size
Definition: stage.hh:590
bool HasSubscribers() const
Definition: stage.hh:2464
double break_beam_inset[2]
distance from the end of the paddle
Definition: stage.hh:2615
void QuitAll()
Definition: stage.hh:1159
void SetFiducialKey(int key)
Definition: model.cc:1293
Definition: stage.hh:2935
std::vector< Model * > & GetChildren()
Definition: stage.hh:706
PowerPack * FindPowerPack() const
Definition: model.cc:985
void UnMap()
Definition: stage.hh:2104
void SetStored(joules_t j)
Definition: powerpack.cc:237
WaypointVis()
Definition: model_position.cc:758
void SetGuiNose(bool val)
Definition: model.cc:1318
void MapWithChildren(unsigned int layer)
Definition: model.cc:617
double constrain(double val, double minval, double maxval)
Definition: stage.cc:235
Pose GlobalToLocal(const Pose &pose) const
Definition: model.cc:525
static const Color green
Definition: stage.hh:217
static const Color yellow
Definition: stage.hh:217
Definition: worldfile.hh:69
ModelActuator(World *world, Model *parent, const std::string &type)
Definition: model_actuator.cc:62
void Translate(double x, double y)
Definition: block.cc:63
BlockGroup()
Definition: blockgroup.cc:13
struct timeval time_t
Definition: stage.hh:180
void DrawSolid(const Geom &geom)
Definition: blockgroup.cc:112
joules_t capacity
Definition: stage.hh:1670
void SetGeom(const Geom &src)
Definition: model.cc:1243
DissipationVis(meters_t width, meters_t height, meters_t cellsize)
Definition: powerpack.cc:267
void AppendValue(float value)
Definition: vis_strip.cc:56
void DrawPose(Pose pose)
Definition: model_draw.cc:239
Bounds & Load(Worldfile *wf, int section, const char *keyword)
Definition: model.cc:158
virtual void Shutdown()
Definition: model.cc:723
Definition: stage.hh:2691
const GLubyte * FrameColor() const
get a reference to camera color image. 4 bytes (RGBA) per pixel
Definition: stage.hh:2915
std::set< ModelPosition * > active_velocity
Definition: stage.hh:1081
Geom()
Definition: stage.hh:416
void SetCenterX(double y)
Definition: block.cc:118
#define FOR_EACH(I, C)
Definition: stage.hh:616
paddle_state_t paddles
Definition: stage.hh:2607
void draw_octagon(float w, float h, float m)
Definition: gl.cc:90
Definition: stage.hh:1838
~ModelBlobfinder()
Definition: model_blobfinder.cc:103
Definition: region.hh:57
SuperRegion * GetSuperRegion(const point_int_t &org)
Definition: world.cc:1112
virtual void Print(const char *prefix) const
Definition: stage.hh:378
virtual void Draw(void) const
Definition: camera.cc:42
Pose est_origin
global origin of the local coordinate system
Definition: stage.hh:3046
Pose GetPose() const
Definition: stage.hh:2382
ModelActuator class
Definition: stage.hh:3053
ModelFiducial(World *world, Model *parent, const std::string &type)
Definition: model_fiducial.cc:67
class Stg::Model::GuiState gui
Definition: stage.hh:1605
const point3_t & GetOffset()
Definition: stage.hh:1324
radians_t a
rotation about the z axis.
Definition: stage.hh:252
Definition: stage.hh:2948
static Option showStrikes
Definition: stage.hh:2762
Definition: canvas.hh:10
meters_t min_range
minimum detection range
Definition: stage.hh:2726
void AppendTouchingModels(std::set< Model * > &touchers)
Definition: blockgroup.cc:40
~Block()
Definition: block.cc:54
void DisableEnergy(Model *m)
Definition: stage.hh:1081
Definition: stage.hh:1840
void setAspect(double aspect)
update vertical fov based on window aspect and current horizontal fov
Definition: stage.hh:1417
std::vector< meters_t > ranges
Definition: stage.hh:2782
bool operator!=(const Pose &other) const
Definition: stage.hh:330
void * arg
Definition: stage.hh:1051
Definition: stage.hh:2555
int(* world_callback_t)(World *world, void *user)
Definition: stage.hh:570
const std::string & GetModelType() const
Definition: stage.hh:2036
void setPitch(double pitch)
Definition: stage.hh:1422
double friction
Definition: stage.hh:1879
Model * CreateModel(Model *parent, const std::string &typestr)
Definition: world.cc:328
Definition: stage.hh:3058
std::vector< double > intensities
Definition: stage.hh:2783
Stg::ModelBlobfinder::Vis vis
void CallUpdateCallbacks()
Call all calbacks in cb_list, removing any that return true;.
Definition: world.cc:565
virtual bool Save(const char *filename)
Definition: world.cc:1002
GuiState()
Definition: model.cc:225
Definition: stage.hh:2594
void AddModelName(Model *mod, const std::string &name)
Definition: world.cc:290
Model * TestCollision()
Definition: blockgroup.cc:46
void SetMass(kg_t mass)
Definition: model.cc:1266
Model * GetChild(const std::string &name) const
Definition: model.cc:1018
virtual void Visualize(Model *mod, Camera *cam)
Definition: vis_strip.cc:36
void move(double x, double y, double z)
Definition: camera.cc:26
double vertFov(void) const
Definition: stage.hh:1420
void setPitch(double pitch)
change the pitch
Definition: stage.hh:2918
Definition: stage.hh:2597
virtual void Load()
Definition: model_blobfinder.cc:147
uint64_t GetUpdateCount() const
Definition: stage.hh:1184
double kg_t
Definition: stage.hh:189
static uint64_t trail_interval
Definition: stage.hh:1993
double paddle_position
0.0 = full open, 1.0 full closed
Definition: stage.hh:2609
bool IsTopView()
Definition: worldgui.cc:876
bool Paused() const
Definition: stage.hh:927
Color color
Definition: stage.hh:2490
Color color
Definition: stage.hh:1859
int boundary
Definition: stage.hh:1780
virtual ~ModelGripper()
Definition: model_gripper.cc:107
bool log_state
iff true, model state is logged
Definition: stage.hh:1907
bool gripper_return
Definition: stage.hh:2052
Definition: stage.hh:2604
unsigned int GetEventQueue(Model *mod) const
Definition: world.cc:697
virtual void Shutdown()
Definition: model_ranger.cc:122
watts_t last_watts
Definition: stage.hh:1681
bool outline
Definition: stage.hh:1892
static std::string ctrlargs
Definition: stage.hh:827
virtual void Redraw(void)
Definition: stage.hh:932
config_t GetConfig()
Definition: stage.hh:2654
static joules_t global_dissipated
Definition: stage.hh:1686
std::vector< std::vector< Model * > > update_lists
Definition: stage.hh:910
void move(double x, double y)
Definition: camera.cc:144
void setYaw(double yaw)
change the yaw
Definition: stage.hh:2921
Definition: stage.hh:2989
double joules_t
Definition: stage.hh:192
World(const std::string &name="MyWorld", double ppm=DEFAULT_PPM)
Definition: world.cc:123
virtual void Update()
Definition: model_ranger.cc:176
Definition: stage.hh:2495
Pose LocalToGlobal(const Pose &pose) const
Definition: stage.hh:2435
bool operator+=(const point_t &other)
Definition: stage.hh:477
cb_t()
Definition: stage.hh:1797
uint32_t GetId() const
Definition: stage.hh:2365
joules_t GetDissipated() const
Definition: powerpack.cc:232
Model * mod
the model struck by this beam
Definition: stage.hh:746
Definition: region.hh:34
RaytraceResult()
Definition: stage.hh:749
virtual void Move()
Definition: model_position.cc:535
virtual void DrawStatus(Camera *cam)
Definition: model_draw.cc:366
void SetOdom(Pose odom)
Definition: model_position.cc:679
point_t * unit_square_points_create()
Definition: stage.cc:218
void * arg
Definition: stage.hh:1789
paddle_state_t
Definition: stage.hh:2579
~ModelBlinkenlight()
Definition: model_blinkenlight.cc:90
bool TestQuit() const
Definition: stage.hh:1153
point_int_t MetersToPixels(const point_t &pt) const
Definition: stage.hh:984
void Print(const char *prefix) const
Definition: color.cc:100
void SetGuiOutline(bool val)
Definition: model.cc:1333
Visibility & Load(Worldfile *wf, int wf_entity)
Definition: model.cc:202
virtual ~Ancestor()
Definition: ancestor.cc:15
void DrawFootPrint()
Definition: block.cc:360
ModelBlobfinder class
Definition: stage.hh:2483
point3_t()
Definition: stage.hh:489
Size(meters_t x, meters_t y, meters_t z)
Definition: stage.hh:230
void BecomeParentOf(Model *child)
Definition: model.cc:971
void PopCoords()
Definition: model_draw.cc:317
virtual ~Vis(void)
Definition: stage.hh:2501
virtual std::string ClockString(void) const
Definition: world.cc:511
int id
the fiducial identifier of the target (i.e. its fiducial_return value), or -1 if none can be detected...
Definition: stage.hh:2700
void UpdateTrail()
Definition: model.cc:868
Ray(const Model *mod, const Pose &origin, const meters_t range, const ray_test_func_t func, const void *arg, const bool ztest)
Definition: stage.hh:758
Ancestor()
Definition: ancestor.cc:5
void SetAcceleration(double x, double y, double a)
Definition: model_position.cc:667
bounds3d_t()
Definition: stage.hh:457
Fiducial * GetFiducials(unsigned int *count)
Definition: stage.hh:2736
Definition: stage.hh:2582
void SetColor(Color col)
Definition: model.cc:1260
double radians_t
Definition: stage.hh:177
int RemoveUpdateCallback(world_callback_t cb, void *user)
Definition: world.cc:546
virtual void Update()
Definition: model_position.cc:262
meters_t x
Definition: stage.hh:251
void reset()
Definition: stage.hh:1433
void MapPoly(const std::vector< point_int_t > &poly, Block *block, unsigned int layer)
Definition: world.cc:1021
virtual std::string ClockString() const
Definition: worldgui.cc:387
Definition: stage.hh:1847
void * GetProperty(std::string &key)
Definition: stage.hh:732
virtual ~ModelRanger()
Definition: model_ranger.cc:111
const char * Token() const
Definition: stage.hh:715
double farClip(void) const
Definition: stage.hh:1430
Color color
Definition: stage.hh:1972
Stg::PowerPack::DissipationVis event_vis
int sgn(int a)
Definition: stage.hh:162
meters_t ModelHeight() const
Definition: model.cc:761
virtual bool Save(const char *filename)
Definition: worldgui.cc:319
Definition: stage.hh:1886
Size size
Definition: stage.hh:2776
Model * gripped
Definition: stage.hh:2611
virtual void Load(const std::string &worldfile_path)
Definition: world.cc:388
virtual void UnLoad()
Definition: worldgui.cc:314
int fiducial_key
Definition: stage.hh:2050
bool grid
Definition: stage.hh:1889
int subs
Definition: stage.hh:1945
void ClearBlocks()
Definition: model.cc:421
void SetObstacleReturn(bool val)
Definition: model.cc:1298
Model * GetGround()
Definition: stage.hh:1193
Model * parent
Definition: stage.hh:1912
void AddPowerPack(PowerPack *pp)
Definition: world.cc:1149
Velocity(double x, double y, double z, double a)
Definition: stage.hh:356
usec_t quit_time
Definition: stage.hh:905
double GetPosition() const
Definition: stage.hh:3105
Waypoint()
Definition: model_position.cc:823
Blob * GetBlobs(unsigned int *count)
Definition: stage.hh:2531
void UnMap(unsigned int layer)
Definition: block.cc:231
Geom GetGeom() const
Definition: stage.hh:2378
Pose()
Definition: stage.hh:261
unsigned int scan_width
Definition: stage.hh:2517
usec_t last_update
time of last update in us
Definition: stage.hh:1906