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 & GLUT
00064 #ifdef __APPLE__
00065 #include <OpenGL/glu.h>
00066 //#include <GLUT/glut.h>
00067 #else
00068 #include <GL/glu.h>
00069 //#include <GL/glut.h>
00070 #endif 
00071 
00073 namespace Stg 
00074 {
00075   // forward declare
00076   class Block;
00077   class Canvas;
00078   class Cell;
00079   class Worldfile;
00080   class World;
00081   class WorldGui;
00082   class Model;
00083   class OptionsDlg;
00084   class Camera;
00085   class FileManager;
00086   class Option;
00087   
00088   typedef Model* (*creator_t)( World*, Model*, const std::string& type );
00089   
00091   typedef std::set<Model*> ModelPtrSet;
00092 
00094   typedef std::vector<Model*> ModelPtrVec;
00095 
00097   typedef std::set<Block*> BlockPtrSet;
00098 
00100   typedef std::vector<Cell*> CellPtrVec;
00101 
00103   void Init( int* argc, char** argv[] );
00104 
00106   bool InitDone();
00107   
00110   const char* Version();
00111 
00113   const char COPYRIGHT[] =                     
00114     "Copyright Richard Vaughan and contributors 2000-2009";
00115 
00117   const char AUTHORS[] =                    
00118     "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, Toby Collett, Jeremy Asher, Alex Couture-Beil and contributors.";
00119 
00121   const char WEBSITE[] = "http://playerstage.org";
00122 
00124   const char DESCRIPTION[] =                       
00125     "Robot simulation library\nPart of the Player Project";
00126 
00128   const char LICENSE[] = 
00129     "Stage robot simulation library\n"                  \
00130     "Copyright (C) 2000-2009 Richard Vaughan and contributors\n"    \
00131     "Part of the Player Project [http://playerstage.org]\n"     \
00132     "\n"                                \
00133     "This program is free software; you can redistribute it and/or\n"   \
00134     "modify it under the terms of the GNU General Public License\n" \
00135     "as published by the Free Software Foundation; either version 2\n"  \
00136     "of the License, or (at your option) any later version.\n"      \
00137     "\n"                                \
00138     "This program is distributed in the hope that it will be useful,\n" \
00139     "but WITHOUT ANY WARRANTY; without even the implied warranty of\n"  \
00140     "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n"   \
00141     "GNU General Public License for more details.\n"            \
00142     "\n"                                \
00143     "You should have received a copy of the GNU General Public License\n" \
00144     "along with this program; if not, write to the Free Software\n" \
00145     "Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.\n" \
00146     "\n"                                \
00147     "The text of the license may also be available online at\n"     \
00148     "http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";
00149   
00151   const double thousand = 1e3;
00152 
00154   const double million = 1e6;
00155 
00157   const double billion = 1e9;
00158 
00160   inline double rtod( double r ){ return( r*180.0/M_PI ); }
00161   
00163   inline double dtor( double d ){ return( d*M_PI/180.0 ); }
00164   
00166   inline double normalize( double a )
00167   {
00168      while( a < -M_PI ) a += 2.0*M_PI;
00169      while( a >  M_PI ) a -= 2.0*M_PI;   
00170      return a;
00171   };
00172 
00174   inline int sgn( int a){ return( a<0 ? -1 : 1); }
00175 
00177   inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); }
00178   
00180   enum { FiducialNone = 0 };
00181 
00183   typedef uint32_t stg_id_t;
00184 
00186   typedef double stg_meters_t;
00187 
00189   typedef double stg_radians_t;
00190 
00192   typedef struct timeval stg_time_t;
00193 
00195   typedef unsigned long stg_msec_t;
00196 
00198   typedef uint64_t stg_usec_t;
00199 
00201   typedef double stg_kg_t; // Kilograms (mass)
00202 
00204   typedef double stg_joules_t;
00205 
00207   typedef double stg_watts_t;
00208   
00210   typedef bool stg_bool_t;
00211   
00212   class Color
00213   {
00214   public:
00215      float r,g,b,a;
00216     
00217      Color( float r, float g, float b, float a=1.0 );
00218     
00222      Color( const std::string& name );  
00223     
00224      Color();
00225     
00226      bool operator!=( const Color& other );
00227      bool operator==( const Color& other );
00228      static Color RandomColor();
00229      void Print( const char* prefix );
00230   };
00231   
00233   class Size
00234   {
00235   public:
00236     stg_meters_t x, y, z;
00237     
00238     Size( stg_meters_t x, 
00239              stg_meters_t y, 
00240              stg_meters_t z )
00241       : x(x), y(y), z(z)
00242     {/*empty*/}
00243     
00245     Size() : x( 0.4 ), y( 0.4 ), z( 1.0 )
00246     {/*empty*/} 
00247     
00248      void Load( Worldfile* wf, int section, const char* keyword );
00249      void Save( Worldfile* wf, int section, const char* keyword );
00250      
00251      void Zero()
00252      { x=y=z=0.0; }
00253   };
00254   
00256   class Pose
00257   {
00258   public:
00259     stg_meters_t x, y, z;
00260     stg_radians_t a;
00261     
00262     Pose( stg_meters_t x, 
00263              stg_meters_t y, 
00264              stg_meters_t z,
00265              stg_radians_t a ) 
00266       : x(x), y(y), z(z), a(a)
00267     { /*empty*/ }
00268     
00269     Pose() : x(0.0), y(0.0), z(0.0), a(0.0)
00270     { /*empty*/ }        
00271     
00272     virtual ~Pose(){};
00273     
00276     static Pose Random( stg_meters_t xmin, stg_meters_t xmax, 
00277                                 stg_meters_t ymin, stg_meters_t ymax )
00278     {        
00279       return Pose( xmin + drand48() * (xmax-xmin),
00280                          ymin + drand48() * (ymax-ymin),
00281                          0, 
00282                          normalize( drand48() * (2.0 * M_PI) ));
00283     }
00284     
00288     virtual void Print( const char* prefix )
00289     {
00290       printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
00291                   prefix, x,y,z,a );
00292     }
00293     
00294      std::string String()
00295      {
00296         char buf[256];
00297         snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]",
00298                      x,y,z,a );
00299         return std::string(buf);
00300      }
00301     
00302      /* returns true iff all components of the velocity are zero. */
00303      bool IsZero() const { return( !(x || y || z || a )); };
00304     
00306      void Zero(){ x=y=z=a=0.0; }
00307     
00308      void Load( Worldfile* wf, int section, const char* keyword );
00309      void Save( Worldfile* wf, int section, const char* keyword );
00310     
00311      inline Pose operator+( const Pose& p )
00312      {
00313         const double cosa = cos(a);
00314         const double sina = sin(a);
00315       
00316         return Pose( x + p.x * cosa - p.y * sina,
00317                          y + p.x * sina + p.y * cosa,
00318                          z + p.z,
00319                          normalize(a + p.a) );   
00320      }  
00321   };
00322   
00323   
00325   class Velocity : public Pose
00326   {
00327   public:
00333     Velocity( stg_meters_t x, 
00334                   stg_meters_t y, 
00335                   stg_meters_t z,
00336                   stg_radians_t a ) :
00337         Pose( x, y, z, a )
00338     { /*empty*/ }
00339     
00340     Velocity()
00341     { /*empty*/ }        
00342     
00346     virtual void Print( const char* prefix )
00347     {
00348       printf( "%s velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
00349                   prefix, x,y,z,a );
00350     }    
00351   };
00352   
00355   class Geom
00356   {
00357   public:
00358     Pose pose;
00359     Size size;
00360     
00361     void Print( const char* prefix )
00362     {
00363       printf( "%s geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n",
00364                   prefix,
00365                   pose.x,
00366                   pose.y,
00367                   pose.a,
00368                   size.x,
00369                   size.y );
00370     }
00371      
00373      Geom() : pose(), size() {}
00374 
00376      Geom( const Pose& p, const Size& s ) : pose(p), size(s) {}
00377      
00378      void Zero()
00379      {
00380         pose.Zero();
00381         size.Zero();
00382      }
00383   };
00384   
00386   class Bounds
00387   {
00388   public:
00390     double min;
00392     double max; 
00393     
00394     Bounds() : min(0), max(0) { /* empty*/  }
00395     Bounds( double min, double max ) : min(min), max(max) { /* empty*/  }
00396   };
00397     
00399   class stg_bounds3d_t
00400   {
00401   public:
00403     Bounds x; 
00405     Bounds y; 
00407     Bounds z; 
00408 
00409      stg_bounds3d_t() : x(), y(), z() {}
00410      stg_bounds3d_t( const Bounds& x, const Bounds& y, const Bounds& z) 
00411         : x(x), y(y), z(z) {}
00412   };
00413   
00415   typedef struct
00416   {
00417     Bounds range; 
00418     stg_radians_t angle; 
00419   } stg_fov_t;
00420   
00422   class stg_point_t
00423   {
00424   public:
00425     stg_meters_t x, y;
00426      stg_point_t( stg_meters_t x, stg_meters_t y ) : x(x), y(y){}    
00427      stg_point_t() : x(0.0), y(0.0){}
00428     
00429      bool operator+=( const stg_point_t& other ) 
00430      { return ((x += other.x) && (y += other.y) ); }  
00431   };
00432   
00434   class stg_point3_t
00435   {
00436   public:
00437     stg_meters_t x,y,z;
00438      stg_point3_t( stg_meters_t x, stg_meters_t y, stg_meters_t z ) 
00439         : x(x), y(y), z(z) {}    
00440   
00441      stg_point3_t() : x(0.0), y(0.0), z(0.0) {}
00442   };
00443   
00445   class stg_point_int_t
00446   {
00447   public:
00448     int x,y;
00449      stg_point_int_t( int x, int y ) : x(x), y(y){}  
00450      stg_point_int_t() : x(0), y(0){}
00451      
00453      bool operator<( const stg_point_int_t& other ) const
00454      { return ((x < other.x) || (y < other.y) ); }
00455 
00456      bool operator==( const stg_point_int_t& other ) const
00457      { return ((x == other.x) && (y == other.y) ); }
00458   };
00459   
00460   typedef std::vector<stg_point_int_t> PointIntVec;
00461 
00464   stg_point_t* stg_unit_square_points_create();
00465   
00466   const char MP_PREFIX[] =             "_mp_";
00467   const char MP_POSE[] =               "_mp_pose";
00468   const char MP_VELOCITY[] =           "_mp_velocity";
00469   const char MP_GEOM[] =               "_mp_geom";
00470   const char MP_COLOR[] =              "_mp_color";
00471   const char MP_WATTS[] =              "_mp_watts";
00472   const char MP_FIDUCIAL_RETURN[] =    "_mp_fiducial_return";
00473   const char MP_LASER_RETURN[] =       "_mp_laser_return";
00474   const char MP_OBSTACLE_RETURN[] =    "_mp_obstacle_return";
00475   const char MP_RANGER_RETURN[] =      "_mp_ranger_return";
00476   const char MP_GRIPPER_RETURN[] =     "_mp_gripper_return";
00477   const char MP_MASS[] =               "_mp_mass";
00478 
00480   typedef enum 
00481     {
00482       LaserTransparent=0, 
00483       LaserVisible, 
00484       LaserBright  
00485     } stg_laser_return_t;
00486   
00489   namespace Gl
00490   {
00491      void pose_shift( const Pose &pose );
00492      void pose_inverse_shift( const Pose &pose );
00493      void coord_shift( double x, double y, double z, double a  );
00494      void draw_grid( stg_bounds3d_t vol );
00496      void draw_string( float x, float y, float z, const char *string);
00497      void draw_string_multiline( float x, float y, float w, float h, 
00498                                           const char *string, Fl_Align align );
00499      void draw_speech_bubble( float x, float y, float z, const char* str );
00500      void draw_octagon( float w, float h, float m );
00501      void draw_octagon( float x, float y, float w, float h, float m );
00502      void draw_vector( double x, double y, double z );
00503      void draw_origin( double len );
00504      void draw_array( float x, float y, float w, float h, 
00505                             float* data, size_t len, size_t offset, 
00506                             float min, float max );
00507      void draw_array( float x, float y, float w, float h, 
00508                             float* data, size_t len, size_t offset );
00510      void draw_centered_rect( float x, float y, float dx, float dy );
00511   } // namespace Gl
00512   
00513   void RegisterModels();
00514   
00515   
00517   class Visualizer {
00518   private:
00519      const std::string menu_name;
00520      const std::string worldfile_name;
00521      
00522   public:
00523      Visualizer( const std::string& menu_name, 
00524                      const std::string& worldfile_name ) 
00525         : menu_name( menu_name ),
00526           worldfile_name( worldfile_name )
00527      { }
00528      
00529      virtual ~Visualizer( void ) { }
00530      virtual void Visualize( Model* mod, Camera* cam ) = 0;
00531      
00532      const std::string& GetMenuName() { return menu_name; }
00533      const std::string& GetWorldfileName() { return worldfile_name; }
00534   };
00535 
00536 
00537   typedef int(*stg_model_callback_t)(Model* mod, void* user );
00538   typedef int(*stg_world_callback_t)(World* world, void* user );
00539   
00540   // return val, or minval if val < minval, or maxval if val > maxval
00541   double constrain( double val, double minval, double maxval );
00542     
00543   typedef struct 
00544   {
00545     int enabled;
00546     Pose pose;
00547     stg_meters_t size; 
00548     Color color;
00549     stg_msec_t period; 
00550     double duty_cycle; 
00551   } stg_blinkenlight_t;
00552 
00553   
00555   typedef struct
00556   {
00557     Pose pose;
00558     Size size;
00559   } stg_rotrect_t; // rotated rectangle
00560 
00566   int stg_rotrects_from_image_file( const char* filename, 
00567                                                 stg_rotrect_t** rects,
00568                                                 unsigned int* rect_count,
00569                                                 unsigned int* widthp, 
00570                                                 unsigned int* heightp );
00571 
00572   
00575   typedef bool (*stg_ray_test_func_t)(Model* candidate, 
00576                                                   Model* finder, 
00577                                                   const void* arg );
00578 
00579   // STL container iterator macros
00580 #define VAR(V,init) __typeof(init) V=(init)
00581 #define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();I++)
00582 
00584   template <class T, class C>
00585   void EraseAll( T thing, C& cont )
00586   { cont.erase( std::remove( cont.begin(), cont.end(), thing ), cont.end() ); }
00587   
00588   // Error macros - output goes to stderr
00589 #define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00590 #define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00591 #define PRINT_ERR2(m,a,b) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00592 #define PRINT_ERR3(m,a,b,c) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00593 #define PRINT_ERR4(m,a,b,c,d) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
00594 #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__)
00595 
00596   // Warning macros
00597 #define PRINT_WARN(m) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00598 #define PRINT_WARN1(m,a) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00599 #define PRINT_WARN2(m,a,b) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00600 #define PRINT_WARN3(m,a,b,c) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00601 #define PRINT_WARN4(m,a,b,c,d) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
00602 #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__)
00603 
00604   // Message macros
00605 #ifdef DEBUG
00606 #define PRINT_MSG(m) printf( "Stage: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00607 #define PRINT_MSG1(m,a) printf( "Stage: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00608 #define PRINT_MSG2(m,a,b) printf( "Stage: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00609 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00610 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
00611 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m" (%s %s)\n", a, b, c, d, e,__FILE__, __FUNCTION__)
00612 #else
00613 #define PRINT_MSG(m) printf( "Stage: "m"\n" )
00614 #define PRINT_MSG1(m,a) printf( "Stage: "m"\n", a)
00615 #define PRINT_MSG2(m,a,b) printf( "Stage: "m"\n,", a, b )
00616 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m"\n", a, b, c )
00617 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m"\n", a, b, c, d )
00618 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m"\n", a, b, c, d, e )
00619 #endif
00620 
00621   // DEBUG macros
00622 #ifdef DEBUG
00623 #define PRINT_DEBUG(m) printf( "debug: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00624 #define PRINT_DEBUG1(m,a) printf( "debug: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00625 #define PRINT_DEBUG2(m,a,b) printf( "debug: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00626 #define PRINT_DEBUG3(m,a,b,c) printf( "debug: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00627 #define PRINT_DEBUG4(m,a,b,c,d) printf( "debug: "m" (%s %s)\n", a, b, c ,d, __FILE__, __FUNCTION__)
00628 #define PRINT_DEBUG5(m,a,b,c,d,e) printf( "debug: "m" (%s %s)\n", a, b, c ,d, e, __FILE__, __FUNCTION__)
00629 #else
00630 #define PRINT_DEBUG(m)
00631 #define PRINT_DEBUG1(m,a)
00632 #define PRINT_DEBUG2(m,a,b)
00633 #define PRINT_DEBUG3(m,a,b,c)
00634 #define PRINT_DEBUG4(m,a,b,c,d)
00635 #define PRINT_DEBUG5(m,a,b,c,d,e)
00636 #endif
00637 
00638   class Block;
00639   class Model;
00640 
00643   typedef int (*stg_model_callback_t)( Model* mod, void* user );
00644   
00645   // ANCESTOR CLASS
00647   class Ancestor
00648   {
00649     friend class Canvas; // allow Canvas access to our private members
00650      
00651   protected:
00652      ModelPtrVec children;
00653     bool debug;
00654      std::string token;
00655      pthread_mutex_t access_mutex; 
00656 
00657      void Load( Worldfile* wf, int section );
00658      void Save( Worldfile* wf, int section );
00659      
00660   public:   
00661      /* The maximum length of a Stage model identifier string */
00662      //static const uint32_t TOKEN_MAX = 64;
00663          
00665      ModelPtrVec& GetChildren(){ return children;}
00666     
00668     void ForEachDescendant( stg_model_callback_t func, void* arg );
00669         
00671      std::map<std::string,unsigned int> child_type_counts;
00672      
00673     Ancestor();
00674     virtual ~Ancestor();
00675      
00676     virtual void AddChild( Model* mod );
00677     virtual void RemoveChild( Model* mod );
00678     virtual Pose GetGlobalPose();
00679      
00680     const char* Token()
00681     { return token.c_str(); }
00682      
00683     //const std::string& Token()
00684     //{ return token; }
00685      
00686     void SetToken( const std::string& str )
00687     { token = str; } 
00688 
00689      void Lock(){ pthread_mutex_lock( &access_mutex ); }    
00690      void Unlock(){ pthread_mutex_unlock( &access_mutex ); }
00691   };
00692 
00695   class RaytraceResult
00696   {
00697   public:
00698     Pose pose; 
00699     stg_meters_t range; 
00700     Model* mod; 
00701     Color color; 
00702      
00703      RaytraceResult() : pose(), range(0), mod(NULL), color() {}
00704      RaytraceResult( const Pose& pose, 
00705                           stg_meters_t range ) 
00706         : pose(pose), range(range), mod(NULL), color() {}    
00707   };
00708 
00709   typedef RaytraceResult stg_raytrace_result_t;
00710     
00711   class Ray
00712   {
00713   public:
00714      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 ) :
00715         mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest)
00716      {}
00717 
00718      Ray() : mod(NULL), origin(0,0,0,0), range(0), func(NULL), arg(NULL), ztest(true)
00719      {}
00720 
00721      const Model* mod;
00722      Pose origin;
00723      stg_meters_t range;
00724      stg_ray_test_func_t func;
00725      const void* arg;
00726      bool ztest;        
00727   };
00728         
00729 
00730   // defined in stage_internal.hh
00731   class Region;
00732   class SuperRegion;
00733   class BlockGroup;
00734   class PowerPack;
00735 
00736   class LogEntry
00737   {
00738      stg_usec_t timestamp;
00739      Model* mod;
00740      Pose pose;
00741      
00742   public:
00743      LogEntry( stg_usec_t timestamp, Model* mod );
00744      
00746      static std::vector<LogEntry> log;
00747      
00749      static size_t Count(){ return log.size(); }
00750      
00752      static void Clear(){ log.clear(); }
00753 
00755      static void Print();
00756   };
00757 
00758   class CtrlArgs
00759   {
00760   public:
00761      std::string worldfile;
00762      std::string cmdline;
00763 
00764      CtrlArgs( std::string w, std::string c ) : worldfile(w), cmdline(c) {}
00765   };
00766 
00768   class World : public Ancestor
00769   {
00770     friend class Block;
00771     friend class Model; // allow access to private members
00772     friend class ModelFiducial;
00773     friend class Canvas;
00774 
00775   public: 
00778      static std::vector<std::string> args;
00779      static std::string ctrlargs;
00780 
00781   private:
00782     
00783     static std::set<World*> world_set; 
00784     static bool quit_all; 
00785     static void UpdateCb( World* world);
00786     static unsigned int next_id; 
00787      
00788     bool destroy;
00789     bool dirty; 
00790 
00792      //std::map<unsigned int, Model*> models_by_id;
00793      
00795      std::set<Model*> models;
00796 
00798      std::map<std::string, Model*> models_by_name;      
00799 
00801      std::map<int,Model*> models_by_wfentity;
00802         
00805      ModelPtrVec models_with_fiducials;
00806      
00808      void FiducialInsert( Model* mod )
00809      { 
00810         FiducialErase( mod ); // make sure it's not there already
00811         models_with_fiducials.push_back( mod ); 
00812      }
00813      
00815      void FiducialErase( Model* mod )
00816      { 
00817         EraseAll( mod, models_with_fiducials );
00818      }
00819 
00820     double ppm; 
00821     bool quit; 
00822      
00823      bool show_clock; 
00824      unsigned int show_clock_interval; 
00825 
00826     pthread_mutex_t thread_mutex; 
00827      unsigned int threads_working; 
00828     pthread_cond_t threads_start_cond; 
00829     pthread_cond_t threads_done_cond; 
00830     int total_subs; 
00831      unsigned int worker_threads; 
00832      
00833   protected:     
00834 
00835      std::list<std::pair<stg_world_callback_t,void*> > cb_list; 
00836     stg_bounds3d_t extent; 
00837     bool graphics;
00838 
00839      std::set<Option*> option_table; 
00840      std::list<PowerPack*> powerpack_list; 
00841 
00842     stg_usec_t quit_time;
00843      std::list<float*> ray_list;
00844     stg_usec_t sim_time; 
00845      std::map<stg_point_int_t,SuperRegion*> superregions;
00846     SuperRegion* sr_cached; 
00847      
00848      std::vector<ModelPtrVec> update_lists;  
00849      
00850     uint64_t updates; 
00851     Worldfile* wf; 
00852 
00853      void CallUpdateCallbacks(); 
00854 
00855   public:
00856      
00857     bool paused; 
00858 
00859     virtual void Start(){ paused = false; };
00860     virtual void Stop(){ paused = true; };
00861     virtual void TogglePause(){ paused ? Start() : Stop(); };
00862 
00863      bool Paused(){ return( paused ); };
00864     
00865      PointIntVec rt_cells;
00866      PointIntVec rt_candidate_cells;
00867 
00868     static const int DEFAULT_PPM = 50;  // default resolution in pixels per meter
00869     //static const stg_msec_t DEFAULT_INTERVAL_SIM = 100;  ///< duration of sim timestep
00870 
00873      void AddUpdateCallback( stg_world_callback_t cb, void* user );
00874 
00877      int RemoveUpdateCallback( stg_world_callback_t cb, void* user );
00879      void Log( Model* mod );
00880 
00882     void NeedRedraw(){ dirty = true; };
00883     
00885     Model* ground;
00886     
00889     virtual std::string ClockString( void ) const;
00890         
00891      Model* CreateModel( Model* parent, const std::string& typestr );    
00892     void LoadModel( Worldfile* wf, int entity );
00893     void LoadBlock( Worldfile* wf, int entity );
00894     void LoadBlockGroup( Worldfile* wf, int entity );
00895     
00896     virtual Model* RecentlySelectedModel(){ return NULL; }
00897         
00898     SuperRegion* AddSuperRegion( const stg_point_int_t& coord );
00899     SuperRegion* GetSuperRegion( const stg_point_int_t& coord );
00900     SuperRegion* GetSuperRegionCached( const stg_point_int_t& coord);
00901     SuperRegion* GetSuperRegionCached( int32_t x, int32_t y );
00902     void ExpireSuperRegion( SuperRegion* sr );
00903         
00904     //inline Cell* GetCell( const stg_point_int_t& glob );
00905         
00908     void ForEachCellInLine( const stg_point_int_t& pt1,
00909                                                         const stg_point_int_t& pt2, 
00910                                                         CellPtrVec& cells );
00911         
00914     int32_t MetersToPixels( stg_meters_t x )
00915     { return (int32_t)floor(x * ppm); };
00916         
00917     stg_point_int_t MetersToPixels( const stg_point_t& pt )
00918     { return stg_point_int_t( MetersToPixels(pt.x), MetersToPixels(pt.y)); };
00919         
00920     // dummy implementations to be overloaded by GUI subclasses
00921     virtual void PushColor( Color col ) 
00922      { /* do nothing */  (void)col; };
00923     virtual void PushColor( double r, double g, double b, double a ) 
00924      { /* do nothing */ (void)r; (void)g; (void)b; (void)a; };
00925      
00926     virtual void PopColor(){ /* do nothing */  };
00927         
00928     SuperRegion* CreateSuperRegion( stg_point_int_t origin );
00929     void DestroySuperRegion( SuperRegion* sr );
00930         
00932      stg_raytrace_result_t Raytrace( const Ray& ray );
00933 
00934     stg_raytrace_result_t Raytrace( const Pose& pose,            
00935                                                 const stg_meters_t range,
00936                                                 const stg_ray_test_func_t func,
00937                                                 const Model* finder,
00938                                                 const void* arg,
00939                                                 const bool ztest );
00940         
00941     void Raytrace( const Pose &pose,             
00942                          const stg_meters_t range,
00943                          const stg_radians_t fov,
00944                          const stg_ray_test_func_t func,
00945                          const Model* finder,
00946                          const void* arg,
00947                          stg_raytrace_result_t* samples,
00948                          const uint32_t sample_count,
00949                          const bool ztest );
00950         
00951         
00953     void Extend( stg_point3_t pt );
00954   
00955     virtual void AddModel( Model* mod );
00956     virtual void RemoveModel( Model* mod );
00957 
00958     void AddModelName( Model* mod, const std::string& name );
00959         
00960     void AddPowerPack( PowerPack* pp );
00961     void RemovePowerPack( PowerPack* pp );
00962         
00963     void ClearRays();
00964   
00966     void RecordRay( double x1, double y1, double x2, double y2 );
00967         
00970     bool PastQuitTime();
00971                 
00972     static void* update_thread_entry( std::pair<World*,int>* info );
00973     
00974     class Event
00975     {
00976     public:
00977       
00978       Event( stg_usec_t time, Model* mod ) 
00979           : time(time), mod(mod) {}
00980       
00981       stg_usec_t time; // time that event occurs
00982       Model* mod; // model to update
00983       
00984       bool operator<( const Event& other ) const;
00985     };
00986      
00987      std::vector<std::priority_queue<Event> > event_queues;
00988      void Enqueue( unsigned int queue_num, stg_usec_t delay, Model* mod );
00989      
00990      std::set<Model*> active_energy;
00991      std::set<Model*> active_velocity;
00992 
00994      stg_usec_t sim_interval;
00995      
00997      void ConsumeQueue( unsigned int queue_num );
00998 
01001      unsigned int GetEventQueue( Model* mod );
01002 
01003   public:
01005     static bool UpdateAll(); 
01006      
01007     World( const std::string& name = "MyWorld", 
01008               double ppm = DEFAULT_PPM );
01009         
01010     virtual ~World();
01011      
01012     stg_usec_t SimTimeNow(void);
01013      
01014     Worldfile* GetWorldFile(){ return wf; };
01015      
01016     virtual bool IsGUI() const { return false; }
01017      
01018     virtual void Load( const char* worldfile_path );
01019     virtual void UnLoad();
01020     virtual void Reload();
01021     virtual bool Save( const char* filename );
01022     virtual bool Update(void);
01023      
01024     bool TestQuit(){ return( quit || quit_all );  }
01025     void Quit(){ quit = true; }
01026     void QuitAll(){ quit_all = true; }
01027     void CancelQuit(){ quit = false; }
01028     void CancelQuitAll(){ quit_all = false; }
01029      
01030      void TryCharge( PowerPack* pp, const Pose& pose );
01031 
01034     double Resolution(){ return ppm; };
01035    
01038     Model* GetModel( const std::string& name ) const;
01039   
01041     const stg_bounds3d_t& GetExtent(){ return extent; };
01042   
01044     uint64_t GetUpdateCount() { return updates; }
01045 
01047      void RegisterOption( Option* opt );    
01048      
01050      void ShowClock( bool enable ){ show_clock = enable; };
01051 
01053      Model* GetGround() {return ground;};
01054     
01055   };
01056 
01057   class Block
01058   {
01059     friend class BlockGroup;
01060     friend class Model;
01061     friend class SuperRegion;
01062     friend class World;
01063     friend class Canvas;
01064   public:
01065   
01069     Block( Model* mod,  
01070               stg_point_t* pts, 
01071               size_t pt_count,
01072               stg_meters_t zmin,
01073               stg_meters_t zmax,
01074               Color color,
01075               bool inherit_color );
01076   
01078     Block(  Model* mod,  Worldfile* wf, int entity);
01079      
01080     ~Block();
01081      
01083     void Map();      
01084      
01086     void UnMap();    
01087      
01089      void DrawSolid();
01090 
01092     void DrawFootPrint(); 
01093 
01095      void Translate( double x, double y );   
01096 
01098      double CenterX();
01099 
01101      double CenterY();
01102 
01104      void SetCenterX( double y );
01105 
01107      void SetCenterY( double y );
01108 
01110      void SetCenter( double x, double y);    
01111 
01113      void SetZ( double min, double max );
01114 
01115     stg_point_t* Points( unsigned int *count )
01116     { if( count ) *count = pt_count; return &pts[0]; };          
01117      
01118      std::vector<stg_point_t>& Points()
01119     { return pts; };             
01120          
01121     inline void RemoveFromCellArray( CellPtrVec* blocks );
01122     inline void GenerateCandidateCells();  
01123 
01124      void AppendTouchingModels( ModelPtrSet& touchers );
01125      
01127     Model* TestCollision(); 
01128     void SwitchToTestedCells();  
01129     void Load( Worldfile* wf, int entity );  
01130     Model* GetModel(){ return mod; };  
01131     const Color& GetColor();        
01132      void Rasterize( uint8_t* data, 
01133                           unsigned int width, unsigned int height,      
01134                           stg_meters_t cellwidth, stg_meters_t cellheight );
01135         
01136   private:
01137     Model* mod; 
01138      std::vector<stg_point_t> mpts; 
01139     size_t pt_count; 
01140      std::vector<stg_point_t> pts; 
01141     Size size;   
01142     Bounds local_z; 
01143     Color color;
01144     bool inherit_color;
01145      
01147      double glow;
01148      
01149     void DrawTop();
01150     void DrawSides();
01151      
01153     Bounds global_z;     
01154     bool mapped;
01155         
01157         std::vector< std::list<Block*>::iterator > list_entries;
01158 
01161      CellPtrVec * rendered_cells;
01162 
01169      CellPtrVec * candidate_cells;
01170     
01171      PointIntVec gpts;
01172     
01175      stg_point_t BlockPointToModelMeters( const stg_point_t& bpt );
01176     
01178      void InvalidateModelPointCache();
01179   };
01180 
01181 
01182   class BlockGroup
01183   {
01184     friend class Model;
01185      friend class Block;
01186 
01187   private:
01188     int displaylist;
01189 
01190     void BuildDisplayList( Model* mod );
01191      
01192      BlockPtrSet blocks;
01193     Size size;
01194     stg_point3_t offset;
01195     stg_meters_t minx, maxx, miny, maxy;
01196 
01197   public:
01198     BlockGroup();
01199     ~BlockGroup();
01200      
01201     uint32_t GetCount(){ return blocks.size(); };
01202     const Size& GetSize(){ return size; };
01203     const stg_point3_t& GetOffset(){ return offset; };
01204      
01207     void CalcSize();
01208      
01209     void AppendBlock( Block* block );
01210     void CallDisplayList( Model* mod );
01211     void Clear() ; 
01213      void AppendTouchingModels( ModelPtrSet& touchers );
01214      
01217     Model* TestCollision();
01218  
01219     void SwitchToTestedCells();
01220      
01221     void Map();
01222     void UnMap();
01223      
01225     void DrawSolid( const Geom &geom); 
01226 
01228      void DrawFootPrint( const Geom &geom);
01229 
01230     void LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfile *wf );
01231     void LoadBlock( Model* mod, Worldfile* wf, int entity );
01232      
01233      void Rasterize( uint8_t* data, 
01234                           unsigned int width, unsigned int height,
01235                           stg_meters_t cellwidth, stg_meters_t cellheight );
01236      
01237      void InvalidateModelPointCache()
01238      {
01239         FOR_EACH( it, blocks )
01240           (*it)->InvalidateModelPointCache();
01241      }
01242 
01243   };
01244 
01245   class Camera 
01246   {
01247   protected:
01248     float _pitch; //left-right (about y)
01249     float _yaw; //up-down (about x)
01250     float _x, _y, _z;
01251     
01252   public:
01253     Camera() : _pitch( 0 ), _yaw( 0 ), _x( 0 ), _y( 0 ), _z( 0 ) { }
01254     virtual ~Camera() { }
01255 
01256     virtual void Draw( void ) const = 0;
01257     virtual void SetProjection( void ) const = 0;
01258 
01259      float yaw( void ) const { return _yaw; }
01260      float pitch( void ) const { return _pitch; }
01261      
01262      float x( void ) const { return _x; }
01263      float y( void ) const { return _y; }
01264      float z( void ) const { return _z; }
01265      
01266     virtual void reset() = 0;
01267     virtual void Load( Worldfile* wf, int sec ) = 0;
01268 
01269     //TODO data should be passed in somehow else. (at least min/max stuff)
01270     //virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const = 0;
01271   };
01272 
01273   class PerspectiveCamera : public Camera
01274   {
01275   private:
01276     float _z_near;
01277     float _z_far;
01278     float _vert_fov;
01279     float _horiz_fov;
01280     float _aspect;
01281 
01282   public:
01283     PerspectiveCamera( void );
01284 
01285     virtual void Draw( void ) const;
01286     virtual void SetProjection( void ) const;
01287     //void SetProjection( float aspect ) const;
01288     void update( void );
01289 
01290     void strafe( float amount );
01291     void forward( float amount );
01292     
01293      void setPose( float x, float y, float z ) { _x = x; _y = y; _z = z; }
01294      void addPose( float x, float y, float z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; }
01295     void move( float x, float y, float z );
01296      void setFov( float horiz_fov, float vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; }
01298      void setAspect( float aspect ) { 
01299       //std::cout << "aspect: " << aspect << " vert: " << _vert_fov << " => " << aspect * _vert_fov << std::endl;
01300       //_vert_fov = aspect / _horiz_fov;
01301       _aspect = aspect;
01302     }
01303      void setYaw( float yaw ) { _yaw = yaw; }
01304      float horizFov( void ) const { return _horiz_fov; }
01305      float vertFov( void ) const { return _vert_fov; }
01306      void addYaw( float yaw ) { _yaw += yaw; }
01307      void setPitch( float pitch ) { _pitch = pitch; }
01308      void addPitch( float pitch ) { _pitch += pitch; if( _pitch < 0 ) _pitch = 0; else if( _pitch > 180 ) _pitch = 180; }
01309     
01310      float realDistance( float z_buf_val ) const {
01311       //formula found at http://www.cs.unc.edu/~hoff/techrep/openglz.html
01312       //Z = Zn*Zf / (Zf - z*(Zf-Zn))
01313       return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) );
01314     }
01315      void scroll( float dy ) { _z += dy; }
01316      float nearClip( void ) const { return _z_near; }
01317      float farClip( void ) const { return _z_far; }
01318      void setClip( float near, float far ) { _z_far = far; _z_near = near; }
01319     
01320      void reset() { setPitch( 70 ); setYaw( 0 ); }
01321     
01322     void Load( Worldfile* wf, int sec );
01323     void Save( Worldfile* wf, int sec );
01324   };
01325   
01326   class OrthoCamera : public Camera
01327   {
01328   private:
01329     float _scale;
01330     float _pixels_width;
01331     float _pixels_height;
01332     float _y_min;
01333     float _y_max;
01334   
01335   public:
01336     OrthoCamera( void ) : _scale( 15 ) { }
01337     virtual void Draw() const;
01338     virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max );
01339     virtual void SetProjection( void ) const;
01340   
01341     void move( float x, float y );
01342      void setYaw( float yaw ) { _yaw = yaw; }
01343      void setPitch( float pitch ) { _pitch = pitch; }
01344      void addYaw( float yaw ) { _yaw += yaw;    }
01345      void addPitch( float pitch ) {
01346       _pitch += pitch;
01347       if( _pitch > 90 )
01348           _pitch = 90;
01349       else if( _pitch < 0 )
01350           _pitch = 0;
01351     }
01352   
01353      void setScale( float scale ) { _scale = scale; }
01354      void setPose( float x, float y) { _x = x; _y = y; }
01355   
01356     void scale( float scale, float shift_x = 0, float h = 0, float shift_y = 0, float w = 0 );  
01357      void reset( void ) { _pitch = _yaw = 0; }
01358   
01359      float scale() const { return _scale; }
01360   
01361     void Load( Worldfile* wf, int sec );
01362     void Save( Worldfile* wf, int sec );
01363   };
01364 
01365 
01369   class WorldGui : public World, public Fl_Window 
01370   {
01371     friend class Canvas;
01372     friend class ModelCamera;
01373     friend class Model;
01374     friend class Option;
01375 
01376   private:
01377 
01378     Canvas* canvas;
01379     std::vector<Option*> drawOptions;
01380     FileManager* fileMan; 
01381      std::vector<stg_usec_t> interval_log;
01382      
01385      float speedup; 
01386 
01387     Fl_Menu_Bar* mbar;
01388     OptionsDlg* oDlg;
01389     bool pause_time;
01390 
01393     stg_usec_t real_time_interval;
01394      
01396     stg_usec_t real_time_now; 
01397 
01400     stg_usec_t real_time_recorded;
01401      
01403      uint64_t timing_interval;
01404 
01405     // static callback functions
01406     static void windowCb( Fl_Widget* w, WorldGui* wg ); 
01407     static void fileLoadCb( Fl_Widget* w, WorldGui* wg );
01408     static void fileSaveCb( Fl_Widget* w, WorldGui* wg );
01409     static void fileSaveAsCb( Fl_Widget* w, WorldGui* wg );
01410     static void fileExitCb( Fl_Widget* w, WorldGui* wg );
01411     static void viewOptionsCb( OptionsDlg* oDlg, WorldGui* wg );
01412     static void optionsDlgCb( OptionsDlg* oDlg, WorldGui* wg );
01413     static void helpAboutCb( Fl_Widget* w, WorldGui* wg );
01414     static void pauseCb( Fl_Widget* w, WorldGui* wg );
01415     static void onceCb( Fl_Widget* w, WorldGui* wg );
01416     static void fasterCb( Fl_Widget* w, WorldGui* wg );
01417     static void slowerCb( Fl_Widget* w, WorldGui* wg );
01418     static void realtimeCb( Fl_Widget* w, WorldGui* wg );
01419     static void fasttimeCb( Fl_Widget* w, WorldGui* wg );
01420     static void resetViewCb( Fl_Widget* w, WorldGui* wg );
01421     static void moreHelptCb( Fl_Widget* w, WorldGui* wg );
01422     
01423     // GUI functions
01424     bool saveAsDialog();
01425     bool closeWindowQuery();
01426     
01427     virtual void AddModel( Model* mod );
01428     
01429      void SetTimeouts();
01430 
01431   protected:
01432     
01433     virtual void PushColor( Color col );
01434     virtual void PushColor( double r, double g, double b, double a );
01435     virtual void PopColor();
01436     
01437     void DrawOccupancy();
01438     void DrawVoxels();
01439      
01440   public:
01441     
01442     WorldGui(int W,int H,const char*L=0);
01443     ~WorldGui();
01444     
01445     virtual std::string ClockString() const;
01446     virtual bool Update();  
01447     virtual void Load( const char* filename );
01448     virtual void UnLoad();
01449     virtual bool Save( const char* filename );  
01450     virtual bool IsGUI() const { return true; };    
01451     virtual Model* RecentlySelectedModel() const;
01452 
01453     virtual void Start();
01454     virtual void Stop();
01455     
01456     stg_usec_t RealTimeNow(void) const;
01457  
01458     void DrawBoundingBoxTree();
01459     
01460     Canvas* GetCanvas( void ) const { return canvas; } 
01461 
01463     void Show(); 
01464 
01466     std::string EnergyString( void );   
01467     virtual void RemoveChild( Model* mod );  
01468   };
01469 
01470 
01471   class StripPlotVis : public Visualizer
01472   {
01473   private:
01474      
01475      Model* mod;
01476      float* data;
01477      size_t len;
01478      size_t count;
01479      unsigned int index;
01480      float x,y,w,h,min,max;
01481      Color fgcolor, bgcolor;
01482      
01483   public:
01484      StripPlotVis( float x, float y, float w, float h, 
01485                         size_t len, 
01486                         Color fgcolor, Color bgcolor,
01487                         const char* name, const char* wfname );
01488      virtual ~StripPlotVis();
01489      virtual void Visualize( Model* mod, Camera* cam );     
01490      void AppendValue( float value );
01491   };
01492 
01493 
01494   class PowerPack
01495   {
01496      friend class WorldGui;
01497      friend class Canvas;
01498      
01499   protected:
01500      
01501      class DissipationVis : public Visualizer
01502      {
01503      private:
01504         unsigned int columns, rows;
01505         stg_meters_t width, height;
01506          
01507          std::vector<stg_joules_t> cells;
01508          
01509          stg_joules_t peak_value;
01510          double cellsize;
01511          
01512         static stg_joules_t global_peak_value; 
01513 
01514      public:
01515         DissipationVis( stg_meters_t width, 
01516                              stg_meters_t height, 
01517                              stg_meters_t cellsize );
01518 
01519         virtual ~DissipationVis();
01520         virtual void Visualize( Model* mod, Camera* cam );      
01521         
01522         void Accumulate( stg_meters_t x, stg_meters_t y, stg_joules_t amount );
01523      } event_vis;
01524      
01525 
01526      StripPlotVis output_vis;
01527      StripPlotVis stored_vis;
01528 
01530      Model* mod;
01531     
01533      stg_joules_t stored;
01534      
01536      stg_joules_t capacity;
01537      
01539      bool charging;
01540      
01542      stg_joules_t dissipated;
01543      
01544      // these are used to visualize the power draw
01545      stg_usec_t last_time;
01546      stg_joules_t last_joules;
01547      stg_watts_t last_watts;
01548 
01549      static stg_joules_t global_stored;
01550      static stg_joules_t global_capacity;
01551      static stg_joules_t global_dissipated;  
01552      static stg_joules_t global_input;
01553 
01554   public:
01555      PowerPack( Model* mod );
01556      ~PowerPack();
01557      
01559      void Visualize( Camera* cam );
01560 
01563      void Print( char* prefix ) const;
01564      
01566      stg_joules_t RemainingCapacity() const;
01567      
01569      void Add( stg_joules_t j );
01570         
01572      void Subtract( stg_joules_t j );
01573         
01575      void TransferTo( PowerPack* dest, stg_joules_t amount );    
01576 
01577      double ProportionRemaining() const
01578      { return( stored / capacity ); }
01579 
01580      void Print( const char* prefix ) const
01581      { printf( "%s PowerPack %.2f/%.2f J\n", prefix, stored, capacity ); }      
01582      
01583      stg_joules_t GetStored() const;
01584      stg_joules_t GetCapacity() const;
01585      stg_joules_t GetDissipated() const;
01586      void SetCapacity( stg_joules_t j );
01587      void SetStored( stg_joules_t j );  
01588 
01590      bool GetCharging() const { return charging; }
01591      
01592      void ChargeStart(){ charging = true; }
01593      void ChargeStop(){ charging = false; }
01594 
01596      void Dissipate( stg_joules_t j );
01597      
01599      void Dissipate( stg_joules_t j, const Pose& p );
01600   };
01601 
01602    
01604   class Model : public Ancestor
01605   {
01606     friend class Ancestor;
01607     friend class World;
01608     friend class World::Event;
01609     friend class WorldGui;
01610     friend class Canvas;
01611     friend class Block;
01612     friend class Region;
01613     friend class BlockGroup;
01614     friend class PowerPack;
01615     friend class Ray;
01616     
01617   private:
01619      static uint32_t count;
01620      static std::map<stg_id_t,Model*> modelsbyid;
01621      std::vector<Option*> drawOptions;
01622      const std::vector<Option*>& getOptions() const { return drawOptions; }
01623      
01624   protected:
01625      pthread_mutex_t access_mutex;
01626 
01629      bool alwayson;
01630 
01631      BlockGroup blockgroup;
01633      int blocks_dl;
01634 
01638      int boundary;
01639         
01642   public:
01643      class stg_cb_t
01644      {
01645      public:
01646         stg_model_callback_t callback;
01647         void* arg;
01648             
01649         stg_cb_t( stg_model_callback_t cb, void* arg ) 
01650           : callback(cb), arg(arg) {}
01651             
01652         stg_cb_t( stg_world_callback_t cb, void* arg ) 
01653           : callback(NULL), arg(arg) { (void)cb; }
01654             
01655         stg_cb_t() : callback(NULL), arg(NULL) {}
01656             
01658         bool operator<( const stg_cb_t& other ) const
01659         { return ((void*)(callback)) < ((void*)(other.callback)); }
01660             
01662         bool operator==( const stg_cb_t& other ) const
01663         { return( callback == other.callback);  }           
01664      };
01665         
01666      class Flag
01667      {
01668      public:
01669         Color color;
01670         double size;
01671         static int displaylist;
01672         
01673         Flag( Color color, double size );
01674         Flag* Nibble( double portion );
01675         
01678         void Draw(  GLUquadric* quadric );
01679      };
01680 
01681   protected:
01685      static std::map<void*, std::set<stg_cb_t> > callbacks;
01686         
01688      Color color;
01689      double friction;
01690         
01694      bool data_fresh;
01695      stg_bool_t disabled; 
01696      std::list<Visualizer*> cv_list;
01697      std::list<Flag*> flag_list;
01698      Geom geom;
01699 
01701      class GuiState
01702      {
01703      public:
01704         bool grid;
01705         bool move;
01706         bool nose;
01707         bool outline;
01708         
01709         GuiState();
01710         void Load( Worldfile* wf, int wf_entity );
01711      } gui;
01712      
01713      bool has_default_block;
01714   
01715      /* Hooks for attaching special callback functions (not used as
01716          variables - we just need unique addresses for them.) */  
01717      class CallbackHooks
01718      {
01719      public:
01720         int flag_incr;
01721         int flag_decr;
01722         int init;
01723         int load;
01724         int save;
01725         int shutdown;
01726         int startup;
01727         int update;
01728         int update_done;
01729 
01730          /* optimization: record the number of attached callbacks for pose
01731                 and velocity, so we can cheaply determine whether we need to
01732                 call a callback for SetPose() and SetVelocity(), which happen
01733                 very frequently. */
01734          int attached_velocity;
01735          int attached_pose;
01736          int attached_update;
01737          
01738          CallbackHooks() : 
01739              attached_velocity(0), 
01740              attached_pose(0), 
01741              attached_update(0) 
01742          {}
01743 
01744      } hooks;
01745   
01747      uint32_t id;   
01748      stg_usec_t interval; 
01749      stg_usec_t interval_energy; 
01750      stg_usec_t interval_pose; 
01751 
01752      stg_usec_t last_update; 
01753      bool log_state; 
01754      stg_meters_t map_resolution;
01755      stg_kg_t mass;
01756 
01758      Model* parent; 
01759 
01762      Pose pose;
01763 
01765      PowerPack* power_pack;
01766 
01769      std::list<PowerPack*> pps_charging;
01770         
01774      std::map<std::string,const void*> props;
01775 
01777      class RasterVis : public Visualizer
01778      {
01779      private:
01780         uint8_t* data;
01781         unsigned int width, height;
01782         stg_meters_t cellwidth, cellheight;
01783         std::vector<stg_point_t> pts;
01784       
01785      public:
01786         RasterVis();
01787         virtual ~RasterVis( void ){}
01788         virtual void Visualize( Model* mod, Camera* cam );
01789       
01790         void SetData( uint8_t* data, 
01791                           unsigned int width, 
01792                           unsigned int height,
01793                           stg_meters_t cellwidth,
01794                           stg_meters_t cellheight );
01795       
01796         int subs;     //< the number of subscriptions to this model
01797         int used;     //< the number of connections to this model
01798       
01799         void AddPoint( stg_meters_t x, stg_meters_t y );
01800         void ClearPts();
01801       
01802      } rastervis;
01803      
01804      bool rebuild_displaylist; 
01805      std::string say_string;   
01806         
01807      stg_bool_t stall;
01808      int subs;    
01809 
01811      bool thread_safe;
01812      
01814      class TrailItem 
01815      {                                                                                          
01816      public:
01817         stg_usec_t time;
01818         Pose pose;
01819         Color color;
01820         
01821         TrailItem( stg_usec_t time, Pose pose, Color color ) 
01822           : time(time), pose(pose), color(color){}
01823      };
01824     
01825      std::list<TrailItem> trail;
01826      
01830      static unsigned int trail_length;
01831      
01833      static uint64_t trail_interval;
01834      
01836         void UpdateTrail();
01837 
01838      //stg_model_type_t type;  
01839      const std::string type;
01842         unsigned int event_queue_num; 
01843         bool used;   
01844         Velocity velocity;
01845         
01849         bool velocity_enable;
01850         
01851         stg_watts_t watts;
01852      
01855      stg_watts_t watts_give;
01856     
01859      stg_watts_t watts_take;
01860     
01861      Worldfile* wf;
01862      int wf_entity;
01863      World* world; // pointer to the world in which this model exists
01864      WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode
01865 
01866   public:
01867      
01868      const std::string& GetModelType() const {return type;}  
01869      std::string GetSayString(){return std::string(say_string);}
01870      
01873     Model* GetChild( const std::string& name ) const;
01874 
01875      class Visibility
01876      {
01877      public:
01878         bool blob_return;
01879         int fiducial_key;
01880         int fiducial_return;
01881         bool gripper_return;
01882         stg_laser_return_t laser_return;
01883         bool obstacle_return;
01884         bool ranger_return;
01885         bool gravity_return;
01886         bool sticky_return;
01887         
01888         Visibility();
01889         void Load( Worldfile* wf, int wf_entity );
01890      } vis;
01891      
01892      stg_usec_t GetUpdateInterval(){ return interval; }
01893      stg_usec_t GetEnergyInterval(){ return interval_energy; }
01894      stg_usec_t GetPoseInterval(){ return interval_pose; }
01895      
01898      void Rasterize( uint8_t* data, 
01899                           unsigned int width, unsigned int height,
01900                           stg_meters_t cellwidth, stg_meters_t cellheight );
01901     
01902   private: 
01905      explicit Model(const Model& original);
01906 
01909      Model& operator=(const Model& original);
01910 
01911   protected:
01912 
01914      void RegisterOption( Option* opt );
01915 
01916      void AppendTouchingModels( ModelPtrSet& touchers );
01917 
01921      Model* TestCollision();
01922 
01925     Model* TestCollisionTree();
01926   
01927      void CommitTestedPose();
01928 
01929      void Map();
01930      void UnMap();
01931 
01932      void MapWithChildren();
01933      void UnMapWithChildren();
01934   
01935      // Find the root model, and map/unmap the whole tree.
01936      void MapFromRoot();
01937      void UnMapFromRoot();
01938 
01941      stg_raytrace_result_t Raytrace( const Pose &pose,
01942                                                 const stg_meters_t range, 
01943                                                 const stg_ray_test_func_t func,
01944                                                 const void* arg,
01945                                                 const bool ztest = true );
01946   
01949      void Raytrace( const Pose &pose,
01950                          const stg_meters_t range, 
01951                          const stg_radians_t fov, 
01952                          const stg_ray_test_func_t func,
01953                          const void* arg,
01954                          stg_raytrace_result_t* samples,
01955                          const uint32_t sample_count,
01956                          const bool ztest = true  );
01957   
01958      stg_raytrace_result_t Raytrace( const stg_radians_t bearing,            
01959                                                 const stg_meters_t range,
01960                                                 const stg_ray_test_func_t func,
01961                                                 const void* arg,
01962                                                 const bool ztest = true );
01963   
01964      void Raytrace( const stg_radians_t bearing,             
01965                          const stg_meters_t range,
01966                          const stg_radians_t fov,
01967                          const stg_ray_test_func_t func,
01968                          const void* arg,
01969                          stg_raytrace_result_t* samples,
01970                          const uint32_t sample_count,
01971                          const bool ztest = true );
01972   
01973 
01977      void GPoseDirtyTree();
01978 
01979      virtual void Startup();
01980      virtual void Shutdown();
01981      virtual void Update();
01982      virtual void UpdatePose();
01983      virtual void UpdateCharge();
01984 
01985      Model* ConditionalMove( const Pose& newpose );
01986 
01987      stg_meters_t ModelHeight() const;
01988 
01989      void DrawBlocksTree();
01990      virtual void DrawBlocks();
01991      void DrawBoundingBox();
01992      void DrawBoundingBoxTree();
01993      virtual void DrawStatus( Camera* cam );
01994      void DrawStatusTree( Camera* cam );
01995   
01996      void DrawOriginTree();
01997      void DrawOrigin();
01998   
01999      void PushLocalCoords();
02000      void PopCoords();
02001   
02003      void DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width=1.0, double height=1.0 );
02004     
02005      virtual void DrawPicker();
02006      virtual void DataVisualize( Camera* cam );  
02007      virtual void DrawSelected(void);
02008     
02009      void DrawTrailFootprint();
02010      void DrawTrailBlocks();
02011      void DrawTrailArrows();
02012      void DrawGrid();
02013      // void DrawBlinkenlights();
02014      void DataVisualizeTree( Camera* cam );
02015      void DrawFlagList();
02016      void DrawPose( Pose pose );
02017      void LoadDataBaseEntries( Worldfile* wf, int entity );
02018     
02019   public:
02020      virtual void PushColor( Color col ){ world->PushColor( col ); }    
02021      virtual void PushColor( double r, double g, double b, double a ){ world->PushColor( r,g,b,a ); }   
02022      virtual void PopColor()    { world->PopColor(); }
02023     
02024      PowerPack* FindPowerPack() const;
02025     
02026      //void RecordRenderPoint( GSList** head, GSList* link, 
02027      //                 unsigned int* c1, unsigned int* c2 );
02028 
02029      void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, 
02030                                     stg_meters_t ymin, stg_meters_t ymax );
02031     
02033      std::string PoseString()
02034      { return pose.String(); }
02035     
02037      static Model* LookupId( uint32_t id )
02038      { return modelsbyid[id]; }
02039      
02041      Model( World* world, 
02042               Model* parent = NULL, 
02043               const std::string& type = "model" );
02044      
02046      virtual ~Model();
02047     
02048      void Say( const std::string& str );
02049      
02051      void AddVisualizer( Visualizer* custom_visual, bool on_by_default );
02052 
02054      void RemoveVisualizer( Visualizer* custom_visual );
02055 
02056      void BecomeParentOf( Model* child );
02057 
02058      void Load( Worldfile* wf, int wf_entity )
02059      {
02062         SetWorldfile( wf, wf_entity );
02063         Load(); // call virtual load
02064      }
02065     
02067      void SetWorldfile( Worldfile* wf, int wf_entity )
02068      { this->wf = wf; this->wf_entity = wf_entity; }
02069     
02071      virtual void Load();
02072     
02074      virtual void Save();
02075     
02076      // Should be called after all models are loaded, to do any last-minute setup */
02077      //void Init(); 
02078      //void InitRecursive();
02079 
02081      void InitControllers();
02082 
02083      void AddFlag(  Flag* flag );
02084      void RemoveFlag( Flag* flag );
02085     
02086      void PushFlag( Flag* flag );
02087      Flag* PopFlag();
02088     
02089      int GetFlagCount() const { return flag_list.size(); }
02090   
02091      //      /** Add a pointer to a blinkenlight to the model. */
02092      //      void AddBlinkenlight( stg_blinkenlight_t* b )
02093      //      { g_ptr_array_add( this->blinkenlights, b ); }
02094   
02095      //      /** Clear all blinkenlights from the model. Does not destroy the
02096      //           blinkenlight objects. */
02097      //      void ClearBlinkenlights()
02098      //      {  g_ptr_array_set_size( this->blinkenlights, 0 ); }
02099   
02104      void Disable(){ disabled = true; };
02105 
02108      void Enable(){ disabled = false; };
02109   
02112      void LoadControllerModule( const char* lib );
02113     
02116      void NeedRedraw();
02117     
02120      void LoadBlock( Worldfile* wf, int entity );
02121 
02124      Block* AddBlockRect( stg_meters_t x, stg_meters_t y, 
02125                                  stg_meters_t dx, stg_meters_t dy, 
02126                                  stg_meters_t dz );
02127     
02129      void ClearBlocks();
02130   
02133      Model* Parent() const { return this->parent; }
02134 
02136      World* GetWorld() const { return this->world; }
02137   
02139      Model* Root(){ return(  parent ? parent->Root() : this ); }
02140   
02141      bool IsAntecedent( const Model* testmod ) const;
02142     
02144      bool IsDescendent( const Model* testmod ) const;
02145     
02147      bool IsRelated( const Model* testmod ) const;
02148 
02150      Pose GetGlobalPose() const;
02151     
02153      Velocity GetGlobalVelocity()  const;
02154     
02155      /* set the velocity of a model in the global coordinate system */
02156      void SetGlobalVelocity( const Velocity& gvel );
02157     
02159      void Subscribe();
02160     
02162      void Unsubscribe();
02163     
02165      void SetGlobalPose(  const Pose& gpose );
02166     
02168      void SetVelocity(  const Velocity& vel );
02169     
02171         void VelocityEnable();
02172 
02174         void VelocityDisable();
02175 
02177      void SetPose(  const Pose& pose );
02178     
02180      void AddToPose(  const Pose& pose );
02181     
02183      void AddToPose(  double dx, double dy, double dz, double da );
02184     
02186      void SetGeom(  const Geom& src );
02187   
02190      void SetFiducialReturn(  int fid );
02191   
02193      int GetFiducialReturn()  const { return vis.fiducial_return; }
02194   
02197      void SetFiducialKey(  int key );
02198     
02199      Color GetColor() const { return color; }
02200      
02202      uint32_t GetId()  const { return id; }
02203      
02205      stg_kg_t GetTotalMass();
02206      
02208      stg_kg_t GetMassOfChildren();
02209 
02211      int SetParent( Model* newparent);
02212     
02215      Geom GetGeom() const { return geom; }
02216     
02219      Pose GetPose() const { return pose; }
02220     
02223      Velocity GetVelocity() const { return velocity; }
02224     
02225      // guess what these do?
02226      void SetColor( Color col );
02227      void SetMass( stg_kg_t mass );
02228      void SetStall( stg_bool_t stall );
02229      void SetGravityReturn( int val );
02230      void SetGripperReturn( int val );
02231      void SetStickyReturn( int val );
02232      void SetLaserReturn( stg_laser_return_t val );
02233      void SetObstacleReturn( int val );
02234      void SetBlobReturn( int val );
02235      void SetRangerReturn( int val );
02236      void SetBoundary( int val );
02237      void SetGuiNose( int val );
02238      void SetGuiMove( int val );
02239      void SetGuiGrid( int val );
02240      void SetGuiOutline( int val );
02241      void SetWatts( stg_watts_t watts );
02242      void SetMapResolution( stg_meters_t res );
02243      void SetFriction( double friction );
02244     
02245      bool DataIsFresh() const { return this->data_fresh; }
02246     
02247      /* attach callback functions to data members. The function gets
02248          called when the member is changed using SetX() accessor method */
02249         
02250      void AddCallback( void* address, 
02251                              stg_model_callback_t cb, 
02252                              void* user );
02253         
02254      int RemoveCallback( void* member,
02255                                 stg_model_callback_t callback );
02256         
02257      int CallCallbacks(  void* address );
02258         
02259      /* wrappers for the generic callback add & remove functions that hide
02260          some implementation detail */
02261     
02262      void AddStartupCallback( stg_model_callback_t cb, void* user )
02263      { AddCallback( &hooks.startup, cb, user ); };
02264     
02265      void RemoveStartupCallback( stg_model_callback_t cb )
02266      { RemoveCallback( &hooks.startup, cb ); };
02267     
02268      void AddShutdownCallback( stg_model_callback_t cb, void* user )
02269      { AddCallback( &hooks.shutdown, cb, user ); };
02270     
02271      void RemoveShutdownCallback( stg_model_callback_t cb )
02272      { RemoveCallback( &hooks.shutdown, cb ); }
02273     
02274      void AddLoadCallback( stg_model_callback_t cb, void* user )
02275      { AddCallback( &hooks.load, cb, user ); }
02276     
02277      void RemoveLoadCallback( stg_model_callback_t cb )
02278      { RemoveCallback( &hooks.load, cb ); }
02279     
02280      void AddSaveCallback( stg_model_callback_t cb, void* user )
02281      { AddCallback( &hooks.save, cb, user ); }
02282     
02283      void RemoveSaveCallback( stg_model_callback_t cb )
02284      { RemoveCallback( &hooks.save, cb ); }
02285   
02286      void AddUpdateCallback( stg_model_callback_t cb, void* user )
02287      { AddCallback( &hooks.update, cb, user ); }
02288      
02289      void RemoveUpdateCallback( stg_model_callback_t cb )
02290      {  RemoveCallback( &hooks.update, cb ); }
02291      
02292      void AddFlagIncrCallback( stg_model_callback_t cb, void* user )
02293      {  AddCallback( &hooks.flag_incr, cb, user ); }
02294     
02295      void RemoveFlagIncrCallback( stg_model_callback_t cb )
02296      {  RemoveCallback( &hooks.flag_incr, cb ); }
02297 
02298      void AddFlagDecrCallback( stg_model_callback_t cb, void* user )
02299      {  AddCallback( &hooks.flag_decr, cb, user ); }
02300     
02301      void RemoveFlagDecrCallback( stg_model_callback_t cb )
02302      {  RemoveCallback( &hooks.flag_decr, cb ); }
02303      
02306      const void* GetProperty( const char* key ) const;
02307      bool GetPropertyFloat( const char* key, float* f, float defaultval ) const;     
02308      bool GetPropertyInt( const char* key, int* i, int defaultval ) const;   
02309      bool GetPropertyStr( const char* key, char** c, char* defaultval ) const;
02310 
02341      int SetProperty( const char* key, const void* data );
02342      void SetPropertyInt( const char* key, int i );
02343      void SetPropertyFloat( const char* key, float f );
02344      void SetPropertyStr( const char* key, const char* str );
02345      
02346      void UnsetProperty( const char* key );
02347         
02348      virtual void Print( char* prefix ) const;
02349      virtual const char* PrintWithPose() const;
02350     
02353      Pose GlobalToLocal( const Pose& pose ) const;
02354      
02357      Pose LocalToGlobal( const Pose& pose ) const
02358      {  
02359         return( ( GetGlobalPose() + geom.pose ) + pose );
02360      }
02361         
02363         void LocalToPixels( const std::vector<stg_point_t>& local,
02364                                                 std::vector<stg_point_int_t>& pixels) const;
02365         
02368      stg_point_t LocalToGlobal( const stg_point_t& pt) const;       
02369 
02372      Model* GetUnsubscribedModelOfType( const std::string& type ) const;
02373     
02376      Model* GetUnusedModelOfType( const std::string& type );
02377   
02380      bool Stalled() const { return this->stall; }
02381      
02384      unsigned int GetSubscriptionCount() const { return subs; }
02385 
02387      bool HasSubscribers() const { return( subs > 0 ); }     
02388 
02389      static std::map< std::string, creator_t> name_map;  
02390   };
02391 
02392 
02393   // BLOBFINDER MODEL --------------------------------------------------------
02394 
02395 
02397   class ModelBlobfinder : public Model
02398   {
02399   public:
02401      class Blob
02402      {
02403      public:
02404         Color color;
02405         uint32_t left, top, right, bottom;
02406         stg_meters_t range;
02407      };
02408 
02409      class Vis : public Visualizer 
02410      {
02411      private:
02412         //static Option showArea;
02413      public:
02414         Vis( World* world );
02415         virtual ~Vis( void ){}
02416         virtual void Visualize( Model* mod, Camera* cam );
02417      } vis;
02418 
02419   private:
02420      std::vector<Blob> blobs;
02421      std::vector<Color> colors;
02422 
02423      // predicate for ray tracing
02424      static bool BlockMatcher( Block* testblock, Model* finder );
02425      //static Option showBlobData;
02426 
02427   public:
02428      stg_radians_t fov;
02429      stg_radians_t pan;
02430      stg_meters_t range;
02431      unsigned int scan_height;
02432      unsigned int scan_width;
02433      
02434      // constructor
02435      ModelBlobfinder( World* world,
02436                             Model* parent,
02437                             const std::string& type );
02438      // destructor
02439      ~ModelBlobfinder();
02440     
02441      virtual void Startup();
02442      virtual void Shutdown();
02443      virtual void Update();
02444      virtual void Load();
02445         
02446      Blob* GetBlobs( unsigned int* count )
02447      { 
02448         if( count ) *count = blobs.size();
02449         return &blobs[0];
02450      }
02451 
02453      void AddColor( Color col );
02454 
02456      void RemoveColor( Color col );
02457 
02460      void RemoveAllColors();
02461   };
02462 
02463 
02464 
02465 
02466   // LASER MODEL --------------------------------------------------------
02467   
02469   class ModelLaser : public Model
02470   {
02471   public:
02473      class Sample
02474      {
02475      public:
02476         stg_meters_t range; 
02477         double reflectance; 
02478      };
02479         
02481      class Config
02482      {
02483      public:
02484         uint32_t sample_count; 
02485         uint32_t resolution; 
02486         Bounds range_bounds; 
02487         stg_radians_t fov; 
02488         stg_usec_t interval; 
02489      };
02490         
02491   private:   
02492      class Vis : public Visualizer 
02493      {
02494      private:
02495         static Option showArea;
02496         static Option showStrikes;
02497         static Option showFov;
02498         static Option showBeams;
02499 
02500      public:
02501         Vis( World* world );        virtual ~Vis( void ){}
02502         virtual void Visualize( Model* mod, Camera* cam );
02503      } vis;
02504         
02505      unsigned int sample_count;
02506      std::vector<Sample> samples;
02507 
02508      stg_meters_t range_max;
02509      stg_radians_t fov;
02510      uint32_t resolution;
02511     
02512      // set up data buffers after the config changes
02513      void SampleConfig();
02514 
02515   public:
02516      // constructor
02517      ModelLaser( World* world,
02518                      Model* parent,
02519                      const std::string& type ); 
02520   
02521      // destructor
02522      ~ModelLaser();
02523     
02524      virtual void Startup();
02525      virtual void Shutdown();
02526      virtual void Update();
02527      virtual void Load();
02528      virtual void Print( char* prefix );
02529   
02531      Sample* GetSamples( uint32_t* count );
02532      
02534      const std::vector<Sample>& GetSamples();
02535      
02537      Config GetConfig( );
02538      
02540      void SetConfig( Config& cfg );  
02541   };
02542   
02543 
02544   // Light indicator model
02545   class ModelLightIndicator : public Model
02546   {
02547   public:
02548      ModelLightIndicator( World* world, 
02549                                  Model* parent,
02550                                  const std::string& type );
02551      ~ModelLightIndicator();
02552   
02553      void SetState(bool isOn);
02554 
02555   protected:
02556      virtual void DrawBlocks();
02557 
02558   private:
02559      bool m_IsOn;
02560   };
02561 
02562   // \todo  GRIPPER MODEL --------------------------------------------------------
02563 
02564 
02565   class ModelGripper : public Model
02566   {
02567   public:
02568 
02569      enum paddle_state_t {
02570         PADDLE_OPEN = 0, // default state
02571         PADDLE_CLOSED, 
02572         PADDLE_OPENING,
02573         PADDLE_CLOSING,
02574      };
02575      
02576      enum lift_state_t {
02577         LIFT_DOWN = 0, // default state
02578         LIFT_UP, 
02579         LIFT_UPPING, // verbed these to match the paddle state
02580         LIFT_DOWNING, 
02581      };
02582      
02583      enum cmd_t {
02584         CMD_NOOP = 0, // default state
02585         CMD_OPEN, 
02586         CMD_CLOSE,
02587         CMD_UP, 
02588         CMD_DOWN    
02589      };
02590      
02591      
02594      struct config_t
02595      {
02596         Size paddle_size; 
02597         paddle_state_t paddles;
02598         lift_state_t lift;      
02599         double paddle_position; 
02600         double lift_position; 
02601         Model* gripped;
02602         bool paddles_stalled; // true iff some solid object stopped the paddles closing or opening
02603         double close_limit; 
02604         bool autosnatch; 
02605         double break_beam_inset[2]; 
02606       Model* beam[2]; 
02607       Model* contact[2]; 
02608      };
02609      
02610   private:
02611      virtual void Update();
02612      virtual void DataVisualize( Camera* cam );
02613      
02614      void FixBlocks();
02615      void PositionPaddles();
02616      void UpdateBreakBeams();
02617      void UpdateContacts();
02618 
02619      config_t cfg;
02620      cmd_t cmd;
02621      
02622      Block* paddle_left;
02623      Block* paddle_right;
02624 
02625      static Option showData;
02626 
02627   public:    
02628      static const Size size;
02629 
02630      // constructor
02631      ModelGripper( World* world,
02632                         Model* parent,
02633                         const std::string& type );
02634      // destructor
02635      virtual ~ModelGripper();
02636   
02637      virtual void Load();
02638      virtual void Save();
02639 
02641      void SetConfig( config_t & newcfg ){ this->cfg = newcfg; FixBlocks(); }
02642      
02644      config_t GetConfig(){ return cfg; };
02645      
02647      void SetCommand( cmd_t cmd ) { this->cmd = cmd; }
02649      void CommandClose() { SetCommand( CMD_CLOSE ); }
02651      void CommandOpen() { SetCommand( CMD_OPEN ); }
02653      void CommandUp() { SetCommand( CMD_UP ); }
02655      void CommandDown() { SetCommand( CMD_DOWN ); }
02656   };
02657 
02658 
02659   // \todo BUMPER MODEL --------------------------------------------------------
02660 
02661   //   typedef struct
02662   //   {
02663   //     Pose pose;
02664   //     stg_meters_t length;
02665   //   } stg_bumper_config_t;
02666 
02667   //   typedef struct
02668   //   {
02669   //     Model* hit;
02670   //     stg_point_t hit_point;
02671   //   } stg_bumper_sample_t;
02672 
02673 
02674   // FIDUCIAL MODEL --------------------------------------------------------
02675 
02677   class ModelFiducial : public Model
02678   {
02679   public:  
02681      class Fiducial
02682      {
02683      public:
02684         stg_meters_t range; 
02685         stg_radians_t bearing; 
02686         Pose geom; 
02687         Pose pose; 
02688         Model* mod; 
02689         int id; 
02690      };
02691 
02692   private:
02693      // if neighbor is visible, add him to the fiducial scan
02694      void AddModelIfVisible( Model* him );
02695 
02696      virtual void Update();
02697      virtual void DataVisualize( Camera* cam );
02698 
02699      static Option showData;
02700      static Option showFov;
02701      
02702      std::vector<Fiducial> fiducials;
02703         
02704   public:       
02705      ModelFiducial( World* world, 
02706                          Model* parent,
02707                          const std::string& type );
02708      virtual ~ModelFiducial();
02709     
02710      virtual void Load();
02711      void Shutdown( void );
02712 
02713      stg_meters_t max_range_anon;
02714      stg_meters_t max_range_id; 
02715      stg_meters_t min_range; 
02716      stg_radians_t fov; 
02717      stg_radians_t heading; 
02718      int key; 
02719         
02720         
02722      std::vector<Fiducial>& GetFiducials() { return fiducials; }
02723         
02725      Fiducial* GetFiducials( unsigned int* count )
02726      {
02727         if( count ) *count = fiducials.size();
02728         return &fiducials[0];
02729      }
02730   };
02731 
02732 
02733   // RANGER MODEL --------------------------------------------------------
02734 
02736   class ModelRanger : public Model
02737   {
02738   public:
02739      class Sensor
02740      {
02741      public:
02742         Pose pose;
02743         Size size;
02744         Bounds bounds_range;
02745         stg_radians_t fov;
02746         int ray_count;
02747         stg_meters_t range;
02748      };
02749 
02750   protected:
02751 
02752      virtual void Startup();
02753      virtual void Shutdown();
02754      virtual void Update();
02755      virtual void DataVisualize( Camera* cam );
02756     
02757   public:
02758      ModelRanger( World* world, Model* parent,
02759                       const std::string& type );
02760      virtual ~ModelRanger();
02761     
02762      virtual void Load();
02763      virtual void Print( char* prefix );
02764      
02765      std::vector<Sensor> sensors;
02766     
02767   private:
02768      static Option showRangerData;
02769      static Option showRangerTransducers;       
02770   };
02771     
02772   // BLINKENLIGHT MODEL ----------------------------------------------------
02773   class ModelBlinkenlight : public Model
02774   {
02775   private:
02776      double dutycycle;
02777      bool enabled;
02778      stg_msec_t period;
02779      bool on;
02780 
02781      static Option showBlinkenData;
02782   public:
02783      ModelBlinkenlight( World* world,
02784                               Model* parent,
02785                               const std::string& type );
02786 
02787      ~ModelBlinkenlight();
02788     
02789      virtual void Load();
02790      virtual void Update();
02791      virtual void DataVisualize( Camera* cam );
02792   };
02793 
02794     
02795   // CAMERA MODEL ----------------------------------------------------
02796 
02798   class ModelCamera : public Model
02799   {
02800   public:
02801      typedef struct 
02802      {
02803         // GL_V3F
02804         GLfloat x, y, z;
02805      } ColoredVertex;
02806   
02807   private:
02808      Canvas* _canvas;
02809 
02810      GLfloat* _frame_data;  //opengl read buffer
02811      GLubyte* _frame_color_data;  //opengl read buffer
02812 
02813      bool _valid_vertexbuf_cache;
02814      ColoredVertex* _vertexbuf_cache; //cached unit vectors with appropriate rotations (these must be scalled by z-buffer length)
02815     
02816      int _width;         //width of buffer
02817      int _height;        //height of buffer
02818      static const int _depth = 4;
02819     
02820      int _camera_quads_size;
02821      GLfloat* _camera_quads;
02822      GLubyte* _camera_colors;
02823     
02824      static Option showCameraData;
02825     
02826      PerspectiveCamera _camera;
02827      float _yaw_offset; //position camera is mounted at
02828      float _pitch_offset;
02829         
02831      bool GetFrame();
02832     
02833   public:
02834      ModelCamera( World* world,
02835                       Model* parent,
02836                       const std::string& type ); 
02837       
02838      ~ModelCamera();
02839   
02840      virtual void Load();
02841     
02843      virtual void Update();
02844     
02846      //virtual void Draw( uint32_t flags, Canvas* canvas );
02847     
02849      virtual void DataVisualize( Camera* cam );
02850     
02852      int getWidth( void ) const { return _width; }
02853     
02855      int getHeight( void ) const { return _height; }
02856     
02858      const PerspectiveCamera& getCamera( void ) const { return _camera; }
02859     
02861      const GLfloat* FrameDepth() const { return _frame_data; }
02862     
02864      const GLubyte* FrameColor() const { return _frame_color_data; }
02865     
02867      void setPitch( float pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; }
02868     
02870      void setYaw( float yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; }
02871   };
02872 
02873   // POSITION MODEL --------------------------------------------------------
02874 
02876   class ModelPosition : public Model
02877   {
02878      friend class Canvas;
02879 
02880   public:
02882      typedef enum
02883         { CONTROL_VELOCITY, 
02884           CONTROL_POSITION 
02885         } ControlMode;
02886      
02888      typedef enum
02889         { LOCALIZATION_GPS, 
02890           LOCALIZATION_ODOM 
02891         } LocalizationMode;
02892      
02894      typedef enum
02895         { DRIVE_DIFFERENTIAL, 
02896           DRIVE_OMNI, 
02897           DRIVE_CAR 
02898         } DriveMode;
02899      
02900   private:
02901      Pose goal;
02902      ControlMode control_mode;
02903      DriveMode drive_mode;
02904      LocalizationMode localization_mode; 
02905      Velocity integration_error; 
02906      
02907      
02908   public:
02909      // constructor
02910      ModelPosition( World* world,
02911                          Model* parent,
02912                          const std::string& type );
02913      // destructor
02914      ~ModelPosition();
02915 
02916      virtual void Startup();
02917      virtual void Shutdown();
02918      virtual void Update();
02919      virtual void Load();
02920         
02923      class Waypoint
02924      {
02925      public:
02926         Waypoint( stg_meters_t x, stg_meters_t y, stg_meters_t z, stg_radians_t a, Color color ) ;
02927         Waypoint( const Pose& pose, Color color ) ;
02928         Waypoint();
02929         void Draw() const;
02930         
02931         Pose pose;
02932         Color color;
02933      };
02934      
02935      std::vector<Waypoint> waypoints;
02936 
02937      class WaypointVis : public Visualizer
02938      {
02939      public:
02940         WaypointVis();
02941         virtual ~WaypointVis( void ){}
02942         virtual void Visualize( Model* mod, Camera* cam );
02943      } wpvis;
02944      
02945      class PoseVis : public Visualizer
02946      {
02947      public:
02948         PoseVis();
02949         virtual ~PoseVis( void ){}
02950         virtual void Visualize( Model* mod, Camera* cam );
02951      } posevis;
02952 
02954      void SetOdom( Pose odom );
02955         
02958      void SetSpeed( double x, double y, double a );
02959      void SetXSpeed( double x );
02960      void SetYSpeed( double y );
02961      void SetZSpeed( double z );
02962      void SetTurnSpeed( double a );
02963      void SetSpeed( Velocity vel );
02965      void Stop();
02966 
02969      void GoTo( double x, double y, double a );
02970      void GoTo( Pose pose );
02971 
02972      // localization state
02973      Pose est_pose; 
02974      Pose est_pose_error; 
02975      Pose est_origin; 
02976   };
02977 
02978 
02979   // ACTUATOR MODEL --------------------------------------------------------
02980 
02982   class ModelActuator : public Model
02983   {
02984   public:
02986      typedef enum
02987         { CONTROL_VELOCITY,
02988           CONTROL_POSITION
02989         } ControlMode;
02990   
02992      typedef enum
02993         { TYPE_LINEAR,
02994           TYPE_ROTATIONAL
02995         } ActuatorType;
02996   
02997   private:
02998      double goal; //< the current velocity or pose to reach, depending on the value of control_mode
02999      double pos;
03000      double max_speed;
03001      double min_position;
03002      double max_position;
03003      ControlMode control_mode;
03004      ActuatorType actuator_type;
03005      stg_point3_t axis;
03006   
03007      Pose InitialPose;
03008   public:  
03009      // constructor
03010      ModelActuator( World* world,
03011                          Model* parent,
03012                          const std::string& type );
03013      // destructor
03014      ~ModelActuator();
03015   
03016      virtual void Startup();
03017      virtual void Shutdown();
03018      virtual void Update();
03019      virtual void Load();
03020   
03023      void SetSpeed( double speed );
03024   
03025      double GetSpeed() const {return goal;}
03026   
03029      void GoTo( double pose );
03030   
03031      double GetPosition() const {return pos;};
03032      double GetMaxPosition() const {return max_position;};
03033      double GetMinPosition() const {return min_position;};
03034   
03035   };
03036 
03037 
03038 }; // end namespace stg
03039 
03040 #endif

Generated on Tue Oct 20 15:42:05 2009 for Stage by  doxygen 1.6.1