stage.hh

Go to the documentation of this file.
00001 #ifndef STG_H
00002 #define STG_H
00003 /*
00004  *  Stage : a multi-robot simulator. Part of the Player Project.
00005  * 
00006  *  Copyright (C) 2001-2009 Richard Vaughan, Brian Gerkey, Andrew
00007  *  Howard, Toby Collett, Reed Hedges, Alex Couture-Beil, Jeremy
00008  *  Asher, Pooya Karimian
00009  *
00010  *  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU General Public License for more details.
00019  *
00020  *  You should have received a copy of the GNU General Public License
00021  *  along with this program; if not, write to the Free Software
00022  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00023  *
00024  */
00025 
00033 // C libs
00034 #include <unistd.h>
00035 #include <stdint.h> // for portable int types eg. uint32_t
00036 #include <assert.h>
00037 #include <stdlib.h>
00038 #include <stdio.h>
00039 #include <libgen.h>
00040 #include <string.h>
00041 #include <sys/types.h>
00042 #include <sys/time.h>
00043 #include <pthread.h> 
00044 
00045 // C++ libs
00046 #include <cmath>
00047 #include <iostream>
00048 #include <vector>
00049 #include <list>
00050 #include <map>
00051 #include <set>
00052 #include <queue>
00053 #include <algorithm>
00054 
00055 // FLTK Gui includes
00056 #include <FL/Fl.H>
00057 #include <FL/Fl_Box.H>
00058 #include <FL/Fl_Gl_Window.H>
00059 #include <FL/Fl_Menu_Bar.H>
00060 #include <FL/Fl_Window.H>
00061 #include <FL/fl_draw.H>
00062 #include <FL/gl.h> // FLTK takes care of platform-specific GL stuff
00063 // except GLU
00064 #ifdef __APPLE__
00065 #include <OpenGL/glu.h>
00066 #else
00067 #include <GL/glu.h>
00068 #endif 
00069 
00071 namespace Stg 
00072 {
00073   // forward declare
00074   class Block;
00075   class Canvas;
00076   class Cell;
00077   class Worldfile;
00078   class World;
00079   class WorldGui;
00080   class Model;
00081   class OptionsDlg;
00082   class Camera;
00083   class FileManager;
00084   class Option;
00085   
00086   typedef Model* (*creator_t)( World*, Model*, const std::string& type );
00087   
00089   typedef std::set<Model*> ModelPtrSet;
00090 
00092   typedef std::vector<Model*> ModelPtrVec;
00093 
00095   typedef std::set<Block*> BlockPtrSet;
00096 
00098   typedef std::vector<Cell*> CellPtrVec;
00099 
00101   void Init( int* argc, char** argv[] );
00102 
00104   bool InitDone();
00105   
00108   const char* Version();
00109 
00111   const char COPYRIGHT[] =                     
00112     "Copyright Richard Vaughan and contributors 2000-2009";
00113 
00115   const char AUTHORS[] =                    
00116     "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, Toby Collett, Jeremy Asher, Alex Couture-Beil and contributors.";
00117 
00119   const char WEBSITE[] = "http://playerstage.org";
00120 
00122   const char DESCRIPTION[] =                       
00123     "Robot simulation library\nPart of the Player Project";
00124 
00126   const char LICENSE[] = 
00127     "Stage robot simulation library\n"                  \
00128     "Copyright (C) 2000-2009 Richard Vaughan and contributors\n"    \
00129     "Part of the Player Project [http://playerstage.org]\n"     \
00130     "\n"                                \
00131     "This program is free software; you can redistribute it and/or\n"   \
00132     "modify it under the terms of the GNU General Public License\n" \
00133     "as published by the Free Software Foundation; either version 2\n"  \
00134     "of the License, or (at your option) any later version.\n"      \
00135     "\n"                                \
00136     "This program is distributed in the hope that it will be useful,\n" \
00137     "but WITHOUT ANY WARRANTY; without even the implied warranty of\n"  \
00138     "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n"   \
00139     "GNU General Public License for more details.\n"            \
00140     "\n"                                \
00141     "You should have received a copy of the GNU General Public License\n" \
00142     "along with this program; if not, write to the Free Software\n" \
00143     "Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.\n" \
00144     "\n"                                \
00145     "The text of the license may also be available online at\n"     \
00146     "http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";
00147   
00149   const double thousand = 1e3;
00150 
00152   const double million = 1e6;
00153 
00155   const double billion = 1e9;
00156 
00158   inline double rtod( double r ){ return( r*180.0/M_PI ); }
00159   
00161   inline double dtor( double d ){ return( d*M_PI/180.0 ); }
00162   
00164   inline double normalize( double a )
00165   {
00166      while( a < -M_PI ) a += 2.0*M_PI;
00167      while( a >  M_PI ) a -= 2.0*M_PI;   
00168      return a;
00169   };
00170     
00172   inline int sgn( int a){ return( a<0 ? -1 : 1); }
00173     
00175   inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); }
00176   
00178   enum { FiducialNone = 0 };
00179 
00181   typedef uint32_t stg_id_t;
00182 
00184   typedef double stg_meters_t;
00185 
00187   typedef double stg_radians_t;
00188 
00190   typedef struct timeval stg_time_t;
00191 
00193   typedef unsigned long stg_msec_t;
00194 
00196   typedef uint64_t stg_usec_t;
00197 
00199   typedef double stg_kg_t; // Kilograms (mass)
00200 
00202   typedef double stg_joules_t;
00203 
00205   typedef double stg_watts_t;
00206   
00208   typedef bool stg_bool_t;
00209   
00210   class Color
00211   {
00212   public:
00213      float r,g,b,a;
00214     
00215      Color( float r, float g, float b, float a=1.0 );
00216     
00220      Color( const std::string& name );  
00221     
00222      Color();
00223     
00224      bool operator!=( const Color& other );
00225      bool operator==( const Color& other );
00226      static Color RandomColor();
00227      void Print( const char* prefix );
00228   };
00229   
00231   class Size
00232   {
00233   public:
00234     stg_meters_t x, y, z;
00235     
00236     Size( stg_meters_t x, 
00237              stg_meters_t y, 
00238              stg_meters_t z )
00239       : x(x), y(y), z(z)
00240     {/*empty*/}
00241     
00243     Size() : x( 0.4 ), y( 0.4 ), z( 1.0 )
00244     {/*empty*/} 
00245     
00246      void Load( Worldfile* wf, int section, const char* keyword );
00247      void Save( Worldfile* wf, int section, const char* keyword );
00248      
00249      void Zero()
00250      { x=y=z=0.0; }
00251   };
00252   
00254   class Pose
00255   {
00256   public:
00257     stg_meters_t x, y, z;
00258     stg_radians_t a;
00259     
00260     Pose( stg_meters_t x, 
00261              stg_meters_t y, 
00262              stg_meters_t z,
00263              stg_radians_t a ) 
00264       : x(x), y(y), z(z), a(a)
00265     { /*empty*/ }
00266     
00267     Pose() : x(0.0), y(0.0), z(0.0), a(0.0)
00268     { /*empty*/ }        
00269     
00270     virtual ~Pose(){};
00271     
00274     static Pose Random( stg_meters_t xmin, stg_meters_t xmax, 
00275                                 stg_meters_t ymin, stg_meters_t ymax )
00276     {        
00277       return Pose( xmin + drand48() * (xmax-xmin),
00278                          ymin + drand48() * (ymax-ymin),
00279                          0, 
00280                          normalize( drand48() * (2.0 * M_PI) ));
00281     }
00282     
00286     virtual void Print( const char* prefix )
00287     {
00288       printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
00289                   prefix, x,y,z,a );
00290     }
00291     
00292      std::string String()
00293      {
00294         char buf[256];
00295         snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]",
00296                      x,y,z,a );
00297         return std::string(buf);
00298      }
00299     
00300      /* returns true iff all components of the velocity are zero. */
00301      bool IsZero() const { return( !(x || y || z || a )); };
00302     
00304      void Zero(){ x=y=z=a=0.0; }
00305     
00306      void Load( Worldfile* wf, int section, const char* keyword );
00307      void Save( Worldfile* wf, int section, const char* keyword );
00308     
00309      inline Pose operator+( const Pose& p )
00310      {
00311         const double cosa = cos(a);
00312         const double sina = sin(a);
00313       
00314         return Pose( x + p.x * cosa - p.y * sina,
00315                          y + p.x * sina + p.y * cosa,
00316                          z + p.z,
00317                          normalize(a + p.a) );   
00318      }  
00319   };
00320   
00321   
00323   class Velocity : public Pose
00324   {
00325   public:
00331     Velocity( stg_meters_t x, 
00332                   stg_meters_t y, 
00333                   stg_meters_t z,
00334                   stg_radians_t a ) :
00335         Pose( x, y, z, a )
00336     { /*empty*/ }
00337     
00338     Velocity()
00339     { /*empty*/ }        
00340     
00344     virtual void Print( const char* prefix )
00345     {
00346       printf( "%s velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
00347                   prefix, x,y,z,a );
00348     }    
00349   };
00350   
00353   class Geom
00354   {
00355   public:
00356     Pose pose;
00357     Size size;
00358     
00359     void Print( const char* prefix )
00360     {
00361       printf( "%s geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n",
00362                   prefix,
00363                   pose.x,
00364                   pose.y,
00365                   pose.a,
00366                   size.x,
00367                   size.y );
00368     }
00369      
00371      Geom() : pose(), size() {}
00372 
00374      Geom( const Pose& p, const Size& s ) : pose(p), size(s) {}
00375      
00376      void Zero()
00377      {
00378         pose.Zero();
00379         size.Zero();
00380      }
00381   };
00382   
00384   class Bounds
00385   {
00386   public:
00388     double min;
00390     double max; 
00391     
00392     Bounds() : min(0), max(0) { /* empty*/  }
00393     Bounds( double min, double max ) : min(min), max(max) { /* empty*/  }
00394   };
00395     
00397   class stg_bounds3d_t
00398   {
00399   public:
00401     Bounds x; 
00403     Bounds y; 
00405     Bounds z; 
00406 
00407      stg_bounds3d_t() : x(), y(), z() {}
00408      stg_bounds3d_t( const Bounds& x, const Bounds& y, const Bounds& z) 
00409         : x(x), y(y), z(z) {}
00410   };
00411   
00413   typedef struct
00414   {
00415     Bounds range; 
00416     stg_radians_t angle; 
00417   } stg_fov_t;
00418   
00420   class stg_point_t
00421   {
00422   public:
00423     stg_meters_t x, y;
00424      stg_point_t( stg_meters_t x, stg_meters_t y ) : x(x), y(y){}    
00425      stg_point_t() : x(0.0), y(0.0){}
00426     
00427      bool operator+=( const stg_point_t& other ) 
00428      { return ((x += other.x) && (y += other.y) ); }  
00429   };
00430   
00432   class stg_point3_t
00433   {
00434   public:
00435     stg_meters_t x,y,z;
00436      stg_point3_t( stg_meters_t x, stg_meters_t y, stg_meters_t z ) 
00437         : x(x), y(y), z(z) {}    
00438   
00439      stg_point3_t() : x(0.0), y(0.0), z(0.0) {}
00440   };
00441   
00443   class stg_point_int_t
00444   {
00445   public:
00446     int x,y;
00447         stg_point_int_t( int x, int y ) : x(x), y(y){}   
00448         stg_point_int_t() : x(0), y(0){}
00449         
00451         bool operator<( const stg_point_int_t& other ) const
00452         {
00453             if( x < other.x ) return true;
00454             if( other.x < x ) return false;
00455             return y < other.y;
00456         }
00457  
00458      bool operator==( const stg_point_int_t& other ) const
00459         { return ((x == other.x) && (y == other.y) ); }
00460   };
00461   
00462   typedef std::vector<stg_point_int_t> PointIntVec;
00463 
00466   stg_point_t* stg_unit_square_points_create();
00467   
00470   typedef enum 
00471     {
00472       LaserTransparent=0, 
00473       LaserVisible, 
00474       LaserBright  
00475     } stg_laser_return_t;
00476   
00479   namespace Gl
00480   {
00481      void pose_shift( const Pose &pose );
00482      void pose_inverse_shift( const Pose &pose );
00483      void coord_shift( double x, double y, double z, double a  );
00484      void draw_grid( stg_bounds3d_t vol );
00486      void draw_string( float x, float y, float z, const char *string);
00487      void draw_string_multiline( float x, float y, float w, float h, 
00488                                           const char *string, Fl_Align align );
00489      void draw_speech_bubble( float x, float y, float z, const char* str );
00490      void draw_octagon( float w, float h, float m );
00491      void draw_octagon( float x, float y, float w, float h, float m );
00492      void draw_vector( double x, double y, double z );
00493      void draw_origin( double len );
00494      void draw_array( float x, float y, float w, float h, 
00495                             float* data, size_t len, size_t offset, 
00496                             float min, float max );
00497      void draw_array( float x, float y, float w, float h, 
00498                             float* data, size_t len, size_t offset );
00500      void draw_centered_rect( float x, float y, float dx, float dy );
00501   } // namespace Gl
00502   
00503   void RegisterModels();
00504   
00505   
00507   class Visualizer {
00508   private:
00509      const std::string menu_name;
00510      const std::string worldfile_name;
00511      
00512   public:
00513      Visualizer( const std::string& menu_name, 
00514                      const std::string& worldfile_name ) 
00515         : menu_name( menu_name ),
00516           worldfile_name( worldfile_name )
00517      { }
00518      
00519      virtual ~Visualizer( void ) { }
00520      virtual void Visualize( Model* mod, Camera* cam ) = 0;
00521      
00522      const std::string& GetMenuName() { return menu_name; }
00523      const std::string& GetWorldfileName() { return worldfile_name; }
00524   };
00525 
00526 
00527   typedef int(*stg_model_callback_t)(Model* mod, void* user );
00528   typedef int(*stg_world_callback_t)(World* world, void* user );
00529   
00530   // return val, or minval if val < minval, or maxval if val > maxval
00531   double constrain( double val, double minval, double maxval );
00532     
00533   typedef struct 
00534   {
00535     int enabled;
00536     Pose pose;
00537     stg_meters_t size; 
00538     Color color;
00539     stg_msec_t period; 
00540     double duty_cycle; 
00541   } stg_blinkenlight_t;
00542 
00543   
00545   typedef struct
00546   {
00547     Pose pose;
00548     Size size;
00549   } stg_rotrect_t; // rotated rectangle
00550 
00556   int stg_rotrects_from_image_file( const char* filename, 
00557                                                 stg_rotrect_t** rects,
00558                                                 unsigned int* rect_count,
00559                                                 unsigned int* widthp, 
00560                                                 unsigned int* heightp );
00561 
00562   
00565   typedef bool (*stg_ray_test_func_t)(Model* candidate, 
00566                                                   Model* finder, 
00567                                                   const void* arg );
00568 
00569   // STL container iterator macros
00570 #define VAR(V,init) __typeof(init) V=(init)
00571 #define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();I++)
00572 
00574   template <class T, class C>
00575   void EraseAll( T thing, C& cont )
00576   { cont.erase( std::remove( cont.begin(), cont.end(), thing ), cont.end() ); }
00577   
00578   // Error macros - output goes to stderr
00579 #define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00580 #define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00581 #define PRINT_ERR2(m,a,b) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00582 #define PRINT_ERR3(m,a,b,c) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00583 #define PRINT_ERR4(m,a,b,c,d) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
00584 #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__)
00585 
00586   // Warning macros
00587 #define PRINT_WARN(m) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00588 #define PRINT_WARN1(m,a) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00589 #define PRINT_WARN2(m,a,b) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00590 #define PRINT_WARN3(m,a,b,c) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00591 #define PRINT_WARN4(m,a,b,c,d) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
00592 #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__)
00593 
00594   // Message macros
00595 #ifdef DEBUG
00596 #define PRINT_MSG(m) printf( "Stage: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00597 #define PRINT_MSG1(m,a) printf( "Stage: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00598 #define PRINT_MSG2(m,a,b) printf( "Stage: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00599 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00600 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
00601 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m" (%s %s)\n", a, b, c, d, e,__FILE__, __FUNCTION__)
00602 #else
00603 #define PRINT_MSG(m) printf( "Stage: "m"\n" )
00604 #define PRINT_MSG1(m,a) printf( "Stage: "m"\n", a)
00605 #define PRINT_MSG2(m,a,b) printf( "Stage: "m"\n,", a, b )
00606 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m"\n", a, b, c )
00607 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m"\n", a, b, c, d )
00608 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m"\n", a, b, c, d, e )
00609 #endif
00610 
00611   // DEBUG macros
00612 #ifdef DEBUG
00613 #define PRINT_DEBUG(m) printf( "debug: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00614 #define PRINT_DEBUG1(m,a) printf( "debug: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00615 #define PRINT_DEBUG2(m,a,b) printf( "debug: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00616 #define PRINT_DEBUG3(m,a,b,c) printf( "debug: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00617 #define PRINT_DEBUG4(m,a,b,c,d) printf( "debug: "m" (%s %s)\n", a, b, c ,d, __FILE__, __FUNCTION__)
00618 #define PRINT_DEBUG5(m,a,b,c,d,e) printf( "debug: "m" (%s %s)\n", a, b, c ,d, e, __FILE__, __FUNCTION__)
00619 #else
00620 #define PRINT_DEBUG(m)
00621 #define PRINT_DEBUG1(m,a)
00622 #define PRINT_DEBUG2(m,a,b)
00623 #define PRINT_DEBUG3(m,a,b,c)
00624 #define PRINT_DEBUG4(m,a,b,c,d)
00625 #define PRINT_DEBUG5(m,a,b,c,d,e)
00626 #endif
00627 
00628   class Block;
00629   class Model;
00630 
00633   typedef int (*stg_model_callback_t)( Model* mod, void* user );
00634   
00635   // ANCESTOR CLASS
00637   class Ancestor
00638   {
00639     friend class Canvas; // allow Canvas access to our private members
00640      
00641   protected:
00642      ModelPtrVec children;
00643     bool debug;
00644      std::string token;
00645      pthread_mutex_t access_mutex; 
00646 
00647      void Load( Worldfile* wf, int section );
00648      void Save( Worldfile* wf, int section );
00649      
00650   public:   
00651      /* The maximum length of a Stage model identifier string */
00652      //static const uint32_t TOKEN_MAX = 64;
00653          
00655      ModelPtrVec& GetChildren(){ return children;}
00656     
00658     void ForEachDescendant( stg_model_callback_t func, void* arg );
00659         
00661      std::map<std::string,unsigned int> child_type_counts;
00662      
00663     Ancestor();
00664     virtual ~Ancestor();
00665      
00666     virtual void AddChild( Model* mod );
00667     virtual void RemoveChild( Model* mod );
00668     virtual Pose GetGlobalPose();
00669      
00670     const char* Token()
00671     { return token.c_str(); }
00672      
00673     void SetToken( const std::string& str )
00674     { token = str; } 
00675 
00676      void Lock(){ pthread_mutex_lock( &access_mutex ); }    
00677      void Unlock(){ pthread_mutex_unlock( &access_mutex ); }
00678   };
00679 
00682   class RaytraceResult
00683   {
00684   public:
00685     Pose pose; 
00686     stg_meters_t range; 
00687     Model* mod; 
00688     Color color; 
00689      
00690      RaytraceResult() : pose(), range(0), mod(NULL), color() {}
00691      RaytraceResult( const Pose& pose, 
00692                           stg_meters_t range ) 
00693         : pose(pose), range(range), mod(NULL), color() {}    
00694   };
00695 
00696   typedef RaytraceResult stg_raytrace_result_t;
00697     
00698   class Ray
00699   {
00700   public:
00701      Ray( const Model* mod, const Pose& origin, const stg_meters_t range, const stg_ray_test_func_t func, const void* arg, const bool ztest ) :
00702         mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest)
00703      {}
00704 
00705      Ray() : mod(NULL), origin(0,0,0,0), range(0), func(NULL), arg(NULL), ztest(true)
00706      {}
00707 
00708      const Model* mod;
00709      Pose origin;
00710      stg_meters_t range;
00711      stg_ray_test_func_t func;
00712      const void* arg;
00713      bool ztest;        
00714   };
00715         
00716 
00717   // defined in stage_internal.hh
00718   class Region;
00719   class SuperRegion;
00720   class BlockGroup;
00721   class PowerPack;
00722 
00723   class LogEntry
00724   {
00725      stg_usec_t timestamp;
00726      Model* mod;
00727      Pose pose;
00728      
00729   public:
00730      LogEntry( stg_usec_t timestamp, Model* mod );
00731      
00733      static std::vector<LogEntry> log;
00734      
00736      static size_t Count(){ return log.size(); }
00737      
00739      static void Clear(){ log.clear(); }
00740 
00742      static void Print();
00743   };
00744 
00745   class CtrlArgs
00746   {
00747   public:
00748      std::string worldfile;
00749      std::string cmdline;
00750 
00751      CtrlArgs( std::string w, std::string c ) : worldfile(w), cmdline(c) {}
00752   };
00753 
00755   class World : public Ancestor
00756   {
00757     friend class Block;
00758     friend class Model; // allow access to private members
00759     friend class ModelFiducial;
00760     friend class Canvas;
00761 
00762   public: 
00765      static std::vector<std::string> args;
00766      static std::string ctrlargs;
00767 
00768   private:
00769     
00770     static std::set<World*> world_set; 
00771     static bool quit_all; 
00772     static void UpdateCb( World* world);
00773     static unsigned int next_id; 
00774      
00775     bool destroy;
00776     bool dirty; 
00777 
00779      std::set<Model*> models;
00780 
00782      std::map<std::string, Model*> models_by_name;      
00783 
00785      std::map<int,Model*> models_by_wfentity;
00786         
00789      ModelPtrVec models_with_fiducials;
00790      
00792      void FiducialInsert( Model* mod )
00793      { 
00794         FiducialErase( mod ); // make sure it's not there already
00795         models_with_fiducials.push_back( mod ); 
00796      }
00797      
00799      void FiducialErase( Model* mod )
00800      { 
00801         EraseAll( mod, models_with_fiducials );
00802      }
00803 
00804     double ppm; 
00805     bool quit; 
00806      
00807      bool show_clock; 
00808      unsigned int show_clock_interval; 
00809 
00810     pthread_mutex_t thread_mutex; 
00811      unsigned int threads_working; 
00812     pthread_cond_t threads_start_cond; 
00813     pthread_cond_t threads_done_cond; 
00814     int total_subs; 
00815      unsigned int worker_threads; 
00816      
00817   protected:     
00818 
00819      std::list<std::pair<stg_world_callback_t,void*> > cb_list; 
00820     stg_bounds3d_t extent; 
00821     bool graphics;
00822 
00823      std::set<Option*> option_table; 
00824      std::list<PowerPack*> powerpack_list; 
00825 
00826     stg_usec_t quit_time;
00827      std::list<float*> ray_list;
00828     stg_usec_t sim_time; 
00829      std::map<stg_point_int_t,SuperRegion*> superregions;
00830     SuperRegion* sr_cached; 
00831      
00832      std::vector<ModelPtrVec> update_lists;  
00833      
00834     uint64_t updates; 
00835     Worldfile* wf; 
00836 
00837      void CallUpdateCallbacks(); 
00838 
00839   public:
00840      
00841     bool paused; 
00842 
00843     virtual void Start(){ paused = false; };
00844     virtual void Stop(){ paused = true; };
00845     virtual void TogglePause(){ paused ? Start() : Stop(); };
00846 
00847      bool Paused(){ return( paused ); };
00848     
00849      PointIntVec rt_cells;
00850      PointIntVec rt_candidate_cells;
00851 
00852     static const int DEFAULT_PPM = 50;  // default resolution in pixels per meter
00853 
00856      void AddUpdateCallback( stg_world_callback_t cb, void* user );
00857 
00860      int RemoveUpdateCallback( stg_world_callback_t cb, void* user );
00862      void Log( Model* mod );
00863 
00865     void NeedRedraw(){ dirty = true; };
00866     
00868     Model* ground;
00869     
00872     virtual std::string ClockString( void ) const;
00873         
00874      Model* CreateModel( Model* parent, const std::string& typestr );    
00875     void LoadModel( Worldfile* wf, int entity );
00876     void LoadBlock( Worldfile* wf, int entity );
00877     void LoadBlockGroup( Worldfile* wf, int entity );
00878     
00879     virtual Model* RecentlySelectedModel(){ return NULL; }
00880         
00881     SuperRegion* AddSuperRegion( const stg_point_int_t& coord );
00882     SuperRegion* GetSuperRegion( const stg_point_int_t& coord );
00883     SuperRegion* GetSuperRegionCached( const stg_point_int_t& coord);
00884     SuperRegion* GetSuperRegionCached( int32_t x, int32_t y );
00885     void ExpireSuperRegion( SuperRegion* sr );
00886         
00889     void ForEachCellInLine( const stg_point_int_t& pt1,
00890                                                         const stg_point_int_t& pt2, 
00891                                                         CellPtrVec& cells );
00892         
00895     int32_t MetersToPixels( stg_meters_t x )
00896     { return (int32_t)floor(x * ppm); };
00897         
00898     stg_point_int_t MetersToPixels( const stg_point_t& pt )
00899     { return stg_point_int_t( MetersToPixels(pt.x), MetersToPixels(pt.y)); };
00900         
00901     // dummy implementations to be overloaded by GUI subclasses
00902     virtual void PushColor( Color col ) 
00903      { /* do nothing */  (void)col; };
00904     virtual void PushColor( double r, double g, double b, double a ) 
00905      { /* do nothing */ (void)r; (void)g; (void)b; (void)a; };
00906      
00907     virtual void PopColor(){ /* do nothing */  };
00908         
00909     SuperRegion* CreateSuperRegion( stg_point_int_t origin );
00910     void DestroySuperRegion( SuperRegion* sr );
00911         
00913      stg_raytrace_result_t Raytrace( const Ray& ray );
00914 
00915     stg_raytrace_result_t Raytrace( const Pose& pose,            
00916                                                 const stg_meters_t range,
00917                                                 const stg_ray_test_func_t func,
00918                                                 const Model* finder,
00919                                                 const void* arg,
00920                                                 const bool ztest );
00921         
00922     void Raytrace( const Pose &pose,             
00923                          const stg_meters_t range,
00924                          const stg_radians_t fov,
00925                          const stg_ray_test_func_t func,
00926                          const Model* finder,
00927                          const void* arg,
00928                          stg_raytrace_result_t* samples,
00929                          const uint32_t sample_count,
00930                          const bool ztest );
00931         
00932         
00934     void Extend( stg_point3_t pt );
00935   
00936     virtual void AddModel( Model* mod );
00937     virtual void RemoveModel( Model* mod );
00938 
00939     void AddModelName( Model* mod, const std::string& name );
00940         
00941     void AddPowerPack( PowerPack* pp );
00942     void RemovePowerPack( PowerPack* pp );
00943         
00944     void ClearRays();
00945   
00947     void RecordRay( double x1, double y1, double x2, double y2 );
00948         
00951     bool PastQuitTime();
00952                 
00953     static void* update_thread_entry( std::pair<World*,int>* info );
00954     
00955     class Event
00956     {
00957     public:
00958       
00959       Event( stg_usec_t time, Model* mod ) 
00960           : time(time), mod(mod) {}
00961       
00962       stg_usec_t time; // time that event occurs
00963       Model* mod; // model to update
00964       
00965       bool operator<( const Event& other ) const;
00966     };
00967      
00968      std::vector<std::priority_queue<Event> > event_queues;
00969      void Enqueue( unsigned int queue_num, stg_usec_t delay, Model* mod );
00970      
00971      std::set<Model*> active_energy;
00972      std::set<Model*> active_velocity;
00973 
00975      stg_usec_t sim_interval;
00976      
00978      void ConsumeQueue( unsigned int queue_num );
00979 
00982      unsigned int GetEventQueue( Model* mod );
00983 
00984   public:
00986     static bool UpdateAll(); 
00987      
00988     World( const std::string& name = "MyWorld", 
00989               double ppm = DEFAULT_PPM );
00990         
00991     virtual ~World();
00992      
00993     stg_usec_t SimTimeNow(void);
00994      
00995     Worldfile* GetWorldFile(){ return wf; };
00996      
00997     virtual bool IsGUI() const { return false; }
00998      
00999     virtual void Load( const char* worldfile_path );
01000     virtual void UnLoad();
01001     virtual void Reload();
01002     virtual bool Save( const char* filename );
01003     virtual bool Update(void);
01004      
01005     bool TestQuit(){ return( quit || quit_all );  }
01006     void Quit(){ quit = true; }
01007     void QuitAll(){ quit_all = true; }
01008     void CancelQuit(){ quit = false; }
01009     void CancelQuitAll(){ quit_all = false; }
01010      
01011      void TryCharge( PowerPack* pp, const Pose& pose );
01012 
01015     double Resolution(){ return ppm; };
01016    
01019     Model* GetModel( const std::string& name ) const;
01020   
01022     const stg_bounds3d_t& GetExtent(){ return extent; };
01023   
01025     uint64_t GetUpdateCount() { return updates; }
01026 
01028      void RegisterOption( Option* opt );    
01029      
01031      void ShowClock( bool enable ){ show_clock = enable; };
01032 
01034      Model* GetGround() {return ground;};
01035     
01036   };
01037 
01038   class Block
01039   {
01040     friend class BlockGroup;
01041     friend class Model;
01042     friend class SuperRegion;
01043     friend class World;
01044     friend class Canvas;
01045   public:
01046   
01050     Block( Model* mod,  
01051               stg_point_t* pts, 
01052               size_t pt_count,
01053               stg_meters_t zmin,
01054               stg_meters_t zmax,
01055               Color color,
01056               bool inherit_color );
01057   
01059     Block(  Model* mod,  Worldfile* wf, int entity);
01060      
01061     ~Block();
01062      
01064     void Map();      
01065      
01067     void UnMap();    
01068      
01070      void DrawSolid();
01071 
01073     void DrawFootPrint(); 
01074 
01076      void Translate( double x, double y );   
01077 
01079      double CenterX();
01080 
01082      double CenterY();
01083 
01085      void SetCenterX( double y );
01086 
01088      void SetCenterY( double y );
01089 
01091      void SetCenter( double x, double y);    
01092 
01094      void SetZ( double min, double max );
01095 
01096     stg_point_t* Points( unsigned int *count )
01097     { if( count ) *count = pt_count; return &pts[0]; };          
01098      
01099      std::vector<stg_point_t>& Points()
01100     { return pts; };             
01101          
01102     inline void RemoveFromCellArray( CellPtrVec* blocks );
01103     inline void GenerateCandidateCells();  
01104 
01105      void AppendTouchingModels( ModelPtrSet& touchers );
01106      
01108     Model* TestCollision(); 
01109     void SwitchToTestedCells();  
01110     void Load( Worldfile* wf, int entity );  
01111     Model* GetModel(){ return mod; };  
01112     const Color& GetColor();        
01113      void Rasterize( uint8_t* data, 
01114                           unsigned int width, unsigned int height,      
01115                           stg_meters_t cellwidth, stg_meters_t cellheight );
01116         
01117   private:
01118     Model* mod; 
01119      std::vector<stg_point_t> mpts; 
01120     size_t pt_count; 
01121      std::vector<stg_point_t> pts; 
01122     Size size;   
01123     Bounds local_z; 
01124     Color color;
01125     bool inherit_color;
01126      
01128      double glow;
01129      
01130     void DrawTop();
01131     void DrawSides();
01132      
01134     Bounds global_z;     
01135     bool mapped;
01136         
01138         std::vector< std::list<Block*>::iterator > list_entries;
01139 
01142      CellPtrVec * rendered_cells;
01143 
01150      CellPtrVec * candidate_cells;
01151     
01152      PointIntVec gpts;
01153     
01156      stg_point_t BlockPointToModelMeters( const stg_point_t& bpt );
01157     
01159      void InvalidateModelPointCache();
01160   };
01161 
01162 
01163   class BlockGroup
01164   {
01165     friend class Model;
01166      friend class Block;
01167 
01168   private:
01169     int displaylist;
01170 
01171     void BuildDisplayList( Model* mod );
01172      
01173      BlockPtrSet blocks;
01174     Size size;
01175     stg_point3_t offset;
01176     stg_meters_t minx, maxx, miny, maxy;
01177 
01178   public:
01179     BlockGroup();
01180     ~BlockGroup();
01181      
01182     uint32_t GetCount(){ return blocks.size(); };
01183     const Size& GetSize(){ return size; };
01184     const stg_point3_t& GetOffset(){ return offset; };
01185      
01188     void CalcSize();
01189      
01190     void AppendBlock( Block* block );
01191     void CallDisplayList( Model* mod );
01192     void Clear() ; 
01194      void AppendTouchingModels( ModelPtrSet& touchers );
01195      
01198     Model* TestCollision();
01199  
01200     void SwitchToTestedCells();
01201      
01202     void Map();
01203     void UnMap();
01204      
01206     void DrawSolid( const Geom &geom); 
01207 
01209      void DrawFootPrint( const Geom &geom);
01210 
01211     void LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfile *wf );
01212     void LoadBlock( Model* mod, Worldfile* wf, int entity );
01213      
01214      void Rasterize( uint8_t* data, 
01215                           unsigned int width, unsigned int height,
01216                           stg_meters_t cellwidth, stg_meters_t cellheight );
01217      
01218      void InvalidateModelPointCache()
01219      {
01220         FOR_EACH( it, blocks )
01221           (*it)->InvalidateModelPointCache();
01222      }
01223 
01224   };
01225 
01226   class Camera 
01227   {
01228   protected:
01229     float _pitch; //left-right (about y)
01230     float _yaw; //up-down (about x)
01231     float _x, _y, _z;
01232     
01233   public:
01234     Camera() : _pitch( 0 ), _yaw( 0 ), _x( 0 ), _y( 0 ), _z( 0 ) { }
01235     virtual ~Camera() { }
01236 
01237     virtual void Draw( void ) const = 0;
01238     virtual void SetProjection( void ) const = 0;
01239 
01240      float yaw( void ) const { return _yaw; }
01241      float pitch( void ) const { return _pitch; }
01242      
01243      float x( void ) const { return _x; }
01244      float y( void ) const { return _y; }
01245      float z( void ) const { return _z; }
01246      
01247     virtual void reset() = 0;
01248     virtual void Load( Worldfile* wf, int sec ) = 0;
01249 
01250     //TODO data should be passed in somehow else. (at least min/max stuff)
01251     //virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const = 0;
01252   };
01253 
01254   class PerspectiveCamera : public Camera
01255   {
01256   private:
01257     float _z_near;
01258     float _z_far;
01259     float _vert_fov;
01260     float _horiz_fov;
01261     float _aspect;
01262 
01263   public:
01264     PerspectiveCamera( void );
01265 
01266     virtual void Draw( void ) const;
01267     virtual void SetProjection( void ) const;
01268     //void SetProjection( float aspect ) const;
01269     void update( void );
01270 
01271     void strafe( float amount );
01272     void forward( float amount );
01273     
01274      void setPose( float x, float y, float z ) { _x = x; _y = y; _z = z; }
01275      void addPose( float x, float y, float z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; }
01276     void move( float x, float y, float z );
01277      void setFov( float horiz_fov, float vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; }
01279      void setAspect( float aspect ) { _aspect = aspect; }
01280      void setYaw( float yaw ) { _yaw = yaw; }
01281      float horizFov( void ) const { return _horiz_fov; }
01282      float vertFov( void ) const { return _vert_fov; }
01283      void addYaw( float yaw ) { _yaw += yaw; }
01284      void setPitch( float pitch ) { _pitch = pitch; }
01285      void addPitch( float pitch ) { _pitch += pitch; if( _pitch < 0 ) _pitch = 0; else if( _pitch > 180 ) _pitch = 180; }
01286     
01287      float realDistance( float z_buf_val ) const {
01288       return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) );
01289     }
01290      void scroll( float dy ) { _z += dy; }
01291      float nearClip( void ) const { return _z_near; }
01292      float farClip( void ) const { return _z_far; }
01293      void setClip( float near, float far ) { _z_far = far; _z_near = near; }
01294     
01295      void reset() { setPitch( 70 ); setYaw( 0 ); }
01296     
01297     void Load( Worldfile* wf, int sec );
01298     void Save( Worldfile* wf, int sec );
01299   };
01300   
01301   class OrthoCamera : public Camera
01302   {
01303   private:
01304     float _scale;
01305     float _pixels_width;
01306     float _pixels_height;
01307     float _y_min;
01308     float _y_max;
01309   
01310   public:
01311     OrthoCamera( void ) : _scale( 15 ) { }
01312     virtual void Draw() const;
01313     virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max );
01314     virtual void SetProjection( void ) const;
01315   
01316     void move( float x, float y );
01317      void setYaw( float yaw ) { _yaw = yaw; }
01318      void setPitch( float pitch ) { _pitch = pitch; }
01319      void addYaw( float yaw ) { _yaw += yaw;    }
01320      void addPitch( float pitch ) {
01321       _pitch += pitch;
01322       if( _pitch > 90 )
01323           _pitch = 90;
01324       else if( _pitch < 0 )
01325           _pitch = 0;
01326     }
01327   
01328      void setScale( float scale ) { _scale = scale; }
01329      void setPose( float x, float y) { _x = x; _y = y; }
01330   
01331     void scale( float scale, float shift_x = 0, float h = 0, float shift_y = 0, float w = 0 );  
01332      void reset( void ) { _pitch = _yaw = 0; }
01333   
01334      float scale() const { return _scale; }
01335   
01336     void Load( Worldfile* wf, int sec );
01337     void Save( Worldfile* wf, int sec );
01338   };
01339 
01340 
01344   class WorldGui : public World, public Fl_Window 
01345   {
01346     friend class Canvas;
01347     friend class ModelCamera;
01348     friend class Model;
01349     friend class Option;
01350 
01351   private:
01352 
01353     Canvas* canvas;
01354     std::vector<Option*> drawOptions;
01355     FileManager* fileMan; 
01356      std::vector<stg_usec_t> interval_log;
01357      
01360      float speedup; 
01361 
01362     Fl_Menu_Bar* mbar;
01363     OptionsDlg* oDlg;
01364     bool pause_time;
01365 
01368     stg_usec_t real_time_interval;
01369      
01371     stg_usec_t real_time_now; 
01372 
01375     stg_usec_t real_time_recorded;
01376      
01378      uint64_t timing_interval;
01379 
01380     // static callback functions
01381     static void windowCb( Fl_Widget* w, WorldGui* wg ); 
01382     static void fileLoadCb( Fl_Widget* w, WorldGui* wg );
01383     static void fileSaveCb( Fl_Widget* w, WorldGui* wg );
01384     static void fileSaveAsCb( Fl_Widget* w, WorldGui* wg );
01385     static void fileExitCb( Fl_Widget* w, WorldGui* wg );
01386     static void viewOptionsCb( OptionsDlg* oDlg, WorldGui* wg );
01387     static void optionsDlgCb( OptionsDlg* oDlg, WorldGui* wg );
01388     static void helpAboutCb( Fl_Widget* w, WorldGui* wg );
01389     static void pauseCb( Fl_Widget* w, WorldGui* wg );
01390     static void onceCb( Fl_Widget* w, WorldGui* wg );
01391     static void fasterCb( Fl_Widget* w, WorldGui* wg );
01392     static void slowerCb( Fl_Widget* w, WorldGui* wg );
01393     static void realtimeCb( Fl_Widget* w, WorldGui* wg );
01394     static void fasttimeCb( Fl_Widget* w, WorldGui* wg );
01395     static void resetViewCb( Fl_Widget* w, WorldGui* wg );
01396     static void moreHelptCb( Fl_Widget* w, WorldGui* wg );
01397     
01398     // GUI functions
01399     bool saveAsDialog();
01400     bool closeWindowQuery();
01401     
01402     virtual void AddModel( Model* mod );
01403     
01404      void SetTimeouts();
01405 
01406   protected:
01407     
01408     virtual void PushColor( Color col );
01409     virtual void PushColor( double r, double g, double b, double a );
01410     virtual void PopColor();
01411     
01412     void DrawOccupancy();
01413     void DrawVoxels();
01414      
01415   public:
01416     
01417     WorldGui(int W,int H,const char*L=0);
01418     ~WorldGui();
01419     
01420     virtual std::string ClockString() const;
01421     virtual bool Update();  
01422     virtual void Load( const char* filename );
01423     virtual void UnLoad();
01424     virtual bool Save( const char* filename );  
01425     virtual bool IsGUI() const { return true; };    
01426     virtual Model* RecentlySelectedModel() const;
01427 
01428     virtual void Start();
01429     virtual void Stop();
01430     
01431     stg_usec_t RealTimeNow(void) const;
01432  
01433     void DrawBoundingBoxTree();
01434     
01435     Canvas* GetCanvas( void ) const { return canvas; } 
01436 
01438     void Show(); 
01439 
01441     std::string EnergyString( void );   
01442     virtual void RemoveChild( Model* mod );  
01443   };
01444 
01445 
01446   class StripPlotVis : public Visualizer
01447   {
01448   private:
01449      
01450      Model* mod;
01451      float* data;
01452      size_t len;
01453      size_t count;
01454      unsigned int index;
01455      float x,y,w,h,min,max;
01456      Color fgcolor, bgcolor;
01457      
01458   public:
01459      StripPlotVis( float x, float y, float w, float h, 
01460                         size_t len, 
01461                         Color fgcolor, Color bgcolor,
01462                         const char* name, const char* wfname );
01463      virtual ~StripPlotVis();
01464      virtual void Visualize( Model* mod, Camera* cam );     
01465      void AppendValue( float value );
01466   };
01467 
01468 
01469   class PowerPack
01470   {
01471      friend class WorldGui;
01472      friend class Canvas;
01473      
01474   protected:
01475      
01476      class DissipationVis : public Visualizer
01477      {
01478      private:
01479         unsigned int columns, rows;
01480         stg_meters_t width, height;
01481          
01482          std::vector<stg_joules_t> cells;
01483          
01484          stg_joules_t peak_value;
01485          double cellsize;
01486          
01487         static stg_joules_t global_peak_value; 
01488 
01489      public:
01490         DissipationVis( stg_meters_t width, 
01491                              stg_meters_t height, 
01492                              stg_meters_t cellsize );
01493 
01494         virtual ~DissipationVis();
01495         virtual void Visualize( Model* mod, Camera* cam );      
01496         
01497         void Accumulate( stg_meters_t x, stg_meters_t y, stg_joules_t amount );
01498      } event_vis;
01499      
01500 
01501      StripPlotVis output_vis;
01502      StripPlotVis stored_vis;
01503 
01505      Model* mod;
01506     
01508      stg_joules_t stored;
01509      
01511      stg_joules_t capacity;
01512      
01514      bool charging;
01515      
01517      stg_joules_t dissipated;
01518      
01519      // these are used to visualize the power draw
01520      stg_usec_t last_time;
01521      stg_joules_t last_joules;
01522      stg_watts_t last_watts;
01523 
01524      static stg_joules_t global_stored;
01525      static stg_joules_t global_capacity;
01526      static stg_joules_t global_dissipated;  
01527      static stg_joules_t global_input;
01528 
01529   public:
01530      PowerPack( Model* mod );
01531      ~PowerPack();
01532      
01534      void Visualize( Camera* cam );
01535 
01538      void Print( char* prefix ) const;
01539      
01541      stg_joules_t RemainingCapacity() const;
01542      
01544      void Add( stg_joules_t j );
01545         
01547      void Subtract( stg_joules_t j );
01548         
01550      void TransferTo( PowerPack* dest, stg_joules_t amount );    
01551 
01552      double ProportionRemaining() const
01553      { return( stored / capacity ); }
01554 
01555      void Print( const char* prefix ) const
01556      { printf( "%s PowerPack %.2f/%.2f J\n", prefix, stored, capacity ); }      
01557      
01558      stg_joules_t GetStored() const;
01559      stg_joules_t GetCapacity() const;
01560      stg_joules_t GetDissipated() const;
01561      void SetCapacity( stg_joules_t j );
01562      void SetStored( stg_joules_t j );  
01563 
01565      bool GetCharging() const { return charging; }
01566      
01567      void ChargeStart(){ charging = true; }
01568      void ChargeStop(){ charging = false; }
01569 
01571      void Dissipate( stg_joules_t j );
01572      
01574      void Dissipate( stg_joules_t j, const Pose& p );
01575   };
01576 
01577    
01579   class Model : public Ancestor
01580   {
01581     friend class Ancestor;
01582     friend class World;
01583     friend class World::Event;
01584     friend class WorldGui;
01585     friend class Canvas;
01586     friend class Block;
01587     friend class Region;
01588     friend class BlockGroup;
01589     friend class PowerPack;
01590     friend class Ray;
01591     
01592   private:
01594      static uint32_t count;
01595      static std::map<stg_id_t,Model*> modelsbyid;
01596      std::vector<Option*> drawOptions;
01597      const std::vector<Option*>& getOptions() const { return drawOptions; }
01598      
01599   protected:
01600      pthread_mutex_t access_mutex;
01601 
01604      bool alwayson;
01605 
01606      BlockGroup blockgroup;
01608      int blocks_dl;
01609 
01613      int boundary;
01614         
01617   public:
01618      class stg_cb_t
01619      {
01620      public:
01621         stg_model_callback_t callback;
01622         void* arg;
01623             
01624         stg_cb_t( stg_model_callback_t cb, void* arg ) 
01625           : callback(cb), arg(arg) {}
01626             
01627         stg_cb_t( stg_world_callback_t cb, void* arg ) 
01628           : callback(NULL), arg(arg) { (void)cb; }
01629             
01630         stg_cb_t() : callback(NULL), arg(NULL) {}
01631             
01633         bool operator<( const stg_cb_t& other ) const
01634         { return ((void*)(callback)) < ((void*)(other.callback)); }
01635             
01637         bool operator==( const stg_cb_t& other ) const
01638         { return( callback == other.callback);  }           
01639      };
01640         
01641      class Flag
01642      {
01643      public:
01644         Color color;
01645         double size;
01646         static int displaylist;
01647         
01648         Flag( Color color, double size );
01649         Flag* Nibble( double portion );
01650         
01653         void Draw(  GLUquadric* quadric );
01654      };
01655 
01656   protected:
01660      static std::map<void*, std::set<stg_cb_t> > callbacks;
01661         
01663      Color color;
01664      double friction;
01665         
01669      bool data_fresh;
01670      stg_bool_t disabled; 
01671      std::list<Visualizer*> cv_list;
01672      std::list<Flag*> flag_list;
01673      Geom geom;
01674 
01676      class GuiState
01677      {
01678      public:
01679         bool grid;
01680         bool move;
01681         bool nose;
01682         bool outline;
01683         
01684         GuiState();
01685         void Load( Worldfile* wf, int wf_entity );
01686      } gui;
01687      
01688      bool has_default_block;
01689   
01690      /* Hooks for attaching special callback functions (not used as
01691          variables - we just need unique addresses for them.) */  
01692      class CallbackHooks
01693      {
01694      public:
01695         int flag_incr;
01696         int flag_decr;
01697         int init;
01698         int load;
01699         int save;
01700         int shutdown;
01701         int startup;
01702         int update;
01703         int update_done;
01704 
01705          /* optimization: record the number of attached callbacks for pose
01706                 and velocity, so we can cheaply determine whether we need to
01707                 call a callback for SetPose() and SetVelocity(), which happen
01708                 very frequently. */
01709          int attached_velocity;
01710          int attached_pose;
01711          int attached_update;
01712          
01713          CallbackHooks() : 
01714              attached_velocity(0), 
01715              attached_pose(0), 
01716              attached_update(0) 
01717          {}
01718 
01719      } hooks;
01720   
01722      uint32_t id;   
01723      stg_usec_t interval; 
01724      stg_usec_t interval_energy; 
01725      stg_usec_t interval_pose; 
01726 
01727      stg_usec_t last_update; 
01728      bool log_state; 
01729      stg_meters_t map_resolution;
01730      stg_kg_t mass;
01731 
01733      Model* parent; 
01734 
01737      Pose pose;
01738 
01740      PowerPack* power_pack;
01741 
01744      std::list<PowerPack*> pps_charging;
01745         
01749      std::map<std::string,const void*> props;
01750 
01752      class RasterVis : public Visualizer
01753      {
01754      private:
01755         uint8_t* data;
01756         unsigned int width, height;
01757         stg_meters_t cellwidth, cellheight;
01758         std::vector<stg_point_t> pts;
01759       
01760      public:
01761         RasterVis();
01762         virtual ~RasterVis( void ){}
01763         virtual void Visualize( Model* mod, Camera* cam );
01764       
01765         void SetData( uint8_t* data, 
01766                           unsigned int width, 
01767                           unsigned int height,
01768                           stg_meters_t cellwidth,
01769                           stg_meters_t cellheight );
01770       
01771         int subs;     //< the number of subscriptions to this model
01772         int used;     //< the number of connections to this model
01773       
01774         void AddPoint( stg_meters_t x, stg_meters_t y );
01775         void ClearPts();
01776       
01777      } rastervis;
01778      
01779      bool rebuild_displaylist; 
01780      std::string say_string;   
01781         
01782      stg_bool_t stall;
01783      int subs;    
01784 
01786      bool thread_safe;
01787      
01789      class TrailItem 
01790      {                                                                                          
01791      public:
01792         stg_usec_t time;
01793         Pose pose;
01794         Color color;
01795         
01796         TrailItem() 
01797           : time(0), pose(), color(){}
01798          
01799          //TrailItem( stg_usec_t time, Pose pose, Color color ) 
01800          //: time(time), pose(pose), color(color){}
01801      };
01802     
01804         std::vector<TrailItem> trail;
01805 
01807         unsigned int trail_index;
01808 
01812      static unsigned int trail_length;
01813      
01815      static uint64_t trail_interval;
01816      
01818         void UpdateTrail();
01819 
01820      //stg_model_type_t type;  
01821      const std::string type;
01824         unsigned int event_queue_num; 
01825         bool used;   
01826         Velocity velocity;
01827         
01831         bool velocity_enable;
01832         
01833         stg_watts_t watts;
01834      
01837      stg_watts_t watts_give;
01838     
01841      stg_watts_t watts_take;
01842     
01843      Worldfile* wf;
01844      int wf_entity;
01845      World* world; // pointer to the world in which this model exists
01846      WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode
01847 
01848   public:
01849      
01850      const std::string& GetModelType() const {return type;}  
01851      std::string GetSayString(){return std::string(say_string);}
01852      
01855     Model* GetChild( const std::string& name ) const;
01856 
01857      class Visibility
01858      {
01859      public:
01860         bool blob_return;
01861         int fiducial_key;
01862         int fiducial_return;
01863         bool gripper_return;
01864         stg_laser_return_t laser_return;
01865         bool obstacle_return;
01866         bool ranger_return;
01867         bool gravity_return;
01868         bool sticky_return;
01869         
01870         Visibility();
01871         void Load( Worldfile* wf, int wf_entity );
01872      } vis;
01873      
01874      stg_usec_t GetUpdateInterval(){ return interval; }
01875      stg_usec_t GetEnergyInterval(){ return interval_energy; }
01876      stg_usec_t GetPoseInterval(){ return interval_pose; }
01877      
01880      void Rasterize( uint8_t* data, 
01881                           unsigned int width, unsigned int height,
01882                           stg_meters_t cellwidth, stg_meters_t cellheight );
01883     
01884   private: 
01887      explicit Model(const Model& original);
01888 
01891      Model& operator=(const Model& original);
01892 
01893   protected:
01894 
01896      void RegisterOption( Option* opt );
01897 
01898      void AppendTouchingModels( ModelPtrSet& touchers );
01899 
01903      Model* TestCollision();
01904 
01907     Model* TestCollisionTree();
01908   
01909      void CommitTestedPose();
01910 
01911      void Map();
01912      void UnMap();
01913 
01914      void MapWithChildren();
01915      void UnMapWithChildren();
01916   
01917      // Find the root model, and map/unmap the whole tree.
01918      void MapFromRoot();
01919      void UnMapFromRoot();
01920 
01923      stg_raytrace_result_t Raytrace( const Pose &pose,
01924                                                 const stg_meters_t range, 
01925                                                 const stg_ray_test_func_t func,
01926                                                 const void* arg,
01927                                                 const bool ztest = true );
01928   
01931      void Raytrace( const Pose &pose,
01932                          const stg_meters_t range, 
01933                          const stg_radians_t fov, 
01934                          const stg_ray_test_func_t func,
01935                          const void* arg,
01936                          stg_raytrace_result_t* samples,
01937                          const uint32_t sample_count,
01938                          const bool ztest = true  );
01939   
01940      stg_raytrace_result_t Raytrace( const stg_radians_t bearing,            
01941                                                 const stg_meters_t range,
01942                                                 const stg_ray_test_func_t func,
01943                                                 const void* arg,
01944                                                 const bool ztest = true );
01945   
01946      void Raytrace( const stg_radians_t bearing,             
01947                          const stg_meters_t range,
01948                          const stg_radians_t fov,
01949                          const stg_ray_test_func_t func,
01950                          const void* arg,
01951                          stg_raytrace_result_t* samples,
01952                          const uint32_t sample_count,
01953                          const bool ztest = true );
01954   
01955 
01959      void GPoseDirtyTree();
01960 
01961      virtual void Startup();
01962      virtual void Shutdown();
01963      virtual void Update();
01964      virtual void UpdatePose();
01965      virtual void UpdateCharge();
01966 
01967      Model* ConditionalMove( const Pose& newpose );
01968 
01969      stg_meters_t ModelHeight() const;
01970 
01971      void DrawBlocksTree();
01972      virtual void DrawBlocks();
01973      void DrawBoundingBox();
01974      void DrawBoundingBoxTree();
01975      virtual void DrawStatus( Camera* cam );
01976      void DrawStatusTree( Camera* cam );
01977   
01978      void DrawOriginTree();
01979      void DrawOrigin();
01980   
01981      void PushLocalCoords();
01982      void PopCoords();
01983   
01985      void DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width=1.0, double height=1.0 );
01986     
01987      virtual void DrawPicker();
01988      virtual void DataVisualize( Camera* cam );  
01989      virtual void DrawSelected(void);
01990     
01991      void DrawTrailFootprint();
01992      void DrawTrailBlocks();
01993      void DrawTrailArrows();
01994      void DrawGrid();
01995      // void DrawBlinkenlights();
01996      void DataVisualizeTree( Camera* cam );
01997      void DrawFlagList();
01998      void DrawPose( Pose pose );
01999     
02000   public:
02001      virtual void PushColor( Color col ){ world->PushColor( col ); }    
02002      virtual void PushColor( double r, double g, double b, double a ){ world->PushColor( r,g,b,a ); }   
02003      virtual void PopColor()    { world->PopColor(); }
02004     
02005      PowerPack* FindPowerPack() const;
02006     
02007      //void RecordRenderPoint( GSList** head, GSList* link, 
02008      //                 unsigned int* c1, unsigned int* c2 );
02009 
02010      void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, 
02011                                     stg_meters_t ymin, stg_meters_t ymax );
02012     
02014      std::string PoseString()
02015      { return pose.String(); }
02016     
02018      static Model* LookupId( uint32_t id )
02019      { return modelsbyid[id]; }
02020      
02022      Model( World* world, 
02023               Model* parent = NULL, 
02024               const std::string& type = "model" );
02025      
02027      virtual ~Model();
02028     
02029      void Say( const std::string& str );
02030      
02032      void AddVisualizer( Visualizer* custom_visual, bool on_by_default );
02033 
02035      void RemoveVisualizer( Visualizer* custom_visual );
02036 
02037      void BecomeParentOf( Model* child );
02038 
02039      void Load( Worldfile* wf, int wf_entity )
02040      {
02043         SetWorldfile( wf, wf_entity );
02044         Load(); // call virtual load
02045      }
02046     
02048      void SetWorldfile( Worldfile* wf, int wf_entity )
02049      { this->wf = wf; this->wf_entity = wf_entity; }
02050     
02052      virtual void Load();
02053     
02055      virtual void Save();
02056     
02058      void InitControllers();
02059 
02060      void AddFlag(  Flag* flag );
02061      void RemoveFlag( Flag* flag );
02062     
02063      void PushFlag( Flag* flag );
02064      Flag* PopFlag();
02065     
02066      int GetFlagCount() const { return flag_list.size(); }
02067     
02072      void Disable(){ disabled = true; };
02073 
02076      void Enable(){ disabled = false; };
02077   
02080      void LoadControllerModule( const char* lib );
02081     
02084      void NeedRedraw();
02085     
02088      void LoadBlock( Worldfile* wf, int entity );
02089 
02092      Block* AddBlockRect( stg_meters_t x, stg_meters_t y, 
02093                                  stg_meters_t dx, stg_meters_t dy, 
02094                                  stg_meters_t dz );
02095     
02097      void ClearBlocks();
02098   
02101      Model* Parent() const { return this->parent; }
02102 
02104      World* GetWorld() const { return this->world; }
02105   
02107      Model* Root(){ return(  parent ? parent->Root() : this ); }
02108   
02109      bool IsAntecedent( const Model* testmod ) const;
02110     
02112      bool IsDescendent( const Model* testmod ) const;
02113     
02115      bool IsRelated( const Model* testmod ) const;
02116 
02118      Pose GetGlobalPose() const;
02119     
02121      Velocity GetGlobalVelocity()  const;
02122     
02123      /* set the velocity of a model in the global coordinate system */
02124      void SetGlobalVelocity( const Velocity& gvel );
02125     
02127      void Subscribe();
02128     
02130      void Unsubscribe();
02131     
02133      void SetGlobalPose(  const Pose& gpose );
02134     
02136      void SetVelocity(  const Velocity& vel );
02137     
02139         void VelocityEnable();
02140 
02142         void VelocityDisable();
02143 
02145      void SetPose(  const Pose& pose );
02146     
02148      void AddToPose(  const Pose& pose );
02149     
02151      void AddToPose(  double dx, double dy, double dz, double da );
02152     
02154      void SetGeom(  const Geom& src );
02155   
02158      void SetFiducialReturn(  int fid );
02159   
02161      int GetFiducialReturn()  const { return vis.fiducial_return; }
02162   
02165      void SetFiducialKey(  int key );
02166     
02167      Color GetColor() const { return color; }
02168      
02170      uint32_t GetId()  const { return id; }
02171      
02173      stg_kg_t GetTotalMass();
02174      
02176      stg_kg_t GetMassOfChildren();
02177 
02179      int SetParent( Model* newparent);
02180     
02183      Geom GetGeom() const { return geom; }
02184     
02187      Pose GetPose() const { return pose; }
02188     
02191      Velocity GetVelocity() const { return velocity; }
02192     
02193      // guess what these do?
02194      void SetColor( Color col );
02195      void SetMass( stg_kg_t mass );
02196      void SetStall( stg_bool_t stall );
02197      void SetGravityReturn( int val );
02198      void SetGripperReturn( int val );
02199      void SetStickyReturn( int val );
02200      void SetLaserReturn( stg_laser_return_t val );
02201      void SetObstacleReturn( int val );
02202      void SetBlobReturn( int val );
02203      void SetRangerReturn( int val );
02204      void SetBoundary( int val );
02205      void SetGuiNose( int val );
02206      void SetGuiMove( int val );
02207      void SetGuiGrid( int val );
02208      void SetGuiOutline( int val );
02209      void SetWatts( stg_watts_t watts );
02210      void SetMapResolution( stg_meters_t res );
02211      void SetFriction( double friction );
02212     
02213      bool DataIsFresh() const { return this->data_fresh; }
02214     
02215      /* attach callback functions to data members. The function gets
02216          called when the member is changed using SetX() accessor method */
02217         
02218      void AddCallback( void* address, 
02219                              stg_model_callback_t cb, 
02220                              void* user );
02221         
02222      int RemoveCallback( void* member,
02223                                 stg_model_callback_t callback );
02224         
02225      int CallCallbacks(  void* address );
02226         
02227      /* wrappers for the generic callback add & remove functions that hide
02228          some implementation detail */
02229     
02230      void AddStartupCallback( stg_model_callback_t cb, void* user )
02231      { AddCallback( &hooks.startup, cb, user ); };
02232     
02233      void RemoveStartupCallback( stg_model_callback_t cb )
02234      { RemoveCallback( &hooks.startup, cb ); };
02235     
02236      void AddShutdownCallback( stg_model_callback_t cb, void* user )
02237      { AddCallback( &hooks.shutdown, cb, user ); };
02238     
02239      void RemoveShutdownCallback( stg_model_callback_t cb )
02240      { RemoveCallback( &hooks.shutdown, cb ); }
02241     
02242      void AddLoadCallback( stg_model_callback_t cb, void* user )
02243      { AddCallback( &hooks.load, cb, user ); }
02244     
02245      void RemoveLoadCallback( stg_model_callback_t cb )
02246      { RemoveCallback( &hooks.load, cb ); }
02247     
02248      void AddSaveCallback( stg_model_callback_t cb, void* user )
02249      { AddCallback( &hooks.save, cb, user ); }
02250     
02251      void RemoveSaveCallback( stg_model_callback_t cb )
02252      { RemoveCallback( &hooks.save, cb ); }
02253   
02254      void AddUpdateCallback( stg_model_callback_t cb, void* user )
02255      { AddCallback( &hooks.update, cb, user ); }
02256      
02257      void RemoveUpdateCallback( stg_model_callback_t cb )
02258      {  RemoveCallback( &hooks.update, cb ); }
02259      
02260      void AddFlagIncrCallback( stg_model_callback_t cb, void* user )
02261      {  AddCallback( &hooks.flag_incr, cb, user ); }
02262     
02263      void RemoveFlagIncrCallback( stg_model_callback_t cb )
02264      {  RemoveCallback( &hooks.flag_incr, cb ); }
02265 
02266      void AddFlagDecrCallback( stg_model_callback_t cb, void* user )
02267      {  AddCallback( &hooks.flag_decr, cb, user ); }
02268     
02269      void RemoveFlagDecrCallback( stg_model_callback_t cb )
02270      {  RemoveCallback( &hooks.flag_decr, cb ); }
02271      
02272         
02273      virtual void Print( char* prefix ) const;
02274      virtual const char* PrintWithPose() const;
02275     
02278      Pose GlobalToLocal( const Pose& pose ) const;
02279      
02282      Pose LocalToGlobal( const Pose& pose ) const
02283      {  
02284         return( ( GetGlobalPose() + geom.pose ) + pose );
02285      }
02286         
02288         void LocalToPixels( const std::vector<stg_point_t>& local,
02289                                                 std::vector<stg_point_int_t>& pixels) const;
02290         
02293      stg_point_t LocalToGlobal( const stg_point_t& pt) const;       
02294 
02297      Model* GetUnsubscribedModelOfType( const std::string& type ) const;
02298     
02301      Model* GetUnusedModelOfType( const std::string& type );
02302   
02305      bool Stalled() const { return this->stall; }
02306      
02309      unsigned int GetSubscriptionCount() const { return subs; }
02310 
02312      bool HasSubscribers() const { return( subs > 0 ); }     
02313 
02314      static std::map< std::string, creator_t> name_map;  
02315   };
02316 
02317 
02318   // BLOBFINDER MODEL --------------------------------------------------------
02319 
02320 
02322   class ModelBlobfinder : public Model
02323   {
02324   public:
02326      class Blob
02327      {
02328      public:
02329         Color color;
02330         uint32_t left, top, right, bottom;
02331         stg_meters_t range;
02332      };
02333 
02334      class Vis : public Visualizer 
02335      {
02336      private:
02337         //static Option showArea;
02338      public:
02339         Vis( World* world );
02340         virtual ~Vis( void ){}
02341         virtual void Visualize( Model* mod, Camera* cam );
02342      } vis;
02343 
02344   private:
02345      std::vector<Blob> blobs;
02346      std::vector<Color> colors;
02347 
02348      // predicate for ray tracing
02349      static bool BlockMatcher( Block* testblock, Model* finder );
02350 
02351   public:
02352      stg_radians_t fov;
02353      stg_radians_t pan;
02354      stg_meters_t range;
02355      unsigned int scan_height;
02356      unsigned int scan_width;
02357      
02358      // constructor
02359      ModelBlobfinder( World* world,
02360                             Model* parent,
02361                             const std::string& type );
02362      // destructor
02363      ~ModelBlobfinder();
02364     
02365      virtual void Startup();
02366      virtual void Shutdown();
02367      virtual void Update();
02368      virtual void Load();
02369         
02370      Blob* GetBlobs( unsigned int* count )
02371      { 
02372         if( count ) *count = blobs.size();
02373         return &blobs[0];
02374      }
02375 
02377      void AddColor( Color col );
02378 
02380      void RemoveColor( Color col );
02381 
02384      void RemoveAllColors();
02385   };
02386 
02387 
02388 
02389 
02390   // LASER MODEL --------------------------------------------------------
02391   
02393   class ModelLaser : public Model
02394   {
02395   public:
02397      class Sample
02398      {
02399      public:
02400         stg_meters_t range; 
02401         double reflectance; 
02402      };
02403         
02405      class Config
02406      {
02407      public:
02408         uint32_t sample_count; 
02409         uint32_t resolution; 
02410         Bounds range_bounds; 
02411         stg_radians_t fov; 
02412         stg_usec_t interval; 
02413      };
02414         
02415   private:   
02416      class Vis : public Visualizer 
02417      {
02418      private:
02419         static Option showArea;
02420         static Option showStrikes;
02421         static Option showFov;
02422         static Option showBeams;
02423 
02424      public:
02425         Vis( World* world );        virtual ~Vis( void ){}
02426         virtual void Visualize( Model* mod, Camera* cam );
02427      } vis;
02428         
02429      unsigned int sample_count;
02430      std::vector<Sample> samples;
02431 
02432      stg_meters_t range_max;
02433      stg_radians_t fov;
02434      uint32_t resolution;
02435     
02436      // set up data buffers after the config changes
02437      void SampleConfig();
02438 
02439   public:
02440      // constructor
02441      ModelLaser( World* world,
02442                      Model* parent,
02443                      const std::string& type ); 
02444   
02445      // destructor
02446      ~ModelLaser();
02447     
02448      virtual void Startup();
02449      virtual void Shutdown();
02450      virtual void Update();
02451      virtual void Load();
02452      virtual void Print( char* prefix );
02453   
02455      Sample* GetSamples( uint32_t* count );
02456      
02458      const std::vector<Sample>& GetSamples();
02459      
02461      Config GetConfig( );
02462      
02464      void SetConfig( Config& cfg );  
02465   };
02466   
02467 
02468   // Light indicator model
02469   class ModelLightIndicator : public Model
02470   {
02471   public:
02472      ModelLightIndicator( World* world, 
02473                                  Model* parent,
02474                                  const std::string& type );
02475      ~ModelLightIndicator();
02476   
02477      void SetState(bool isOn);
02478 
02479   protected:
02480      virtual void DrawBlocks();
02481 
02482   private:
02483      bool m_IsOn;
02484   };
02485 
02486   // \todo  GRIPPER MODEL --------------------------------------------------------
02487 
02488 
02489   class ModelGripper : public Model
02490   {
02491   public:
02492 
02493      enum paddle_state_t {
02494         PADDLE_OPEN = 0, // default state
02495         PADDLE_CLOSED, 
02496         PADDLE_OPENING,
02497         PADDLE_CLOSING,
02498      };
02499      
02500      enum lift_state_t {
02501         LIFT_DOWN = 0, // default state
02502         LIFT_UP, 
02503         LIFT_UPPING, // verbed these to match the paddle state
02504         LIFT_DOWNING, 
02505      };
02506      
02507      enum cmd_t {
02508         CMD_NOOP = 0, // default state
02509         CMD_OPEN, 
02510         CMD_CLOSE,
02511         CMD_UP, 
02512         CMD_DOWN    
02513      };
02514      
02515      
02518      struct config_t
02519      {
02520         Size paddle_size; 
02521         paddle_state_t paddles;
02522         lift_state_t lift;      
02523         double paddle_position; 
02524         double lift_position; 
02525         Model* gripped;
02526         bool paddles_stalled; // true iff some solid object stopped the paddles closing or opening
02527         double close_limit; 
02528         bool autosnatch; 
02529         double break_beam_inset[2]; 
02530       Model* beam[2]; 
02531       Model* contact[2]; 
02532      };
02533      
02534   private:
02535      virtual void Update();
02536      virtual void DataVisualize( Camera* cam );
02537      
02538      void FixBlocks();
02539      void PositionPaddles();
02540      void UpdateBreakBeams();
02541      void UpdateContacts();
02542 
02543      config_t cfg;
02544      cmd_t cmd;
02545      
02546      Block* paddle_left;
02547      Block* paddle_right;
02548 
02549      static Option showData;
02550 
02551   public:    
02552      static const Size size;
02553 
02554      // constructor
02555      ModelGripper( World* world,
02556                         Model* parent,
02557                         const std::string& type );
02558      // destructor
02559      virtual ~ModelGripper();
02560   
02561      virtual void Load();
02562      virtual void Save();
02563 
02565      void SetConfig( config_t & newcfg ){ this->cfg = newcfg; FixBlocks(); }
02566      
02568      config_t GetConfig(){ return cfg; };
02569      
02571      void SetCommand( cmd_t cmd ) { this->cmd = cmd; }
02573      void CommandClose() { SetCommand( CMD_CLOSE ); }
02575      void CommandOpen() { SetCommand( CMD_OPEN ); }
02577      void CommandUp() { SetCommand( CMD_UP ); }
02579      void CommandDown() { SetCommand( CMD_DOWN ); }
02580   };
02581 
02582 
02583   // \todo BUMPER MODEL --------------------------------------------------------
02584 
02585   //   typedef struct
02586   //   {
02587   //     Pose pose;
02588   //     stg_meters_t length;
02589   //   } stg_bumper_config_t;
02590 
02591   //   typedef struct
02592   //   {
02593   //     Model* hit;
02594   //     stg_point_t hit_point;
02595   //   } stg_bumper_sample_t;
02596 
02597 
02598   // FIDUCIAL MODEL --------------------------------------------------------
02599 
02601   class ModelFiducial : public Model
02602   {
02603   public:  
02605      class Fiducial
02606      {
02607      public:
02608         stg_meters_t range; 
02609         stg_radians_t bearing; 
02610         Pose geom; 
02611         Pose pose; 
02612         Model* mod; 
02613         int id; 
02614      };
02615 
02616   private:
02617      // if neighbor is visible, add him to the fiducial scan
02618      void AddModelIfVisible( Model* him );
02619 
02620      virtual void Update();
02621      virtual void DataVisualize( Camera* cam );
02622 
02623      static Option showData;
02624      static Option showFov;
02625      
02626      std::vector<Fiducial> fiducials;
02627         
02628   public:       
02629      ModelFiducial( World* world, 
02630                          Model* parent,
02631                          const std::string& type );
02632      virtual ~ModelFiducial();
02633     
02634      virtual void Load();
02635      void Shutdown( void );
02636 
02637      stg_meters_t max_range_anon;
02638      stg_meters_t max_range_id; 
02639      stg_meters_t min_range; 
02640      stg_radians_t fov; 
02641      stg_radians_t heading; 
02642      int key; 
02643         
02644         
02646      std::vector<Fiducial>& GetFiducials() { return fiducials; }
02647         
02649      Fiducial* GetFiducials( unsigned int* count )
02650      {
02651         if( count ) *count = fiducials.size();
02652         return &fiducials[0];
02653      }
02654   };
02655 
02656 
02657   // RANGER MODEL --------------------------------------------------------
02658 
02660   class ModelRanger : public Model
02661   {
02662   public:
02663      class Sensor
02664      {
02665      public:
02666         Pose pose;
02667         Size size;
02668         Bounds bounds_range;
02669         stg_radians_t fov;
02670         int ray_count;
02671         stg_meters_t range;
02672      };
02673 
02674   protected:
02675 
02676      virtual void Startup();
02677      virtual void Shutdown();
02678      virtual void Update();
02679      virtual void DataVisualize( Camera* cam );
02680     
02681   public:
02682      ModelRanger( World* world, Model* parent,
02683                       const std::string& type );
02684      virtual ~ModelRanger();
02685     
02686      virtual void Load();
02687      virtual void Print( char* prefix );
02688      
02689      std::vector<Sensor> sensors;
02690     
02691   private:
02692      static Option showRangerData;
02693      static Option showRangerTransducers;       
02694   };
02695     
02696   // BLINKENLIGHT MODEL ----------------------------------------------------
02697   class ModelBlinkenlight : public Model
02698   {
02699   private:
02700      double dutycycle;
02701      bool enabled;
02702      stg_msec_t period;
02703      bool on;
02704 
02705      static Option showBlinkenData;
02706   public:
02707      ModelBlinkenlight( World* world,
02708                               Model* parent,
02709                               const std::string& type );
02710 
02711      ~ModelBlinkenlight();
02712     
02713      virtual void Load();
02714      virtual void Update();
02715      virtual void DataVisualize( Camera* cam );
02716   };
02717 
02718     
02719   // CAMERA MODEL ----------------------------------------------------
02720 
02722   class ModelCamera : public Model
02723   {
02724   public:
02725      typedef struct 
02726      {
02727         // GL_V3F
02728         GLfloat x, y, z;
02729      } ColoredVertex;
02730   
02731   private:
02732      Canvas* _canvas;
02733 
02734      GLfloat* _frame_data;  //opengl read buffer
02735      GLubyte* _frame_color_data;  //opengl read buffer
02736 
02737      bool _valid_vertexbuf_cache;
02738      ColoredVertex* _vertexbuf_cache; //cached unit vectors with appropriate rotations (these must be scalled by z-buffer length)
02739     
02740      int _width;         //width of buffer
02741      int _height;        //height of buffer
02742      static const int _depth = 4;
02743     
02744      int _camera_quads_size;
02745      GLfloat* _camera_quads;
02746      GLubyte* _camera_colors;
02747     
02748      static Option showCameraData;
02749     
02750      PerspectiveCamera _camera;
02751      float _yaw_offset; //position camera is mounted at
02752      float _pitch_offset;
02753         
02755      bool GetFrame();
02756     
02757   public:
02758      ModelCamera( World* world,
02759                       Model* parent,
02760                       const std::string& type ); 
02761       
02762      ~ModelCamera();
02763   
02764      virtual void Load();
02765     
02767      virtual void Update();
02768     
02770      //virtual void Draw( uint32_t flags, Canvas* canvas );
02771     
02773      virtual void DataVisualize( Camera* cam );
02774     
02776      int getWidth( void ) const { return _width; }
02777     
02779      int getHeight( void ) const { return _height; }
02780     
02782      const PerspectiveCamera& getCamera( void ) const { return _camera; }
02783     
02785      const GLfloat* FrameDepth() const { return _frame_data; }
02786     
02788      const GLubyte* FrameColor() const { return _frame_color_data; }
02789     
02791      void setPitch( float pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; }
02792     
02794      void setYaw( float yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; }
02795   };
02796 
02797   // POSITION MODEL --------------------------------------------------------
02798 
02800   class ModelPosition : public Model
02801   {
02802      friend class Canvas;
02803 
02804   public:
02806      typedef enum
02807         { CONTROL_VELOCITY, 
02808           CONTROL_POSITION 
02809         } ControlMode;
02810      
02812      typedef enum
02813         { LOCALIZATION_GPS, 
02814           LOCALIZATION_ODOM 
02815         } LocalizationMode;
02816      
02818      typedef enum
02819         { DRIVE_DIFFERENTIAL, 
02820           DRIVE_OMNI, 
02821           DRIVE_CAR 
02822         } DriveMode;
02823      
02824   private:
02825      Pose goal;
02826      ControlMode control_mode;
02827      DriveMode drive_mode;
02828      LocalizationMode localization_mode; 
02829      Velocity integration_error; 
02830      
02831      
02832   public:
02833      // constructor
02834      ModelPosition( World* world,
02835                          Model* parent,
02836                          const std::string& type );
02837      // destructor
02838      ~ModelPosition();
02839 
02840      virtual void Startup();
02841      virtual void Shutdown();
02842      virtual void Update();
02843      virtual void Load();
02844         
02847      class Waypoint
02848      {
02849      public:
02850         Waypoint( stg_meters_t x, stg_meters_t y, stg_meters_t z, stg_radians_t a, Color color ) ;
02851         Waypoint( const Pose& pose, Color color ) ;
02852         Waypoint();
02853         void Draw() const;
02854         
02855         Pose pose;
02856         Color color;
02857      };
02858      
02859      std::vector<Waypoint> waypoints;
02860 
02861      class WaypointVis : public Visualizer
02862      {
02863      public:
02864         WaypointVis();
02865         virtual ~WaypointVis( void ){}
02866         virtual void Visualize( Model* mod, Camera* cam );
02867      } wpvis;
02868      
02869      class PoseVis : public Visualizer
02870      {
02871      public:
02872         PoseVis();
02873         virtual ~PoseVis( void ){}
02874         virtual void Visualize( Model* mod, Camera* cam );
02875      } posevis;
02876 
02878      void SetOdom( Pose odom );
02879         
02882      void SetSpeed( double x, double y, double a );
02883      void SetXSpeed( double x );
02884      void SetYSpeed( double y );
02885      void SetZSpeed( double z );
02886      void SetTurnSpeed( double a );
02887      void SetSpeed( Velocity vel );
02889      void Stop();
02890 
02893      void GoTo( double x, double y, double a );
02894      void GoTo( Pose pose );
02895 
02896      // localization state
02897      Pose est_pose; 
02898      Pose est_pose_error; 
02899      Pose est_origin; 
02900   };
02901 
02902 
02903   // ACTUATOR MODEL --------------------------------------------------------
02904 
02906   class ModelActuator : public Model
02907   {
02908   public:
02910      typedef enum
02911         { CONTROL_VELOCITY,
02912           CONTROL_POSITION
02913         } ControlMode;
02914   
02916      typedef enum
02917         { TYPE_LINEAR,
02918           TYPE_ROTATIONAL
02919         } ActuatorType;
02920   
02921   private:
02922      double goal; //< the current velocity or pose to reach, depending on the value of control_mode
02923      double pos;
02924      double max_speed;
02925      double min_position;
02926      double max_position;
02927      ControlMode control_mode;
02928      ActuatorType actuator_type;
02929      stg_point3_t axis;
02930   
02931      Pose InitialPose;
02932   public:  
02933      // constructor
02934      ModelActuator( World* world,
02935                          Model* parent,
02936                          const std::string& type );
02937      // destructor
02938      ~ModelActuator();
02939   
02940      virtual void Startup();
02941      virtual void Shutdown();
02942      virtual void Update();
02943      virtual void Load();
02944   
02947      void SetSpeed( double speed );
02948   
02949      double GetSpeed() const {return goal;}
02950   
02953      void GoTo( double pose );
02954   
02955      double GetPosition() const {return pos;};
02956      double GetMaxPosition() const {return max_position;};
02957      double GetMinPosition() const {return min_position;};
02958   
02959   };
02960 
02961 
02962 }; // end namespace stg
02963 
02964 #endif
Generated by  doxygen 1.6.2-20100208