stage.hh
Go to the documentation of this file.
00001 
00002 #ifndef STG_H
00003 #define STG_H
00004 /*
00005  *  Stage : a multi-robot simulator. Part of the Player Project.
00006  * 
00007  *  Copyright (C) 2001-2009 Richard Vaughan, Brian Gerkey, Andrew
00008  *  Howard, Toby Collett, Reed Hedges, Alex Couture-Beil, Jeremy
00009  *  Asher, Pooya Karimian
00010  *
00011  *  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU General Public License for more details.
00020  *
00021  *  You should have received a copy of the GNU General Public License
00022  *  along with this program; if not, write to the Free Software
00023  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00024  *
00025  */
00026 
00034 // C libs
00035 #include <unistd.h>
00036 #include <stdint.h> // for portable int types eg. uint32_t
00037 #include <assert.h>
00038 #include <stdlib.h>
00039 #include <stdio.h>
00040 #include <libgen.h>
00041 #include <string.h>
00042 #include <sys/types.h>
00043 #include <sys/time.h>
00044 #include <pthread.h> 
00045 
00046 // C++ libs
00047 #include <cmath>
00048 #include <iostream>
00049 #include <vector>
00050 #include <list>
00051 #include <map>
00052 #include <set>
00053 #include <queue>
00054 #include <algorithm>
00055 
00056 // FLTK Gui includes
00057 #include <FL/Fl.H>
00058 #include <FL/Fl_Box.H>
00059 #include <FL/Fl_Gl_Window.H>
00060 #include <FL/Fl_Menu_Bar.H>
00061 #include <FL/Fl_Window.H>
00062 #include <FL/fl_draw.H>
00063 #include <FL/gl.h> // FLTK takes care of platform-specific GL stuff
00064 // except GLU
00065 #ifdef __APPLE__
00066 #include <OpenGL/glu.h>
00067 #else
00068 #include <GL/glu.h>
00069 #endif 
00070 
00072 namespace Stg 
00073 {
00074   // forward declare
00075   class Block;
00076   class Canvas;
00077   class Cell;
00078   class Worldfile;
00079   class World;
00080   class WorldGui;
00081   class Model;
00082   class OptionsDlg;
00083   class Camera;
00084   class FileManager;
00085   class Option;
00086   
00087   typedef Model* (*creator_t)( World*, Model*, const std::string& type );
00088   
00090   typedef std::set<Model*> ModelPtrSet;
00091 
00093   typedef std::vector<Model*> ModelPtrVec;
00094 
00096   typedef std::set<Block*> BlockPtrSet;
00097 
00099   typedef std::vector<Cell*> CellPtrVec;
00100 
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 id_t;
00184 
00186   typedef double meters_t;
00187 
00189   typedef double radians_t;
00190 
00192   typedef struct timeval time_t;
00193 
00195   typedef unsigned long msec_t;
00196 
00198   typedef uint64_t usec_t;
00199 
00201   typedef double kg_t; // Kilograms (mass)
00202 
00204   typedef double joules_t;
00205 
00207   typedef double watts_t;
00208   
00209   class Color
00210   {
00211   public:
00212      float r,g,b,a;
00213     
00214      Color( float r, float g, float b, float a=1.0 );
00215     
00219      Color( const std::string& name );  
00220     
00221      Color();
00222     
00223      bool operator!=( const Color& other ) const;
00224      bool operator==( const Color& other ) const;
00225      static Color RandomColor();
00226      void Print( const char* prefix ) const;
00227 
00229         static const Color blue, red, green, yellow, magenta, cyan;
00230 
00231         bool Load( Worldfile* wf, int entity );
00232   };
00233   
00235   class Size
00236   {
00237   public:
00238     meters_t x, y, z;
00239     
00240     Size( meters_t x, 
00241              meters_t y, 
00242              meters_t z )
00243       : x(x), y(y), z(z)
00244     {/*empty*/}
00245     
00247     Size() : x( 0.4 ), y( 0.4 ), z( 1.0 )
00248     {/*empty*/} 
00249     
00250      void Load( Worldfile* wf, int section, const char* keyword );
00251      void Save( Worldfile* wf, int section, const char* keyword ) const;
00252      
00253      void Zero()
00254      { x=y=z=0.0; }
00255   };
00256   
00258   class Pose
00259   {
00260   public:
00261     meters_t x, y, z;
00262     radians_t a;
00263     
00264     Pose( meters_t x, 
00265              meters_t y, 
00266              meters_t z,
00267              radians_t a ) 
00268       : x(x), y(y), z(z), a(a)
00269     { /*empty*/ }
00270     
00271     Pose() : x(0.0), y(0.0), z(0.0), a(0.0)
00272     { /*empty*/ }        
00273     
00274     virtual ~Pose(){};
00275     
00278     static Pose Random( meters_t xmin, meters_t xmax, 
00279                                 meters_t ymin, meters_t ymax )
00280     {        
00281       return Pose( xmin + drand48() * (xmax-xmin),
00282                          ymin + drand48() * (ymax-ymin),
00283                          0, 
00284                          normalize( drand48() * (2.0 * M_PI) ));
00285     }
00286     
00290     virtual void Print( const char* prefix ) const
00291     {
00292       printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
00293                   prefix, x,y,z,a );
00294     }
00295     
00296      std::string String() const
00297      {
00298         char buf[256];
00299         snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]",
00300                      x,y,z,a );
00301         return std::string(buf);
00302      }
00303     
00304      /* returns true iff all components of the velocity are zero. */
00305      bool IsZero() const 
00306      { return( !(x || y || z || a )); };
00307     
00309      void Zero()
00310      { x=y=z=a=0.0; }
00311     
00312      void Load( Worldfile* wf, int section, const char* keyword );
00313      void Save( Worldfile* wf, int section, const char* keyword );
00314     
00315      inline Pose operator+( const Pose& p ) const
00316      {
00317         const double cosa = cos(a);
00318         const double sina = sin(a);
00319       
00320         return Pose( x + p.x * cosa - p.y * sina,
00321                          y + p.x * sina + p.y * cosa,
00322                          z + p.z,
00323                          normalize(a + p.a) );   
00324      }  
00325      
00326      // a < b iff a is closer to the origin than b
00327      bool operator<( const Pose& other ) const
00328      {
00329         return( hypot( y, x ) < hypot( other.y, other.x ));
00330      }
00331      
00332      bool operator==( const Pose& other ) const
00333      {
00334         return(  x==other.x && 
00335                     y==other.y && 
00336                     z==other.z && 
00337                     a==other.a );
00338      }
00339 
00340      bool operator!=( const Pose& other ) const
00341      {
00342         return(  x!=other.x ||
00343                     y!=other.y || 
00344                     z!=other.z || 
00345                     a!=other.a );
00346      }
00347      
00348      meters_t Distance2D( const Pose& other ) const
00349      {
00350         return hypot( x-other.x, y-other.y );
00351      }
00352   };
00353   
00354   
00357   class Velocity : public Pose
00358   {
00359   public:
00365     Velocity( meters_t x, 
00366                   meters_t y, 
00367                   meters_t z,
00368                   radians_t a ) :
00369         Pose( x, y, z, a )
00370     { /*empty*/ }
00371     
00372     Velocity()
00373     { /*empty*/ }        
00374     
00380     virtual void Print( const char* prefix ) const
00381     {
00382         if( prefix )
00383           printf( "%s", prefix );
00384         
00385         printf( "velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
00386                   x,y,z,a );        
00387     }    
00388   };
00389   
00392   class Geom
00393   {
00394   public:
00395     Pose pose;
00396     Size size;
00397     
00403     void Print( const char* prefix ) const
00404     {
00405         if( prefix )
00406           printf( "%s", prefix );
00407         
00408         printf( "geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n",
00409                   pose.x,
00410                   pose.y,
00411                   pose.a,
00412                   size.x,
00413                   size.y );
00414     }
00415      
00417      Geom() : pose(), size() {}
00418 
00420      Geom( const Pose& p, const Size& s ) : pose(p), size(s) {}
00421      
00422      void Zero()
00423      {
00424         pose.Zero();
00425         size.Zero();
00426      }
00427   };
00428   
00430   class Bounds
00431   {
00432   public:
00434     double min;
00436     double max; 
00437     
00438     Bounds() : min(0), max(0) { /* empty*/  }
00439     Bounds( double min, double max ) : min(min), max(max) { /* empty*/  }
00440 
00441         void Load( Worldfile* wf, int section, const char* keyword );
00442   };
00443     
00445   class bounds3d_t
00446   {
00447   public:
00449     Bounds x; 
00451     Bounds y; 
00453     Bounds z; 
00454 
00455      bounds3d_t() : x(), y(), z() {}
00456      bounds3d_t( const Bounds& x, const Bounds& y, const Bounds& z) 
00457         : x(x), y(y), z(z) {}
00458   };
00459   
00461   typedef struct
00462   {
00463     Bounds range; 
00464     radians_t angle; 
00465   } fov_t;
00466   
00468   class point_t
00469   {
00470   public:
00471     meters_t x, y;
00472      point_t( meters_t x, meters_t y ) : x(x), y(y){}    
00473      point_t() : x(0.0), y(0.0){}
00474     
00475      bool operator+=( const point_t& other ) 
00476      { return ((x += other.x) && (y += other.y) ); }  
00477   };
00478   
00480   class point3_t
00481   {
00482   public:
00483     meters_t x,y,z;
00484      point3_t( meters_t x, meters_t y, meters_t z ) 
00485         : x(x), y(y), z(z) {}    
00486   
00487      point3_t() : x(0.0), y(0.0), z(0.0) {}
00488   };
00489   
00491   class point_int_t
00492   {
00493   public:
00494     int x,y;
00495         point_int_t( int x, int y ) : x(x), y(y){}   
00496         point_int_t() : x(0), y(0){}
00497         
00499         bool operator<( const point_int_t& other ) const
00500         {
00501             if( x < other.x ) return true;
00502             if( other.x < x ) return false;
00503             return y < other.y;
00504         }
00505  
00506      bool operator==( const point_int_t& other ) const
00507         { return ((x == other.x) && (y == other.y) ); }
00508   };
00509   
00510   typedef std::vector<point_int_t> PointIntVec;
00511 
00514   point_t* unit_square_points_create();
00515   
00516   
00519   namespace Gl
00520   {
00521      void pose_shift( const Pose &pose );
00522      void pose_inverse_shift( const Pose &pose );
00523      void coord_shift( double x, double y, double z, double a  );
00524      void draw_grid( bounds3d_t vol );
00526      void draw_string( float x, float y, float z, const char *string);
00527      void draw_string_multiline( float x, float y, float w, float h, 
00528                                           const char *string, Fl_Align align );
00529      void draw_speech_bubble( float x, float y, float z, const char* str );
00530      void draw_octagon( float w, float h, float m );
00531      void draw_octagon( float x, float y, float w, float h, float m );
00532      void draw_vector( double x, double y, double z );
00533      void draw_origin( double len );
00534      void draw_array( float x, float y, float w, float h, 
00535                             float* data, size_t len, size_t offset, 
00536                             float min, float max );
00537      void draw_array( float x, float y, float w, float h, 
00538                             float* data, size_t len, size_t offset );
00540      void draw_centered_rect( float x, float y, float dx, float dy );
00541   } // namespace Gl
00542   
00543   void RegisterModels();
00544   
00545   
00547   class Visualizer {
00548   private:
00549      const std::string menu_name;
00550      const std::string worldfile_name;
00551      
00552   public:
00553      Visualizer( const std::string& menu_name, 
00554                      const std::string& worldfile_name ) 
00555         : menu_name( menu_name ),
00556           worldfile_name( worldfile_name )
00557      { }
00558      
00559      virtual ~Visualizer( void ) { }
00560      virtual void Visualize( Model* mod, Camera* cam ) = 0;
00561      
00562      const std::string& GetMenuName() { return menu_name; }
00563      const std::string& GetWorldfileName() { return worldfile_name; }    
00564   };
00565 
00566 
00567   typedef int(*model_callback_t)(Model* mod, void* user );
00568   typedef int(*world_callback_t)(World* world, void* user );
00569   
00570   // return val, or minval if val < minval, or maxval if val > maxval
00571   double constrain( double val, double minval, double maxval );
00572     
00573   typedef struct 
00574   {
00575     int enabled;
00576     Pose pose;
00577     meters_t size; 
00578     Color color;
00579     msec_t period; 
00580     double duty_cycle; 
00581   } blinkenlight_t;
00582 
00583   
00585   typedef struct
00586   {
00587     Pose pose;
00588     Size size;
00589   } rotrect_t; // rotated rectangle
00590 
00596   int rotrects_from_image_file( const std::string& filename, 
00597                                                                 std::vector<rotrect_t>& rects,
00598                                                                 unsigned int& widthp, 
00599                                                                 unsigned int& heightp );
00600 
00601   
00604   typedef bool (*ray_test_func_t)(Model* candidate, 
00605                                                   Model* finder, 
00606                                                   const void* arg );
00607 
00608   // STL container iterator macros
00609 #define VAR(V,init) __typeof(init) V=(init)
00610 #define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();I++)
00611 
00613   template <class T, class C>
00614   void EraseAll( T thing, C& cont )
00615   { cont.erase( std::remove( cont.begin(), cont.end(), thing ), cont.end() ); }
00616   
00617   // Error macros - output goes to stderr
00618 #define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00619 #define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00620 #define PRINT_ERR2(m,a,b) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00621 #define PRINT_ERR3(m,a,b,c) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00622 #define PRINT_ERR4(m,a,b,c,d) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
00623 #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__)
00624 
00625   // Warning macros
00626 #define PRINT_WARN(m) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00627 #define PRINT_WARN1(m,a) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00628 #define PRINT_WARN2(m,a,b) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00629 #define PRINT_WARN3(m,a,b,c) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00630 #define PRINT_WARN4(m,a,b,c,d) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
00631 #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__)
00632 
00633   // Message macros
00634 #ifdef DEBUG
00635 #define PRINT_MSG(m) printf( "Stage: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00636 #define PRINT_MSG1(m,a) printf( "Stage: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00637 #define PRINT_MSG2(m,a,b) printf( "Stage: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00638 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00639 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
00640 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m" (%s %s)\n", a, b, c, d, e,__FILE__, __FUNCTION__)
00641 #else
00642 #define PRINT_MSG(m) printf( "Stage: "m"\n" )
00643 #define PRINT_MSG1(m,a) printf( "Stage: "m"\n", a)
00644 #define PRINT_MSG2(m,a,b) printf( "Stage: "m"\n,", a, b )
00645 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m"\n", a, b, c )
00646 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m"\n", a, b, c, d )
00647 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m"\n", a, b, c, d, e )
00648 #endif
00649 
00650   // DEBUG macros
00651 #ifdef DEBUG
00652 #define PRINT_DEBUG(m) printf( "debug: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00653 #define PRINT_DEBUG1(m,a) printf( "debug: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00654 #define PRINT_DEBUG2(m,a,b) printf( "debug: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00655 #define PRINT_DEBUG3(m,a,b,c) printf( "debug: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00656 #define PRINT_DEBUG4(m,a,b,c,d) printf( "debug: "m" (%s %s)\n", a, b, c ,d, __FILE__, __FUNCTION__)
00657 #define PRINT_DEBUG5(m,a,b,c,d,e) printf( "debug: "m" (%s %s)\n", a, b, c ,d, e, __FILE__, __FUNCTION__)
00658 #else
00659 #define PRINT_DEBUG(m)
00660 #define PRINT_DEBUG1(m,a)
00661 #define PRINT_DEBUG2(m,a,b)
00662 #define PRINT_DEBUG3(m,a,b,c)
00663 #define PRINT_DEBUG4(m,a,b,c,d)
00664 #define PRINT_DEBUG5(m,a,b,c,d,e)
00665 #endif
00666 
00667   class Block;
00668   class Model;
00669 
00672   typedef int (*model_callback_t)( Model* mod, void* user );
00673 
00674   // ANCESTOR CLASS
00676   class Ancestor
00677   {
00678     friend class Canvas; // allow Canvas access to our private members
00679      
00680   protected:
00681 
00683      std::map<std::string,unsigned int> child_type_counts;
00684 
00685      ModelPtrVec children;
00686 
00687     bool debug;
00688 
00690      std::map<std::string,void*> props;
00691 
00692      std::string token;
00693 
00694      void Load( Worldfile* wf, int section );
00695      void Save( Worldfile* wf, int section );    
00696              
00697   public:   
00698     Ancestor();
00699     virtual ~Ancestor();
00700      
00702      ModelPtrVec& GetChildren(){ return children;}
00703     
00705     void ForEachDescendant( model_callback_t func, void* arg );
00706      
00707     virtual void AddChild( Model* mod );
00708     virtual void RemoveChild( Model* mod );
00709     virtual Pose GetGlobalPose();
00710      
00711     const char* Token(){ return token.c_str(); }
00712 
00713     const std::string& TokenStr(){ return token; }
00714      
00715     void SetToken( const std::string& str ){ token = str; } 
00716      
00718      void SetProperty( std::string& key, void* value ){ props[ key ] = value; }
00719      
00721      void* GetProperty( std::string& key )
00722      {
00723         std::map<std::string,void*>::iterator it = props.find( key );       
00724         return( it == props.end() ? NULL : it->second );
00725      }
00726   };
00727 
00730   class RaytraceResult
00731   {
00732   public:
00733     Pose pose; 
00734     meters_t range; 
00735     Model* mod; 
00736     Color color; 
00737      
00738      RaytraceResult() : pose(), range(0), mod(NULL), color() {}
00739      RaytraceResult( const Pose& pose, 
00740                           meters_t range ) 
00741         : pose(pose), range(range), mod(NULL), color() {}    
00742   };
00743     
00744   class Ray
00745   {
00746   public:
00747      Ray( const Model* mod, const Pose& origin, const meters_t range, const ray_test_func_t func, const void* arg, const bool ztest ) :
00748         mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest)
00749      {}
00750 
00751      Ray() : mod(NULL), origin(0,0,0,0), range(0), func(NULL), arg(NULL), ztest(true)
00752      {}
00753         
00754         const Model* mod;
00755         Pose origin;
00756         meters_t range;
00757         ray_test_func_t func;
00758         const void* arg;
00759      bool ztest;        
00760   };
00761         
00762 
00763   // defined in stage_internal.hh
00764   class Region;
00765   class SuperRegion;
00766   class BlockGroup;
00767   class PowerPack;
00768 
00769   class LogEntry
00770   {
00771      usec_t timestamp;
00772      Model* mod;
00773      Pose pose;
00774      
00775   public:
00776      LogEntry( usec_t timestamp, Model* mod );
00777      
00779      static std::vector<LogEntry> log;
00780      
00782      static size_t Count(){ return log.size(); }
00783      
00785      static void Clear(){ log.clear(); }
00786 
00788      static void Print();
00789   };
00790 
00791   class CtrlArgs
00792   {
00793   public:
00794      std::string worldfile;
00795      std::string cmdline;
00796 
00797      CtrlArgs( std::string w, std::string c ) : worldfile(w), cmdline(c) {}
00798   };
00799 
00801   class World : public Ancestor
00802   {
00803     friend class Block;
00804     friend class Model; // allow access to private members
00805     friend class ModelFiducial;
00806     friend class Canvas;
00807 
00808   public: 
00811      static std::vector<std::string> args;
00812      static std::string ctrlargs;
00813 
00814   private:
00815     
00816     static std::set<World*> world_set; 
00817     static bool quit_all; 
00818     static void UpdateCb( World* world);
00819     static unsigned int next_id; 
00820      
00821     bool destroy;
00822     bool dirty; 
00823 
00825      //std::vector<Model*> jit_render;
00826 
00828      std::set<Model*> models;
00829 
00831      std::map<std::string, Model*> models_by_name;      
00832 
00834      std::map<int,Model*> models_by_wfentity;
00835         
00838      ModelPtrVec models_with_fiducials;
00839         
00840         struct ltx
00841         {
00842             bool operator()(const Model* a, const Model* b) const;
00843         };
00844         
00845         struct lty
00846         {
00847             bool operator()(const Model* a, const Model* b) const;
00848         };
00849         
00850         std::set<Model*,ltx> models_with_fiducials_byx;
00851         std::set<Model*,lty> models_with_fiducials_byy;
00852                      
00854      void FiducialInsert( Model* mod )
00855      { 
00856         FiducialErase( mod ); // make sure it's not there already
00857         models_with_fiducials.push_back( mod ); 
00858      }
00859      
00861      void FiducialErase( Model* mod )
00862      { 
00863         EraseAll( mod, models_with_fiducials );
00864      }
00865 
00866     double ppm; 
00867     bool quit; 
00868      
00869      bool show_clock; 
00870      unsigned int show_clock_interval; 
00871 
00872     pthread_mutex_t thread_mutex; 
00873      unsigned int threads_working; 
00874     pthread_cond_t threads_start_cond; 
00875     pthread_cond_t threads_done_cond; 
00876     int total_subs; 
00877      unsigned int worker_threads; 
00878     
00879   protected:     
00880 
00881      std::list<std::pair<world_callback_t,void*> > cb_list; 
00882     bounds3d_t extent; 
00883     bool graphics;
00884 
00885      std::set<Option*> option_table; 
00886      std::list<PowerPack*> powerpack_list; 
00887 
00888     usec_t quit_time;
00889      std::list<float*> ray_list;
00890     usec_t sim_time; 
00891      std::map<point_int_t,SuperRegion*> superregions;
00892     SuperRegion* sr_cached; 
00893      
00894      std::vector<ModelPtrVec> update_lists;  
00895      
00896     uint64_t updates; 
00897     Worldfile* wf; 
00898 
00899      void CallUpdateCallbacks(); 
00900 
00901   public:
00902      
00903     bool paused; 
00904 
00905     virtual void Start(){ paused = false; };
00906     virtual void Stop(){ paused = true; };
00907     virtual void TogglePause(){ paused ? Start() : Stop(); };
00908 
00909         bool Paused() const { return( paused ); };
00910         
00914         virtual void Redraw( void ){ }; // does nothing
00915 
00916      PointIntVec rt_cells;
00917      PointIntVec rt_candidate_cells;
00918 
00919     static const int DEFAULT_PPM = 50;  // default resolution in pixels per meter
00920 
00923      void AddUpdateCallback( world_callback_t cb, void* user );
00924 
00927      int RemoveUpdateCallback( world_callback_t cb, void* user );
00928 
00930      void Log( Model* mod );
00931 
00933     void NeedRedraw(){ dirty = true; };
00934     
00936     Model* ground;
00937     
00940     virtual std::string ClockString( void ) const;
00941         
00942      Model* CreateModel( Model* parent, const std::string& typestr );    
00943     void LoadModel( Worldfile* wf, int entity );
00944     void LoadBlock( Worldfile* wf, int entity );
00945     void LoadBlockGroup( Worldfile* wf, int entity );
00946     
00947         void LoadSensor( Worldfile* wf, int entity );
00948 
00949     virtual Model* RecentlySelectedModel() const { return NULL; }
00950         
00951     SuperRegion* AddSuperRegion( const point_int_t& coord );
00952     SuperRegion* GetSuperRegion( int32_t x, int32_t y );
00953     void ExpireSuperRegion( SuperRegion* sr );
00954         
00957     void ForEachCellInLine( const point_int_t& pt1,
00958                                                         const point_int_t& pt2, 
00959                                                         CellPtrVec& cells );
00960         
00963     int32_t MetersToPixels( meters_t x ) const
00964     { return (int32_t)floor(x * ppm); };
00965         
00966     point_int_t MetersToPixels( const point_t& pt ) const
00967     { return point_int_t( MetersToPixels(pt.x), MetersToPixels(pt.y)); };
00968         
00969     // dummy implementations to be overloaded by GUI subclasses
00970     virtual void PushColor( Color col ) 
00971      { /* do nothing */  (void)col; };
00972     virtual void PushColor( double r, double g, double b, double a ) 
00973      { /* do nothing */ (void)r; (void)g; (void)b; (void)a; };
00974      
00975     virtual void PopColor(){ /* do nothing */  };
00976         
00977     SuperRegion* CreateSuperRegion( point_int_t origin );
00978     void DestroySuperRegion( SuperRegion* sr );
00979         
00981      RaytraceResult Raytrace( const Ray& ray );
00982 
00983     RaytraceResult Raytrace( const Pose& pose,           
00984                                                 const meters_t range,
00985                                                 const ray_test_func_t func,
00986                                                 const Model* finder,
00987                                                 const void* arg,
00988                                                 const bool ztest );
00989         
00990     void Raytrace( const Pose &pose,             
00991                          const meters_t range,
00992                          const radians_t fov,
00993                          const ray_test_func_t func,
00994                          const Model* finder,
00995                          const void* arg,
00996                          RaytraceResult* samples,
00997                          const uint32_t sample_count,
00998                          const bool ztest );
00999         
01000         
01002     void Extend( point3_t pt );
01003   
01004     virtual void AddModel( Model* mod );
01005     virtual void RemoveModel( Model* mod );
01006 
01007     void AddModelName( Model* mod, const std::string& name );
01008         
01009     void AddPowerPack( PowerPack* pp );
01010     void RemovePowerPack( PowerPack* pp );
01011         
01012     void ClearRays();
01013   
01015     void RecordRay( double x1, double y1, double x2, double y2 );
01016         
01019     bool PastQuitTime();
01020                 
01021     static void* update_thread_entry( std::pair<World*,int>* info );
01022     
01023     class Event
01024     {
01025     public:
01026       
01027       Event( usec_t time, Model* mod ) 
01028           : time(time), mod(mod) {}
01029       
01030       usec_t time; 
01031       Model* mod; 
01032       
01035       bool operator<( const Event& other ) const;
01036     };
01037      
01039      std::vector<std::priority_queue<Event> > event_queues;
01040 
01052      void Enqueue( unsigned int queue_num, usec_t delay, Model* mod );
01053      
01055      std::set<Model*> active_energy;
01056 
01058      std::set<Model*> active_velocity;
01059 
01061      usec_t sim_interval;
01062      
01064      void ConsumeQueue( unsigned int queue_num );
01065 
01068      unsigned int GetEventQueue( Model* mod ) const;
01069 
01070   public:
01072     static bool UpdateAll(); 
01073      
01074     World( const std::string& name = "MyWorld", 
01075               double ppm = DEFAULT_PPM );
01076         
01077     virtual ~World();
01078     
01080     usec_t SimTimeNow(void) const { return sim_time; }
01081         
01084     Worldfile* GetWorldFile()   { return wf; };
01085         
01089     virtual bool IsGUI() const { return false; }
01090      
01096     virtual void Load( const std::string& worldfile_path );
01097 
01098     virtual void UnLoad();
01099 
01100     virtual void Reload();
01101 
01104     virtual bool Save( const char* filename );
01105 
01109     virtual bool Update(void);
01110      
01114     bool TestQuit() const { return( quit || quit_all );  }
01115         
01117     void Quit(){ quit = true; }
01118 
01120     void QuitAll(){ quit_all = true; }
01121 
01123     void CancelQuit(){ quit = false; }
01124 
01126     void CancelQuitAll(){ quit_all = false; }
01127      
01128      void TryCharge( PowerPack* pp, const Pose& pose );
01129 
01132     double Resolution() const { return ppm; };
01133    
01136     Model* GetModel( const std::string& name ) const;
01137 
01139     const std::set<Model*> GetAllModels() const { return models; };
01140   
01142     const bounds3d_t& GetExtent() const { return extent; };
01143   
01145     uint64_t GetUpdateCount() const { return updates; }
01146 
01148      void RegisterOption( Option* opt );    
01149      
01151      void ShowClock( bool enable ){ show_clock = enable; };
01152 
01154      Model* GetGround() {return ground;};
01155     
01156   };
01157 
01158   class Block
01159   {
01160     friend class BlockGroup;
01161     friend class Model;
01162     friend class SuperRegion;
01163     friend class World;
01164     friend class Canvas;
01165   public:
01166   
01170     Block( Model* mod,  
01171                      const std::vector<point_t>& pts,
01172                      meters_t zmin,
01173                      meters_t zmax,
01174                      Color color,
01175                      bool inherit_color,
01176                      bool wheel );
01177   
01179     Block(  Model* mod,  Worldfile* wf, int entity);
01180      
01181     ~Block();
01182      
01184     void Map();      
01185      
01187     void UnMap();    
01188      
01190      void DrawSolid();
01191 
01193     void DrawFootPrint(); 
01194 
01196      void Translate( double x, double y );   
01197 
01199      double CenterX();
01200 
01202      double CenterY();
01203 
01205      void SetCenterX( double y );
01206 
01208      void SetCenterY( double y );
01209 
01211      void SetCenter( double x, double y);    
01212 
01214      void SetZ( double min, double max );
01215         
01216     inline void RemoveFromCellArray( CellPtrVec* blocks );
01217     inline void GenerateCandidateCells();  
01218         
01219         void AppendTouchingModels( ModelPtrSet& touchers );
01220      
01222     Model* TestCollision(); 
01223     void SwitchToTestedCells();  
01224     void Load( Worldfile* wf, int entity );  
01225     Model* GetModel(){ return mod; };  
01226     const Color& GetColor();        
01227      void Rasterize( uint8_t* data, 
01228                           unsigned int width, unsigned int height,      
01229                           meters_t cellwidth, meters_t cellheight );
01230         
01231   private:
01232     Model* mod; 
01233      std::vector<point_t> mpts; 
01234     size_t pt_count; 
01235      std::vector<point_t> pts; 
01236     Size size;   
01237     Bounds local_z; 
01238     Color color;
01239     bool inherit_color;
01240         bool wheel;
01241 
01242     void DrawTop();
01243     void DrawSides();
01244      
01246     Bounds global_z;     
01247     bool mapped;
01248         
01250         std::vector< std::list<Block*>::iterator > list_entries;
01251 
01254      CellPtrVec * rendered_cells;
01255 
01262      CellPtrVec * candidate_cells;
01263     
01264      PointIntVec gpts;
01265     
01268      point_t BlockPointToModelMeters( const point_t& bpt );
01269     
01271      void InvalidateModelPointCache();
01272   };
01273 
01274     
01275   class BlockGroup
01276   {
01277     friend class Model;
01278         friend class Block;
01279         
01280   private:
01281     int displaylist;
01282         
01283     void BuildDisplayList( Model* mod );
01284         
01285         BlockPtrSet blocks;
01286     Size size;
01287     point3_t offset;
01288     meters_t minx, maxx, miny, maxy;
01289         
01290   public:
01291     BlockGroup();
01292     ~BlockGroup();
01293         
01294     uint32_t GetCount(){ return blocks.size(); };
01295     const Size& GetSize(){ return size; };
01296     const point3_t& GetOffset(){ return offset; };
01297         
01300     void CalcSize();
01301      
01302     void AppendBlock( Block* block );
01303     void CallDisplayList( Model* mod );
01304     void Clear() ; 
01306      void AppendTouchingModels( ModelPtrSet& touchers );
01307      
01310     Model* TestCollision();
01311  
01312     void SwitchToTestedCells();
01313      
01314     void Map();
01315     void UnMap();
01316      
01318     void DrawSolid( const Geom &geom); 
01319 
01321      void DrawFootPrint( const Geom &geom);
01322 
01323     void LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfile *wf );
01324     void LoadBlock( Model* mod, Worldfile* wf, int entity );
01325      
01326      void Rasterize( uint8_t* data, 
01327                           unsigned int width, unsigned int height,
01328                           meters_t cellwidth, meters_t cellheight );
01329      
01330      void InvalidateModelPointCache()
01331      {
01332         FOR_EACH( it, blocks )
01333           (*it)->InvalidateModelPointCache();
01334      }
01335 
01336   };
01337 
01338   class Camera 
01339   {
01340   protected:
01341     float _pitch; //left-right (about y)
01342     float _yaw; //up-down (about x)
01343     float _x, _y, _z;
01344     
01345   public:
01346     Camera() : _pitch( 0 ), _yaw( 0 ), _x( 0 ), _y( 0 ), _z( 0 ) { }
01347     virtual ~Camera() { }
01348 
01349     virtual void Draw( void ) const = 0;
01350     virtual void SetProjection( void ) const = 0;
01351 
01352      float yaw( void ) const { return _yaw; }
01353      float pitch( void ) const { return _pitch; }
01354      
01355      float x( void ) const { return _x; }
01356      float y( void ) const { return _y; }
01357      float z( void ) const { return _z; }
01358      
01359     virtual void reset() = 0;
01360     virtual void Load( Worldfile* wf, int sec ) = 0;
01361 
01362     //TODO data should be passed in somehow else. (at least min/max stuff)
01363     //virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const = 0;
01364   };
01365 
01366   class PerspectiveCamera : public Camera
01367   {
01368   private:
01369     float _z_near;
01370     float _z_far;
01371     float _vert_fov;
01372     float _horiz_fov;
01373     float _aspect;
01374 
01375   public:
01376     PerspectiveCamera( void );
01377 
01378     virtual void Draw( void ) const;
01379     virtual void SetProjection( void ) const;
01380     //void SetProjection( float aspect ) const;
01381     void update( void );
01382 
01383     void strafe( float amount );
01384     void forward( float amount );
01385     
01386      void setPose( float x, float y, float z ) { _x = x; _y = y; _z = z; }
01387      void addPose( float x, float y, float z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; }
01388     void move( float x, float y, float z );
01389      void setFov( float horiz_fov, float vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; }
01391      void setAspect( float aspect ) { _aspect = aspect; }
01392      void setYaw( float yaw ) { _yaw = yaw; }
01393      float horizFov( void ) const { return _horiz_fov; }
01394      float vertFov( void ) const { return _vert_fov; }
01395      void addYaw( float yaw ) { _yaw += yaw; }
01396      void setPitch( float pitch ) { _pitch = pitch; }
01397      void addPitch( float pitch ) { _pitch += pitch; if( _pitch < 0 ) _pitch = 0; else if( _pitch > 180 ) _pitch = 180; }
01398     
01399      float realDistance( float z_buf_val ) const {
01400       return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) );
01401     }
01402      void scroll( float dy ) { _z += dy; }
01403      float nearClip( void ) const { return _z_near; }
01404      float farClip( void ) const { return _z_far; }
01405      void setClip( float near, float far ) { _z_far = far; _z_near = near; }
01406     
01407      void reset() { setPitch( 70 ); setYaw( 0 ); }
01408     
01409     void Load( Worldfile* wf, int sec );
01410     void Save( Worldfile* wf, int sec );
01411   };
01412   
01413   class OrthoCamera : public Camera
01414   {
01415   private:
01416     float _scale;
01417     float _pixels_width;
01418     float _pixels_height;
01419     float _y_min;
01420     float _y_max;
01421   
01422   public:
01423     OrthoCamera( void ) : _scale( 15 ) { }
01424     virtual void Draw() const;
01425     virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max );
01426     virtual void SetProjection( void ) const;
01427   
01428     void move( float x, float y );
01429      void setYaw( float yaw ) { _yaw = yaw; }
01430      void setPitch( float pitch ) { _pitch = pitch; }
01431      void addYaw( float yaw ) { _yaw += yaw;    }
01432      void addPitch( float pitch ) {
01433       _pitch += pitch;
01434       if( _pitch > 90 )
01435           _pitch = 90;
01436       else if( _pitch < 0 )
01437           _pitch = 0;
01438     }
01439   
01440      void setScale( float scale ) { _scale = scale; }
01441      void setPose( float x, float y) { _x = x; _y = y; }
01442   
01443     void scale( float scale, float shift_x = 0, float h = 0, float shift_y = 0, float w = 0 );  
01444      void reset( void ) { _pitch = _yaw = 0; }
01445   
01446      float scale() const { return _scale; }
01447   
01448     void Load( Worldfile* wf, int sec );
01449     void Save( Worldfile* wf, int sec );
01450   };
01451 
01452 
01456   class WorldGui : public World, public Fl_Window 
01457   {
01458     friend class Canvas;
01459     friend class ModelCamera;
01460     friend class Model;
01461     friend class Option;
01462 
01463   private:
01464 
01465     Canvas* canvas;
01466     std::vector<Option*> drawOptions;
01467     FileManager* fileMan; 
01468      std::vector<usec_t> interval_log;
01469      
01472      float speedup; 
01473 
01474     Fl_Menu_Bar* mbar;
01475     OptionsDlg* oDlg;
01476     bool pause_time;
01477 
01480     usec_t real_time_interval;
01481      
01483     usec_t real_time_now; 
01484 
01487     usec_t real_time_recorded;
01488      
01490      uint64_t timing_interval;
01491 
01492     // static callback functions
01493     static void windowCb( Fl_Widget* w, WorldGui* wg ); 
01494     static void fileLoadCb( Fl_Widget* w, WorldGui* wg );
01495     static void fileSaveCb( Fl_Widget* w, WorldGui* wg );
01496     static void fileSaveAsCb( Fl_Widget* w, WorldGui* wg );
01497     static void fileExitCb( Fl_Widget* w, WorldGui* wg );
01498     static void viewOptionsCb( OptionsDlg* oDlg, WorldGui* wg );
01499     static void optionsDlgCb( OptionsDlg* oDlg, WorldGui* wg );
01500     static void helpAboutCb( Fl_Widget* w, WorldGui* wg );
01501     static void pauseCb( Fl_Widget* w, WorldGui* wg );
01502     static void onceCb( Fl_Widget* w, WorldGui* wg );
01503     static void fasterCb( Fl_Widget* w, WorldGui* wg );
01504     static void slowerCb( Fl_Widget* w, WorldGui* wg );
01505     static void realtimeCb( Fl_Widget* w, WorldGui* wg );
01506     static void fasttimeCb( Fl_Widget* w, WorldGui* wg );
01507     static void resetViewCb( Fl_Widget* w, WorldGui* wg );
01508     static void moreHelptCb( Fl_Widget* w, WorldGui* wg );
01509     
01510     // GUI functions
01511     bool saveAsDialog();
01512     bool closeWindowQuery();
01513     
01514     virtual void AddModel( Model* mod );
01515     
01516      void SetTimeouts();
01517 
01518   protected:
01519     
01520     virtual void PushColor( Color col );
01521     virtual void PushColor( double r, double g, double b, double a );
01522     virtual void PopColor();
01523     
01524     void DrawOccupancy() const;
01525     void DrawVoxels() const;
01526      
01527   public:
01528     
01529     WorldGui(int W,int H,const char*L=0);
01530     ~WorldGui();
01531 
01533         virtual void Redraw( void );
01534 
01535     virtual std::string ClockString() const;
01536     virtual bool Update();  
01537     virtual void Load( const std::string& filename );
01538     virtual void UnLoad();
01539     virtual bool Save( const char* filename );  
01540     virtual bool IsGUI() const { return true; };    
01541     virtual Model* RecentlySelectedModel() const;
01542 
01543     virtual void Start();
01544     virtual void Stop();
01545     
01546     usec_t RealTimeNow(void) const;
01547  
01548     void DrawBoundingBoxTree();
01549     
01550     Canvas* GetCanvas( void ) const { return canvas; } 
01551 
01553     void Show(); 
01554 
01556     std::string EnergyString( void ) const; 
01557     virtual void RemoveChild( Model* mod );  
01558   };
01559 
01560 
01561   class StripPlotVis : public Visualizer
01562   {
01563   private:
01564      
01565      Model* mod;
01566      float* data;
01567      size_t len;
01568      size_t count;
01569      unsigned int index;
01570      float x,y,w,h,min,max;
01571      Color fgcolor, bgcolor;
01572      
01573   public:
01574      StripPlotVis( float x, float y, float w, float h, 
01575                         size_t len, 
01576                         Color fgcolor, Color bgcolor,
01577                         const char* name, const char* wfname );
01578      virtual ~StripPlotVis();
01579      virtual void Visualize( Model* mod, Camera* cam );     
01580      void AppendValue( float value );
01581   };
01582 
01583 
01584   class PowerPack
01585   {
01586      friend class WorldGui;
01587      friend class Canvas;
01588      
01589   protected:
01590      
01591      class DissipationVis : public Visualizer
01592      {
01593      private:
01594         unsigned int columns, rows;
01595         meters_t width, height;
01596          
01597          std::vector<joules_t> cells;
01598          
01599          joules_t peak_value;
01600          double cellsize;
01601          
01602         static joules_t global_peak_value; 
01603 
01604      public:
01605         DissipationVis( meters_t width, 
01606                              meters_t height, 
01607                              meters_t cellsize );
01608 
01609         virtual ~DissipationVis();
01610         virtual void Visualize( Model* mod, Camera* cam );      
01611         
01612         void Accumulate( meters_t x, meters_t y, joules_t amount );
01613      } event_vis;
01614      
01615 
01616      StripPlotVis output_vis;
01617      StripPlotVis stored_vis;
01618 
01620      Model* mod;
01621     
01623      joules_t stored;
01624      
01626      joules_t capacity;
01627      
01629      bool charging;
01630      
01632      joules_t dissipated;
01633      
01634      // these are used to visualize the power draw
01635      usec_t last_time;
01636      joules_t last_joules;
01637      watts_t last_watts;
01638 
01639   public:
01640      static joules_t global_stored;
01641      static joules_t global_capacity;
01642      static joules_t global_dissipated;  
01643      static joules_t global_input;
01644 
01645   public:
01646      PowerPack( Model* mod );
01647      ~PowerPack();
01648      
01650      void Visualize( Camera* cam );
01651 
01653      joules_t RemainingCapacity() const;
01654      
01656      void Add( joules_t j );
01657         
01659      void Subtract( joules_t j );
01660         
01662      void TransferTo( PowerPack* dest, joules_t amount );    
01663 
01664      double ProportionRemaining() const
01665      { return( stored / capacity ); }
01666 
01669      void Print( const char* prefix ) const
01670      { 
01671         if( prefix )
01672           printf( "%s", prefix );
01673 
01674         printf( "PowerPack %.2f/%.2f J\n", stored, capacity ); 
01675      }      
01676      
01677      joules_t GetStored() const;
01678      joules_t GetCapacity() const;
01679      joules_t GetDissipated() const;
01680      void SetCapacity( joules_t j );
01681      void SetStored( joules_t j );  
01682 
01684      bool GetCharging() const { return charging; }
01685      
01686      void ChargeStart(){ charging = true; }
01687      void ChargeStop(){ charging = false; }
01688 
01690      void Dissipate( joules_t j );
01691      
01693      void Dissipate( joules_t j, const Pose& p );
01694   };
01695 
01696    
01698   class Model : public Ancestor
01699   {
01700     friend class Ancestor;
01701     friend class World;
01702     friend class World::Event;
01703     friend class WorldGui;
01704     friend class Canvas;
01705     friend class Block;
01706     friend class Region;
01707     friend class BlockGroup;
01708     friend class PowerPack;
01709     friend class Ray;
01710         friend class ModelFiducial;
01711 
01712   private:
01714      static uint32_t count;
01715      static std::map<id_t,Model*> modelsbyid;
01716      std::vector<Option*> drawOptions;
01717      const std::vector<Option*>& getOptions() const { return drawOptions; }
01718      
01719   protected:
01720 
01723      bool alwayson;
01724 
01726      // TODO bool background;
01727 
01728      BlockGroup blockgroup;
01730      int blocks_dl;
01731 
01735      int boundary;
01736         
01739   public:
01740      class cb_t
01741      {
01742      public:
01743         model_callback_t callback;
01744         void* arg;
01745             
01746         cb_t( model_callback_t cb, void* arg ) 
01747           : callback(cb), arg(arg) {}
01748             
01749         cb_t( world_callback_t cb, void* arg ) 
01750           : callback(NULL), arg(arg) { (void)cb; }
01751             
01752         cb_t() : callback(NULL), arg(NULL) {}
01753             
01755          bool operator<( const cb_t& other ) const
01756          {
01757              if( callback == other.callback )
01758                  return( arg < other.arg );
01759              //else
01760              return ((void*)(callback)) < ((void*)(other.callback)); 
01761          }
01762          
01764          bool operator==( const cb_t& other ) const
01765          { return( callback == other.callback);  }          
01766      };
01767         
01768      class Flag
01769      {
01770      private:
01771         Color color;
01772         double size;
01773         int displaylist;
01774 
01775      public:
01776          void SetColor( const Color& col );
01777          void SetSize( double sz );
01778          
01779          Color GetColor(){ return color; }
01780          double GetSize(){ return size; }
01781          
01782         Flag( Color color, double size );
01783         Flag* Nibble( double portion );
01784         
01787         void Draw(  GLUquadric* quadric );
01788      };
01789 
01790         typedef enum {
01791             CB_FLAGDECR,
01792             CB_FLAGINCR,
01793             CB_GEOM,
01794             CB_INIT,
01795             CB_LOAD,
01796             CB_PARENT,
01797             CB_POSE,
01798             CB_SAVE,
01799             CB_SHUTDOWN,
01800             CB_STARTUP,
01801             CB_UPDATE,
01802             CB_VELOCITY,
01803             //CB_POSTUPDATE,
01804             __CB_TYPE_COUNT // must be the last entry: counts the number of types
01805         } callback_type_t;
01806         
01807   protected:
01811         std::vector<std::set<cb_t> > callbacks;
01812         
01813 
01815      Color color;
01816         
01819      double friction;
01820         
01824      bool data_fresh;
01825 
01829         bool disabled; 
01830 
01832      std::list<Visualizer*> cv_list;
01833 
01835      std::list<Flag*> flag_list;
01836         
01839         Geom geom;
01840 
01842      class GuiState
01843      {
01844      public:
01845         bool grid;
01846         bool move;
01847         bool nose;
01848         bool outline;
01849         
01850         GuiState();
01851         void Load( Worldfile* wf, int wf_entity );
01852      } gui;
01853      
01854      bool has_default_block;
01855                 
01856   
01858      uint32_t id;   
01859      usec_t interval; 
01860      usec_t interval_energy; 
01861      usec_t interval_pose; 
01862 
01863      usec_t last_update; 
01864      bool log_state; 
01865      meters_t map_resolution;
01866      kg_t mass;
01867 
01869      Model* parent; 
01870 
01873      Pose pose;
01874 
01876      PowerPack* power_pack;
01877 
01880      std::list<PowerPack*> pps_charging;
01881         
01883      class RasterVis : public Visualizer
01884      {
01885      private:
01886         uint8_t* data;
01887         unsigned int width, height;
01888         meters_t cellwidth, cellheight;
01889         std::vector<point_t> pts;
01890       
01891      public:
01892         RasterVis();
01893         virtual ~RasterVis( void ){}
01894         virtual void Visualize( Model* mod, Camera* cam );
01895       
01896         void SetData( uint8_t* data, 
01897                           unsigned int width, 
01898                           unsigned int height,
01899                           meters_t cellwidth,
01900                           meters_t cellheight );
01901       
01902         int subs;     //< the number of subscriptions to this model
01903         int used;     //< the number of connections to this model
01904       
01905         void AddPoint( meters_t x, meters_t y );
01906         void ClearPts();
01907       
01908      } rastervis;
01909      
01910      bool rebuild_displaylist; 
01911      std::string say_string;   
01912         
01913     bool stack_children; 
01914 
01915         bool stall; 
01916      int subs;    
01917 
01921      bool thread_safe;
01922      
01924      class TrailItem 
01925      {                                                                                          
01926      public:
01927         usec_t time;
01928         Pose pose;
01929         Color color;
01930         
01931         TrailItem() 
01932           : time(0), pose(), color(){}
01933          
01934          //TrailItem( usec_t time, Pose pose, Color color ) 
01935          //: time(time), pose(pose), color(color){}
01936      };
01937     
01939         std::vector<TrailItem> trail;
01940 
01942         unsigned int trail_index;
01943 
01947      static unsigned int trail_length;
01948      
01950      static uint64_t trail_interval;
01951      
01953         void UpdateTrail();
01954 
01955      //model_type_t type;  
01956      const std::string type;
01959         unsigned int event_queue_num; 
01960         bool used;   
01961         Velocity velocity;
01962         
01966         bool velocity_enable;
01967         
01968         watts_t watts;
01969      
01972      watts_t watts_give;
01973     
01976      watts_t watts_take;
01977     
01978      Worldfile* wf;
01979      int wf_entity;
01980      World* world; // pointer to the world in which this model exists
01981      WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode
01982 
01983   public:
01984      
01985      const std::string& GetModelType() const {return type;}  
01986      std::string GetSayString(){return std::string(say_string);}
01987      
01990     Model* GetChild( const std::string& name ) const;
01991 
01993         usec_t GetInterval(){ return interval; }
01994         
01995      class Visibility
01996      {
01997      public:
01998          bool blob_return;
01999          int fiducial_key;
02000          int fiducial_return;
02001          bool gripper_return;
02002          bool obstacle_return;
02003          float ranger_return; // 0 - 1
02004         
02005         Visibility();
02006 
02007         void Load( Worldfile* wf, int wf_entity );
02008         void Save( Worldfile* wf, int wf_entity );
02009      } vis;
02010      
02011      usec_t GetUpdateInterval() const { return interval; }
02012      usec_t GetEnergyInterval() const { return interval_energy; }
02013      usec_t GetPoseInterval() const { return interval_pose; }
02014      
02017      void Rasterize( uint8_t* data, 
02018                           unsigned int width, unsigned int height,
02019                           meters_t cellwidth, meters_t cellheight );
02020     
02021   private: 
02024      explicit Model(const Model& original);
02025 
02028      Model& operator=(const Model& original);
02029 
02030   protected:
02031 
02033      void RegisterOption( Option* opt );
02034 
02035      void AppendTouchingModels( ModelPtrSet& touchers );
02036 
02040      Model* TestCollision();
02041 
02044     Model* TestCollisionTree();
02045   
02046      void CommitTestedPose();
02047 
02048      void Map();
02049      void UnMap();
02050 
02051      void MapWithChildren();
02052      void UnMapWithChildren();
02053   
02054      // Find the root model, and map/unmap the whole tree.
02055      void MapFromRoot();
02056      void UnMapFromRoot();
02057 
02060      RaytraceResult Raytrace( const Pose &pose,
02061                                                 const meters_t range, 
02062                                                 const ray_test_func_t func,
02063                                                 const void* arg,
02064                                                 const bool ztest = true );
02065   
02068      void Raytrace( const Pose &pose,
02069                          const meters_t range, 
02070                          const radians_t fov, 
02071                          const ray_test_func_t func,
02072                          const void* arg,
02073                          RaytraceResult* samples,
02074                          const uint32_t sample_count,
02075                          const bool ztest = true  );
02076   
02077      RaytraceResult Raytrace( const radians_t bearing,           
02078                                                 const meters_t range,
02079                                                 const ray_test_func_t func,
02080                                                 const void* arg,
02081                                                 const bool ztest = true );
02082   
02083      void Raytrace( const radians_t bearing,             
02084                          const meters_t range,
02085                          const radians_t fov,
02086                          const ray_test_func_t func,
02087                          const void* arg,
02088                          RaytraceResult* samples,
02089                          const uint32_t sample_count,
02090                          const bool ztest = true );
02091   
02092 
02096      //void GPoseDirtyTree();
02097 
02098      virtual void Startup();
02099      virtual void Shutdown();
02100      virtual void Update();
02101      virtual void UpdatePose();
02102      virtual void UpdateCharge();
02103 
02104      Model* ConditionalMove( const Pose& newpose );
02105 
02106      meters_t ModelHeight() const;
02107 
02108      void DrawBlocksTree();
02109      virtual void DrawBlocks();
02110      void DrawBoundingBox();
02111      void DrawBoundingBoxTree();
02112      virtual void DrawStatus( Camera* cam );
02113      void DrawStatusTree( Camera* cam );
02114   
02115      void DrawOriginTree();
02116      void DrawOrigin();
02117   
02118      void PushLocalCoords();
02119      void PopCoords();
02120   
02122      void DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width=1.0, double height=1.0 );
02123     
02124      virtual void DrawPicker();
02125      virtual void DataVisualize( Camera* cam );  
02126      virtual void DrawSelected(void);
02127     
02128      void DrawTrailFootprint();
02129      void DrawTrailBlocks();
02130      void DrawTrailArrows();
02131      void DrawGrid();
02132      // void DrawBlinkenlights();
02133      void DataVisualizeTree( Camera* cam );
02134      void DrawFlagList();
02135      void DrawPose( Pose pose );
02136     
02137   public:
02138      virtual void PushColor( Color col ){ world->PushColor( col ); }    
02139      virtual void PushColor( double r, double g, double b, double a ){ world->PushColor( r,g,b,a ); }   
02140      virtual void PopColor()    { world->PopColor(); }
02141     
02142      PowerPack* FindPowerPack() const;
02143     
02144      //void RecordRenderPoint( GSList** head, GSList* link, 
02145      //                 unsigned int* c1, unsigned int* c2 );
02146 
02147      void PlaceInFreeSpace( meters_t xmin, meters_t xmax, 
02148                                     meters_t ymin, meters_t ymax );
02149     
02151      std::string PoseString()
02152      { return pose.String(); }
02153     
02155      static Model* LookupId( uint32_t id )
02156      { return modelsbyid[id]; }
02157      
02159      Model( World* world, 
02160               Model* parent = NULL, 
02161               const std::string& type = "model" );
02162      
02164      virtual ~Model();
02165         
02167         Model() 
02168             : parent(NULL), world(NULL) 
02169         {}
02170         
02171      void Say( const std::string& str );
02172      
02174      void AddVisualizer( Visualizer* custom_visual, bool on_by_default );
02175 
02177      void RemoveVisualizer( Visualizer* custom_visual );
02178 
02179      void BecomeParentOf( Model* child );
02180 
02181      void Load( Worldfile* wf, int wf_entity )
02182      {
02185         SetWorldfile( wf, wf_entity );
02186         Load(); // call virtual load
02187      }
02188     
02190      void SetWorldfile( Worldfile* wf, int wf_entity )
02191      { this->wf = wf; this->wf_entity = wf_entity; }
02192     
02194      virtual void Load();
02195     
02197      virtual void Save();
02198     
02200      void InitControllers();
02201 
02202      void AddFlag(  Flag* flag );
02203      void RemoveFlag( Flag* flag );
02204     
02205      void PushFlag( Flag* flag );
02206      Flag* PopFlag();
02207     
02208      unsigned int GetFlagCount() const { return flag_list.size(); }
02209     
02214      void Disable(){ disabled = true; };
02215 
02218      void Enable(){ disabled = false; };
02219   
02222      void LoadControllerModule( const char* lib );
02223     
02226      void NeedRedraw();
02227         
02229         void Redraw();
02230 
02233      void LoadBlock( Worldfile* wf, int entity );
02234 
02237      Block* AddBlockRect( meters_t x, meters_t y, 
02238                                  meters_t dx, meters_t dy, 
02239                                  meters_t dz );
02240     
02242      void ClearBlocks();
02243   
02246      Model* Parent() const { return this->parent; }
02247 
02249      World* GetWorld() const { return this->world; }
02250   
02252      Model* Root(){ return(  parent ? parent->Root() : this ); }
02253   
02254      bool IsAntecedent( const Model* testmod ) const;
02255     
02257      bool IsDescendent( const Model* testmod ) const;
02258     
02260      bool IsRelated( const Model* testmod ) const;
02261 
02263      Pose GetGlobalPose() const;
02264     
02266      Velocity GetGlobalVelocity()  const;
02267     
02268      /* set the velocity of a model in the global coordinate system */
02269      void SetGlobalVelocity( const Velocity& gvel );
02270     
02272      void Subscribe();
02273     
02275      void Unsubscribe();
02276     
02278      void SetGlobalPose(  const Pose& gpose );
02279     
02281      void SetVelocity(  const Velocity& vel );
02282     
02284         void VelocityEnable();
02285 
02287         void VelocityDisable();
02288 
02290      void SetPose(  const Pose& pose );
02291     
02293      void AddToPose(  const Pose& pose );
02294     
02296      void AddToPose(  double dx, double dy, double dz, double da );
02297     
02299      void SetGeom(  const Geom& src );
02300   
02303      void SetFiducialReturn(  int fid );
02304   
02306      int GetFiducialReturn()  const { return vis.fiducial_return; }
02307   
02310      void SetFiducialKey(  int key );
02311     
02312      Color GetColor() const { return color; }
02313      
02315      uint32_t GetId()  const { return id; }
02316      
02318      kg_t GetTotalMass() const;
02319      
02321      kg_t GetMassOfChildren() const;
02322 
02324      int SetParent( Model* newparent);
02325     
02328      Geom GetGeom() const { return geom; }
02329     
02332      Pose GetPose() const { return pose; }
02333     
02336      Velocity GetVelocity() const { return velocity; }
02337     
02338      // guess what these do?
02339      void SetColor( Color col );
02340      void SetMass( kg_t mass );
02341      void SetStall( bool stall );
02342      void SetGravityReturn( bool val );
02343      void SetGripperReturn( bool val );
02344      void SetStickyReturn( bool val );
02345      void SetRangerReturn( float val );
02346      void SetObstacleReturn( bool val );
02347      void SetBlobReturn( bool val );
02348      void SetRangerReturn( bool val );
02349      void SetBoundary( bool val );
02350      void SetGuiNose( bool val );
02351      void SetGuiMove( bool val );
02352      void SetGuiGrid( bool val );
02353      void SetGuiOutline( bool val );
02354      void SetWatts( watts_t watts );
02355      void SetMapResolution( meters_t res );
02356      void SetFriction( double friction );
02357     
02358      bool DataIsFresh() const { return this->data_fresh; }
02359         
02360         /* attach callback functions to data members. The function gets
02361              called when the member is changed using SetX() accessor method */
02362         
02369         void AddCallback( callback_type_t type, 
02370                                             model_callback_t cb, 
02371                                             void* user );
02372         
02373         int RemoveCallback( callback_type_t type,
02374                                                 model_callback_t callback );
02375         
02376         int CallCallbacks(  callback_type_t type );
02377         
02378         
02379      virtual void Print( char* prefix ) const;
02380      virtual const char* PrintWithPose() const;
02381     
02384      Pose GlobalToLocal( const Pose& pose ) const;
02385      
02388      Pose LocalToGlobal( const Pose& pose ) const
02389      {  
02390         return( ( GetGlobalPose() + geom.pose ) + pose );
02391      }
02392         
02394         void LocalToPixels( const std::vector<point_t>& local,
02395                                                 std::vector<point_int_t>& pixels) const;
02396         
02399      point_t LocalToGlobal( const point_t& pt) const;       
02400 
02403      Model* GetUnsubscribedModelOfType( const std::string& type ) const;
02404     
02407      Model* GetUnusedModelOfType( const std::string& type );
02408   
02411      bool Stalled() const { return this->stall; }
02412      
02415      unsigned int GetSubscriptionCount() const { return subs; }
02416 
02418      bool HasSubscribers() const { return( subs > 0 ); }     
02419 
02420      static std::map< std::string, creator_t> name_map;  
02421 
02422 //      class Neighbors
02423 //      {
02424 //          Model *left, *right, *up, *down;
02425 //      public:
02426 //          Neighbors() : left(NULL), right(NULL), up(NULL), down(NULL) {}
02427 //      } nbors; // instance
02428 
02429                             
02430   };
02431 
02432 
02433   // BLOBFINDER MODEL --------------------------------------------------------
02434 
02435 
02437   class ModelBlobfinder : public Model
02438   {
02439   public:
02441      class Blob
02442      {
02443      public:
02444         Color color;
02445         uint32_t left, top, right, bottom;
02446         meters_t range;
02447      };
02448 
02449      class Vis : public Visualizer 
02450      {
02451      private:
02452         //static Option showArea;
02453      public:
02454         Vis( World* world );
02455         virtual ~Vis( void ){}
02456         virtual void Visualize( Model* mod, Camera* cam );
02457      } vis;
02458 
02459   private:
02460      std::vector<Blob> blobs;
02461      std::vector<Color> colors;
02462 
02463      // predicate for ray tracing
02464      static bool BlockMatcher( Block* testblock, Model* finder );
02465 
02466   public:
02467      radians_t fov;
02468      radians_t pan;
02469      meters_t range;
02470      unsigned int scan_height;
02471      unsigned int scan_width;
02472      
02473      // constructor
02474      ModelBlobfinder( World* world,
02475                             Model* parent,
02476                             const std::string& type );
02477      // destructor
02478      ~ModelBlobfinder();
02479     
02480      virtual void Startup();
02481      virtual void Shutdown();
02482      virtual void Update();
02483      virtual void Load();
02484         
02485      Blob* GetBlobs( unsigned int* count )
02486      { 
02487         if( count ) *count = blobs.size();
02488         return &blobs[0];
02489      }
02490 
02491      std::vector<Blob> GetBlobs() const { return blobs; }
02492 
02494      void AddColor( Color col );
02495 
02497      void RemoveColor( Color col );
02498 
02501      void RemoveAllColors();
02502   };
02503 
02504 
02505 
02506   
02507 
02508   // Light indicator model
02509   class ModelLightIndicator : public Model
02510   {
02511   public:
02512      ModelLightIndicator( World* world, 
02513                                  Model* parent,
02514                                  const std::string& type );
02515      ~ModelLightIndicator();
02516   
02517      void SetState(bool isOn);
02518 
02519   protected:
02520      virtual void DrawBlocks();
02521 
02522   private:
02523      bool m_IsOn;
02524   };
02525 
02526   // \todo  GRIPPER MODEL --------------------------------------------------------
02527 
02528 
02529   class ModelGripper : public Model
02530   {
02531   public:
02532 
02533      enum paddle_state_t {
02534         PADDLE_OPEN = 0, // default state
02535         PADDLE_CLOSED, 
02536         PADDLE_OPENING,
02537         PADDLE_CLOSING,
02538      };
02539      
02540      enum lift_state_t {
02541         LIFT_DOWN = 0, // default state
02542         LIFT_UP, 
02543         LIFT_UPPING, // verbed these to match the paddle state
02544         LIFT_DOWNING, 
02545      };
02546      
02547      enum cmd_t {
02548         CMD_NOOP = 0, // default state
02549         CMD_OPEN, 
02550         CMD_CLOSE,
02551         CMD_UP, 
02552         CMD_DOWN    
02553      };
02554      
02555      
02558      struct config_t
02559      {
02560         Size paddle_size; 
02561         paddle_state_t paddles;
02562         lift_state_t lift;      
02563         double paddle_position; 
02564         double lift_position; 
02565         Model* gripped;
02566         bool paddles_stalled; // true iff some solid object stopped the paddles closing or opening
02567         double close_limit; 
02568         bool autosnatch; 
02569         double break_beam_inset[2]; 
02570       Model* beam[2]; 
02571       Model* contact[2]; 
02572      };
02573      
02574   private:
02575      virtual void Update();
02576      virtual void DataVisualize( Camera* cam );
02577      
02578      void FixBlocks();
02579      void PositionPaddles();
02580      void UpdateBreakBeams();
02581      void UpdateContacts();
02582 
02583      config_t cfg;
02584      cmd_t cmd;
02585      
02586      Block* paddle_left;
02587      Block* paddle_right;
02588 
02589      static Option showData;
02590 
02591   public:    
02592      static const Size size;
02593 
02594      // constructor
02595      ModelGripper( World* world,
02596                         Model* parent,
02597                         const std::string& type );
02598      // destructor
02599      virtual ~ModelGripper();
02600   
02601      virtual void Load();
02602      virtual void Save();
02603 
02605      void SetConfig( config_t & newcfg ){ this->cfg = newcfg; FixBlocks(); }
02606      
02608      config_t GetConfig(){ return cfg; };
02609      
02611      void SetCommand( cmd_t cmd ) { this->cmd = cmd; }
02613      void CommandClose() { SetCommand( CMD_CLOSE ); }
02615      void CommandOpen() { SetCommand( CMD_OPEN ); }
02617      void CommandUp() { SetCommand( CMD_UP ); }
02619      void CommandDown() { SetCommand( CMD_DOWN ); }
02620   };
02621 
02622 
02623   // \todo BUMPER MODEL --------------------------------------------------------
02624 
02625   //   typedef struct
02626   //   {
02627   //     Pose pose;
02628   //     meters_t length;
02629   //   } bumper_config_t;
02630 
02631   //   typedef struct
02632   //   {
02633   //     Model* hit;
02634   //     point_t hit_point;
02635   //   } bumper_sample_t;
02636 
02637 
02638   // FIDUCIAL MODEL --------------------------------------------------------
02639 
02641   class ModelFiducial : public Model
02642   {
02643   public:  
02645      class Fiducial
02646      {
02647      public:
02648         meters_t range; 
02649         radians_t bearing; 
02650         Pose geom; 
02651          //Pose pose_rel; /// relative pose of the target in local coordinates       
02652         Pose pose; 
02653         Model* mod; 
02654         int id; 
02655      };
02656 
02657   private:
02658      // if neighbor is visible, add him to the fiducial scan
02659      void AddModelIfVisible( Model* him );
02660 
02661      virtual void Update();
02662      virtual void DataVisualize( Camera* cam );
02663 
02664      static Option showData;
02665      static Option showFov;
02666      
02667      std::vector<Fiducial> fiducials;
02668         
02669   public:       
02670      ModelFiducial( World* world, 
02671                          Model* parent,
02672                          const std::string& type );
02673      virtual ~ModelFiducial();
02674     
02675      virtual void Load();
02676      void Shutdown( void );
02677 
02678      meters_t max_range_anon;
02679      meters_t max_range_id; 
02680      meters_t min_range; 
02681      radians_t fov; 
02682      radians_t heading; 
02683      int key; 
02684     bool ignore_zloc;  
02685         
02687      std::vector<Fiducial>& GetFiducials() { return fiducials; }
02688         
02690      Fiducial* GetFiducials( unsigned int* count )
02691      {
02692         if( count ) *count = fiducials.size();
02693         return &fiducials[0];
02694      }
02695   };
02696     
02697     
02698   // RANGER MODEL --------------------------------------------------------
02699     
02701   class ModelRanger : public Model
02702   {
02703   public:
02704   public:
02705         ModelRanger( World* world, Model* parent,
02706                                  const std::string& type );
02707         virtual ~ModelRanger();
02708         
02709         virtual void Load();
02710         virtual void Print( char* prefix ) const;
02711         
02712         class Vis : public Visualizer 
02713         {
02714         public:
02715             static Option showArea;
02716             static Option showStrikes;
02717             static Option showFov;
02718             static Option showBeams;
02719             static Option showTransducers;      
02720             
02721             Vis( World* world );        
02722           virtual ~Vis( void ){} 
02723             virtual void Visualize( Model* mod, Camera* cam );
02724         } vis;
02725         
02726         class Sensor
02727         {       
02728         public:
02729             Pose pose; 
02730             Size size;
02731             Bounds range;
02732             radians_t fov;
02733             unsigned int sample_count;
02734             Color col;
02735             
02736             std::vector<meters_t> ranges;
02737             std::vector<double> intensities;
02738             
02739             Sensor() : pose( 0,0,0,0 ), 
02740                                  size( 0.02, 0.02, 0.02 ), // teeny transducer
02741                                  range( 0.0, 5.0 ),
02742                                  fov( 0.1 ), 
02743                                  sample_count(1),
02744                                  col( 0,1,0,0.3 ),
02745                                  ranges(),
02746                                  intensities()
02747             {}
02748             
02749             void Update( ModelRanger* rgr );            
02750             void Visualize( Vis* vis, ModelRanger* rgr ) const;
02751             std::string String() const;         
02752             void Load( Worldfile* wf, int entity );
02753         };
02754 
02756      const std::vector<Sensor>& GetSensors() const
02757      { return sensors; }
02758      
02761      const std::vector<meters_t>& GetRanges( unsigned int sensor=0) const 
02762      { 
02763         if( sensor < sensors.size() )
02764           return sensors[sensor].ranges;
02765         
02766         PRINT_ERR1( "invalid sensor index specified (%d)", sensor );
02767         exit(-1);
02768      }
02769 
02772      std::vector<meters_t>& GetRangesMutable( unsigned int sensor=0) 
02773      { 
02774         if( sensor < sensors.size() )
02775           return sensors[sensor].ranges;
02776         
02777         PRINT_ERR1( "invalid sensor index specified (%d)", sensor );
02778         exit(-1);
02779      }
02780         
02783         meters_t* GetRangesArr( unsigned int sensor, uint32_t* count )
02784         {
02785             assert(count);
02786             *count = sensors[sensor].ranges.size();
02787             return &sensors[sensor].ranges[0];
02788         }
02789 
02792         meters_t* GetIntensitiesArr( unsigned int sensor, uint32_t* count )
02793         {
02794             assert(count);
02795             *count = sensors[sensor].intensities.size();
02796             return &sensors[sensor].intensities[0];
02797         }
02798         
02801      const std::vector<double>& GetIntensities( unsigned int sensor=0) const 
02802      { 
02803         if( sensor < sensors.size() )
02804           return sensors[sensor].intensities;
02805         
02806         PRINT_ERR1( "invalid sensor index specified (%d)", sensor );
02807         exit(-1);
02808      }
02809      
02810         void LoadSensor( Worldfile* wf, int entity );
02811         
02812   private:
02813         std::vector<Sensor> sensors;        
02814         
02815   protected:
02816         
02817         virtual void Startup();
02818         virtual void Shutdown();
02819         virtual void Update();      
02820   };
02821     
02822   // BLINKENLIGHT MODEL ----------------------------------------------------
02823   class ModelBlinkenlight : public Model
02824   {
02825   private:
02826      double dutycycle;
02827      bool enabled;
02828      msec_t period;
02829      bool on;
02830 
02831      static Option showBlinkenData;
02832   public:
02833      ModelBlinkenlight( World* world,
02834                               Model* parent,
02835                               const std::string& type );
02836 
02837      ~ModelBlinkenlight();
02838     
02839      virtual void Load();
02840      virtual void Update();
02841      virtual void DataVisualize( Camera* cam );
02842   };
02843 
02844     
02845   // CAMERA MODEL ----------------------------------------------------
02846 
02848   class ModelCamera : public Model
02849   {
02850   public:
02851      typedef struct 
02852      {
02853         // GL_V3F
02854         GLfloat x, y, z;
02855      } ColoredVertex;
02856   
02857   private:
02858      Canvas* _canvas;
02859 
02860      GLfloat* _frame_data;  //opengl read buffer
02861      GLubyte* _frame_color_data;  //opengl read buffer
02862 
02863      bool _valid_vertexbuf_cache;
02864      ColoredVertex* _vertexbuf_cache; //cached unit vectors with appropriate rotations (these must be scalled by z-buffer length)
02865     
02866      int _width;         //width of buffer
02867      int _height;        //height of buffer
02868      static const int _depth = 4;
02869     
02870      int _camera_quads_size;
02871      GLfloat* _camera_quads;
02872      GLubyte* _camera_colors;
02873     
02874      static Option showCameraData;
02875     
02876      PerspectiveCamera _camera;
02877      float _yaw_offset; //position camera is mounted at
02878      float _pitch_offset;
02879         
02881      bool GetFrame();
02882     
02883   public:
02884      ModelCamera( World* world,
02885                       Model* parent,
02886                       const std::string& type ); 
02887       
02888      ~ModelCamera();
02889   
02890      virtual void Load();
02891     
02893      virtual void Update();
02894     
02896      //virtual void Draw( uint32_t flags, Canvas* canvas );
02897     
02899      virtual void DataVisualize( Camera* cam );
02900     
02902      int getWidth( void ) const { return _width; }
02903     
02905      int getHeight( void ) const { return _height; }
02906     
02908      const PerspectiveCamera& getCamera( void ) const { return _camera; }
02909     
02911      const GLfloat* FrameDepth() const { return _frame_data; }
02912     
02914      const GLubyte* FrameColor() const { return _frame_color_data; }
02915     
02917      void setPitch( float pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; }
02918     
02920      void setYaw( float yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; }
02921   };
02922 
02923   // POSITION MODEL --------------------------------------------------------
02924 
02926   class ModelPosition : public Model
02927   {
02928      friend class Canvas;
02929 
02930   public:
02932      typedef enum
02933         { CONTROL_VELOCITY, 
02934           CONTROL_POSITION 
02935         } ControlMode;
02936      
02938      typedef enum
02939         { LOCALIZATION_GPS, 
02940           LOCALIZATION_ODOM 
02941         } LocalizationMode;
02942      
02944      typedef enum
02945         { DRIVE_DIFFERENTIAL, 
02946           DRIVE_OMNI, 
02947           DRIVE_CAR 
02948         } DriveMode;
02949      
02950   private:
02951      Pose goal;
02952      ControlMode control_mode;
02953      DriveMode drive_mode;   
02954      LocalizationMode localization_mode; 
02955      Velocity integration_error; 
02956      double wheelbase;
02957      
02958   public:
02959      // constructor
02960      ModelPosition( World* world,
02961                          Model* parent,
02962                          const std::string& type );
02963      // destructor
02964      ~ModelPosition();
02965 
02966      virtual void Startup();
02967      virtual void Shutdown();
02968      virtual void Update();
02969      virtual void Load();
02970         
02973      class Waypoint
02974      {
02975      public:
02976         Waypoint( meters_t x, meters_t y, meters_t z, radians_t a, Color color ) ;
02977         Waypoint( const Pose& pose, Color color ) ;
02978         Waypoint();
02979         void Draw() const;
02980         
02981         Pose pose;
02982         Color color;
02983      };
02984      
02985      std::vector<Waypoint> waypoints;
02986 
02987      class WaypointVis : public Visualizer
02988      {
02989      public:
02990         WaypointVis();
02991         virtual ~WaypointVis( void ){}
02992         virtual void Visualize( Model* mod, Camera* cam );
02993      } wpvis;
02994      
02995      class PoseVis : public Visualizer
02996      {
02997      public:
02998         PoseVis();
02999         virtual ~PoseVis( void ){}
03000         virtual void Visualize( Model* mod, Camera* cam );
03001      } posevis;
03002 
03004      void SetOdom( Pose odom );
03005         
03008      void SetSpeed( double x, double y, double a );
03009      void SetXSpeed( double x );
03010      void SetYSpeed( double y );
03011      void SetZSpeed( double z );
03012      void SetTurnSpeed( double a );
03013      void SetSpeed( Velocity vel );
03015      void Stop();
03016 
03019      void GoTo( double x, double y, double a );
03020      void GoTo( Pose pose );
03021 
03022      // localization state
03023      Pose est_pose; 
03024      Pose est_pose_error; 
03025      Pose est_origin; 
03026   };
03027 
03028 
03029   // ACTUATOR MODEL --------------------------------------------------------
03030 
03032   class ModelActuator : public Model
03033   {
03034   public:
03036      typedef enum
03037         { CONTROL_VELOCITY,
03038           CONTROL_POSITION
03039         } ControlMode;
03040   
03042      typedef enum
03043         { TYPE_LINEAR,
03044           TYPE_ROTATIONAL
03045         } ActuatorType;
03046   
03047   private:
03048      double goal; //< the current velocity or pose to reach, depending on the value of control_mode
03049      double pos;
03050      double max_speed;
03051      double min_position;
03052      double max_position;
03053      double start_position;
03054      double cosa;
03055      double sina;
03056      ControlMode control_mode;
03057      ActuatorType actuator_type;
03058      point3_t axis;
03059   
03060      Pose InitialPose;
03061   public:  
03062      // constructor
03063      ModelActuator( World* world,
03064                          Model* parent,
03065                          const std::string& type );
03066      // destructor
03067      ~ModelActuator();
03068   
03069      virtual void Startup();
03070      virtual void Shutdown();
03071      virtual void Update();
03072      virtual void Load();
03073   
03076      void SetSpeed( double speed );
03077   
03078      double GetSpeed() const {return goal;}
03079   
03082      void GoTo( double pose );
03083   
03084         double GetPosition() const {return pos;};
03085         double GetMaxPosition() const {return max_position;};
03086         double GetMinPosition() const {return min_position;};
03087         
03088         ActuatorType GetType() const { return actuator_type; }
03089         point3_t GetAxis() const { return axis; }
03090   };
03091 
03092 
03093 }; // end namespace stg
03094 
03095 #endif