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 - __typeof is a gcc extension, so
00609   // this could be an issue one day.
00610 #define VAR(V,init) __typeof(init) V=(init)
00611 
00612 //#define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();++I) 
00613 
00614 // NOTE:
00615 // this version assumes the container is not modified in the loop,
00616 // which I think is true everywhere it is used in Stage
00617 #define FOR_EACH(I,C) for(VAR(I,(C).begin()),ite=(C).end();(I)!=ite;++(I))
00618 
00620   template <class T, class C>
00621   void EraseAll( T thing, C& cont )
00622   { cont.erase( std::remove( cont.begin(), cont.end(), thing ), cont.end() ); }
00623   
00624   // Error macros - output goes to stderr
00625 #define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00626 #define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00627 #define PRINT_ERR2(m,a,b) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00628 #define PRINT_ERR3(m,a,b,c) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00629 #define PRINT_ERR4(m,a,b,c,d) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
00630 #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__)
00631 
00632   // Warning macros
00633 #define PRINT_WARN(m) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00634 #define PRINT_WARN1(m,a) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00635 #define PRINT_WARN2(m,a,b) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00636 #define PRINT_WARN3(m,a,b,c) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00637 #define PRINT_WARN4(m,a,b,c,d) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
00638 #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__)
00639 
00640   // Message macros
00641 #ifdef DEBUG
00642 #define PRINT_MSG(m) printf( "Stage: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00643 #define PRINT_MSG1(m,a) printf( "Stage: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00644 #define PRINT_MSG2(m,a,b) printf( "Stage: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00645 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00646 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
00647 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m" (%s %s)\n", a, b, c, d, e,__FILE__, __FUNCTION__)
00648 #else
00649 #define PRINT_MSG(m) printf( "Stage: "m"\n" )
00650 #define PRINT_MSG1(m,a) printf( "Stage: "m"\n", a)
00651 #define PRINT_MSG2(m,a,b) printf( "Stage: "m"\n,", a, b )
00652 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m"\n", a, b, c )
00653 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m"\n", a, b, c, d )
00654 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m"\n", a, b, c, d, e )
00655 #endif
00656 
00657   // DEBUG macros
00658 #ifdef DEBUG
00659 #define PRINT_DEBUG(m) printf( "debug: "m" (%s %s)\n", __FILE__, __FUNCTION__)
00660 #define PRINT_DEBUG1(m,a) printf( "debug: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
00661 #define PRINT_DEBUG2(m,a,b) printf( "debug: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
00662 #define PRINT_DEBUG3(m,a,b,c) printf( "debug: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
00663 #define PRINT_DEBUG4(m,a,b,c,d) printf( "debug: "m" (%s %s)\n", a, b, c ,d, __FILE__, __FUNCTION__)
00664 #define PRINT_DEBUG5(m,a,b,c,d,e) printf( "debug: "m" (%s %s)\n", a, b, c ,d, e, __FILE__, __FUNCTION__)
00665 #else
00666 #define PRINT_DEBUG(m)
00667 #define PRINT_DEBUG1(m,a)
00668 #define PRINT_DEBUG2(m,a,b)
00669 #define PRINT_DEBUG3(m,a,b,c)
00670 #define PRINT_DEBUG4(m,a,b,c,d)
00671 #define PRINT_DEBUG5(m,a,b,c,d,e)
00672 #endif
00673 
00674   class Block;
00675   class Model;
00676 
00679   typedef int (*model_callback_t)( Model* mod, void* user );
00680 
00681   // ANCESTOR CLASS
00683   class Ancestor
00684   {
00685     friend class Canvas; // allow Canvas access to our private members
00686      
00687   protected:
00688 
00690      std::map<std::string,unsigned int> child_type_counts;
00691 
00692      ModelPtrVec children;
00693 
00694     bool debug;
00695 
00697      std::map<std::string,void*> props;
00698 
00699      std::string token;
00700 
00701      void Load( Worldfile* wf, int section );
00702      void Save( Worldfile* wf, int section );    
00703              
00704   public:   
00705     Ancestor();
00706     virtual ~Ancestor();
00707      
00709      ModelPtrVec& GetChildren(){ return children;}
00710     
00712     void ForEachDescendant( model_callback_t func, void* arg );
00713      
00714     virtual void AddChild( Model* mod );
00715     virtual void RemoveChild( Model* mod );
00716     virtual Pose GetGlobalPose();
00717      
00718     const char* Token(){ return token.c_str(); }
00719 
00720     const std::string& TokenStr(){ return token; }
00721      
00722     void SetToken( const std::string& str ){ token = str; } 
00723      
00725      void SetProperty( std::string& key, void* value ){ props[ key ] = value; }
00726      
00728         void* GetProperty( std::string& key )
00729         {
00730             std::map<std::string,void*>::iterator it = props.find( key );       
00731             return( it == props.end() ? NULL : it->second );
00732         }
00733   };
00734 
00737   class RaytraceResult
00738   {
00739   public:
00740     Pose pose; 
00741     meters_t range; 
00742     Model* mod; 
00743     Color color; 
00744      
00745      RaytraceResult() : pose(), range(0), mod(NULL), color() {}
00746      RaytraceResult( const Pose& pose, 
00747                           meters_t range ) 
00748         : pose(pose), range(range), mod(NULL), color() {}    
00749   };
00750     
00751   class Ray
00752   {
00753   public:
00754      Ray( const Model* mod, const Pose& origin, const meters_t range, const ray_test_func_t func, const void* arg, const bool ztest ) :
00755         mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest)
00756      {}
00757 
00758      Ray() : mod(NULL), origin(0,0,0,0), range(0), func(NULL), arg(NULL), ztest(true)
00759      {}
00760         
00761         const Model* mod;
00762         Pose origin;
00763         meters_t range;
00764         ray_test_func_t func;
00765         const void* arg;
00766      bool ztest;        
00767   };
00768         
00769 
00770   // defined in stage_internal.hh
00771   class Region;
00772   class SuperRegion;
00773   class BlockGroup;
00774   class PowerPack;
00775 
00776   class LogEntry
00777   {
00778      usec_t timestamp;
00779      Model* mod;
00780      Pose pose;
00781      
00782   public:
00783      LogEntry( usec_t timestamp, Model* mod );
00784      
00786      static std::vector<LogEntry> log;
00787      
00789      static size_t Count(){ return log.size(); }
00790      
00792      static void Clear(){ log.clear(); }
00793 
00795      static void Print();
00796   };
00797 
00798   class CtrlArgs
00799   {
00800   public:
00801      std::string worldfile;
00802      std::string cmdline;
00803 
00804      CtrlArgs( std::string w, std::string c ) : worldfile(w), cmdline(c) {}
00805   };
00806 
00808   class World : public Ancestor
00809   {
00810     private: 
00811      //pthread_mutex_t bflock;
00812      //pthread_rwlock_t rwlock;
00813 
00814     public:
00815         //inline int Lock(){ return pthread_mutex_lock( &bflock ); }
00816         //inline int Unlock(){ return pthread_mutex_unlock( &bflock ); }
00817 
00818         //inline int ReadLock(){ return pthread_mutex_lock( &bflock ); }
00819         //inline int WriteLock(){ return pthread_mutex_lock( &bflock ); }
00820         //inline int Unlock(){ return pthread_mutex_unlock( &bflock ); }
00821         
00822     friend class Block;
00823     friend class Model; // allow access to private members
00824     friend class ModelFiducial;
00825     friend class Canvas;
00826 
00827   public: 
00830      static std::vector<std::string> args;
00831      static std::string ctrlargs;
00832 
00833   private:
00834     
00835     static std::set<World*> world_set; 
00836     static bool quit_all; 
00837     static void UpdateCb( World* world);
00838     static unsigned int next_id; 
00839      
00840     bool destroy;
00841     bool dirty; 
00842      
00844      std::set<Model*> models;
00845 
00847      std::map<std::string, Model*> models_by_name;      
00848 
00850      std::map<int,Model*> models_by_wfentity;
00851         
00854      ModelPtrVec models_with_fiducials;
00855         
00856         struct ltx
00857         {
00858             bool operator()(const Model* a, const Model* b) const;
00859         };
00860         
00861         struct lty
00862         {
00863             bool operator()(const Model* a, const Model* b) const;
00864         };
00865         
00868         std::set<Model*,ltx> models_with_fiducials_byx;
00869         
00872         std::set<Model*,lty> models_with_fiducials_byy;
00873                      
00875      void FiducialInsert( Model* mod )
00876      { 
00877         FiducialErase( mod ); // make sure it's not there already
00878         models_with_fiducials.push_back( mod ); 
00879      }
00880      
00882      void FiducialErase( Model* mod )
00883      { 
00884         EraseAll( mod, models_with_fiducials );
00885      }
00886 
00887     double ppm; 
00888     bool quit; 
00889      
00890      bool show_clock; 
00891      unsigned int show_clock_interval; 
00892         
00893     pthread_mutex_t sync_mutex; 
00894         unsigned int threads_working; 
00895     pthread_cond_t threads_start_cond; 
00896     pthread_cond_t threads_done_cond; 
00897     int total_subs; 
00898      unsigned int worker_threads; 
00899     
00900   protected:     
00901 
00902      std::list<std::pair<world_callback_t,void*> > cb_list; 
00903     bounds3d_t extent; 
00904     bool graphics;
00905 
00906      std::set<Option*> option_table; 
00907      std::list<PowerPack*> powerpack_list; 
00908 
00909     usec_t quit_time;
00910      std::list<float*> ray_list;
00911     usec_t sim_time; 
00912      std::map<point_int_t,SuperRegion*> superregions;
00913     SuperRegion* sr_cached; 
00914      
00915      std::vector<ModelPtrVec> update_lists;  
00916      
00917     uint64_t updates; 
00918     Worldfile* wf; 
00919 
00920      void CallUpdateCallbacks(); 
00921 
00922   public:
00923      
00924     bool paused; 
00925 
00926     virtual void Start(){ paused = false; };
00927     virtual void Stop(){ paused = true; };
00928     virtual void TogglePause(){ paused ? Start() : Stop(); };
00929 
00930         bool Paused() const { return( paused ); };
00931         
00935         virtual void Redraw( void ){ }; // does nothing
00936 
00937      PointIntVec rt_cells;
00938      PointIntVec rt_candidate_cells;
00939 
00940     static const int DEFAULT_PPM = 50;  // default resolution in pixels per meter
00941 
00944      void AddUpdateCallback( world_callback_t cb, void* user );
00945 
00948      int RemoveUpdateCallback( world_callback_t cb, void* user );
00949 
00951      void Log( Model* mod );
00952 
00954     void NeedRedraw(){ dirty = true; };
00955     
00957     Model* ground;
00958     
00961     virtual std::string ClockString( void ) const;
00962         
00963      Model* CreateModel( Model* parent, const std::string& typestr );    
00964     void LoadModel( Worldfile* wf, int entity );
00965     void LoadBlock( Worldfile* wf, int entity );
00966     void LoadBlockGroup( Worldfile* wf, int entity );
00967     
00968         void LoadSensor( Worldfile* wf, int entity );
00969 
00970     virtual Model* RecentlySelectedModel() const { return NULL; }
00971         
00973     void MapPoly( const PointIntVec& poly,
00974                                     Block* block,
00975                                     unsigned int layer );
00976 
00977     SuperRegion* AddSuperRegion( const point_int_t& coord );
00978     SuperRegion* GetSuperRegion( const point_int_t& org );
00979     SuperRegion* GetSuperRegionCreate( const point_int_t& org );
00980     //void ExpireSuperRegion( SuperRegion* sr );
00981         
00984     int32_t MetersToPixels( meters_t x ) const
00985     { return (int32_t)floor(x * ppm); };
00986         
00987     point_int_t MetersToPixels( const point_t& pt ) const
00988     { return point_int_t( MetersToPixels(pt.x), MetersToPixels(pt.y)); };
00989         
00990     // dummy implementations to be overloaded by GUI subclasses
00991     virtual void PushColor( Color col ) 
00992      { /* do nothing */  (void)col; };
00993     virtual void PushColor( double r, double g, double b, double a ) 
00994      { /* do nothing */ (void)r; (void)g; (void)b; (void)a; };
00995      
00996     virtual void PopColor(){ /* do nothing */  };
00997         
00998     SuperRegion* CreateSuperRegion( point_int_t origin );
00999     void DestroySuperRegion( SuperRegion* sr );
01000         
01002      RaytraceResult Raytrace( const Ray& ray );
01003 
01004     RaytraceResult Raytrace( const Pose& pose,           
01005                                                 const meters_t range,
01006                                                 const ray_test_func_t func,
01007                                                 const Model* finder,
01008                                                 const void* arg,
01009                                                 const bool ztest );
01010         
01011     void Raytrace( const Pose &pose,             
01012                          const meters_t range,
01013                          const radians_t fov,
01014                          const ray_test_func_t func,
01015                          const Model* finder,
01016                          const void* arg,
01017                          RaytraceResult* samples,
01018                          const uint32_t sample_count,
01019                          const bool ztest );
01020         
01021         
01023     inline void Extend( point3_t pt );
01024   
01025     virtual void AddModel( Model* mod );
01026     virtual void RemoveModel( Model* mod );
01027 
01028     void AddModelName( Model* mod, const std::string& name );
01029         
01030     void AddPowerPack( PowerPack* pp );
01031     void RemovePowerPack( PowerPack* pp );
01032         
01033     void ClearRays();
01034   
01036     void RecordRay( double x1, double y1, double x2, double y2 );
01037         
01040     bool PastQuitTime();
01041                 
01042     static void* update_thread_entry( std::pair<World*,int>* info );
01043     
01044     class Event
01045     {
01046     public:
01047       
01048       Event( usec_t time, Model* mod, model_callback_t cb, void* arg ) 
01049                 : time(time), mod(mod), cb(cb), arg(arg) {}
01050             
01051       usec_t time; 
01052       Model* mod; 
01053       model_callback_t cb;
01054             void* arg;
01055             
01058       bool operator<( const Event& other ) const;
01059     };
01060      
01062      std::vector<std::priority_queue<Event> > event_queues;
01063 
01065         std::vector<std::queue<Model*> > pending_update_callbacks;
01066         
01078         void Enqueue( unsigned int queue_num, usec_t delay, Model* mod, model_callback_t cb, void* arg )
01079         {  event_queues[queue_num].push( Event( sim_time + delay, mod, cb, arg ) ); }
01080         
01082      std::set<Model*> active_energy;
01083 
01085      std::set<Model*> active_velocity;
01086         
01088      usec_t sim_interval;
01089         
01090         // debug instrumentation - making sure the number of update callbacks
01091         // in each thread is consistent with the number that have been
01092         // registered globally
01093         int update_cb_count;
01094 
01096      void ConsumeQueue( unsigned int queue_num );
01097 
01100      unsigned int GetEventQueue( Model* mod ) const;
01101 
01102   public:
01104     static bool UpdateAll(); 
01105      
01106     World( const std::string& name = "MyWorld", 
01107               double ppm = DEFAULT_PPM );
01108         
01109     virtual ~World();
01110     
01112     usec_t SimTimeNow(void) const { return sim_time; }
01113         
01116     Worldfile* GetWorldFile()   { return wf; };
01117         
01121     virtual bool IsGUI() const { return false; }
01122      
01128     virtual void Load( const std::string& worldfile_path );
01129 
01130     virtual void UnLoad();
01131 
01132     virtual void Reload();
01133 
01136     virtual bool Save( const char* filename );
01137 
01141     virtual bool Update(void);
01142      
01146     bool TestQuit() const { return( quit || quit_all );  }
01147         
01149     void Quit(){ quit = true; }
01150 
01152     void QuitAll(){ quit_all = true; }
01153 
01155     void CancelQuit(){ quit = false; }
01156 
01158     void CancelQuitAll(){ quit_all = false; }
01159      
01160      void TryCharge( PowerPack* pp, const Pose& pose );
01161 
01164     double Resolution() const { return ppm; };
01165    
01168     Model* GetModel( const std::string& name ) const;
01169 
01171     const std::set<Model*> GetAllModels() const { return models; };
01172   
01174     const bounds3d_t& GetExtent() const { return extent; };
01175   
01177     uint64_t GetUpdateCount() const { return updates; }
01178 
01180      void RegisterOption( Option* opt );    
01181      
01183      void ShowClock( bool enable ){ show_clock = enable; };
01184 
01186      Model* GetGround() {return ground;};
01187     
01188   };
01189   
01190   class Block
01191   {
01192     friend class BlockGroup;
01193     friend class Model;
01194     friend class SuperRegion;
01195     friend class World;
01196     friend class Canvas;
01197         friend class Cell;
01198   public:
01199         
01203     Block( Model* mod,  
01204                      const std::vector<point_t>& pts,
01205                      meters_t zmin,
01206                      meters_t zmax,
01207                      Color color,
01208                      bool inherit_color,
01209                      bool wheel );
01210         
01212     Block(  Model* mod,  Worldfile* wf, int entity);
01213         
01214     ~Block();
01215      
01217     void Map( unsigned int layer );      
01218      
01220     void UnMap( unsigned int layer );    
01221          
01223      void DrawSolid(bool topview);
01224 
01226     void DrawFootPrint(); 
01227 
01229      void Translate( double x, double y );   
01230 
01232      double CenterX();
01233 
01235      double CenterY();
01236 
01238      void SetCenterX( double y );
01239 
01241      void SetCenterY( double y );
01242 
01244      void SetCenter( double x, double y);    
01245 
01247      void SetZ( double min, double max );
01248         
01249         void AppendTouchingModels( ModelPtrSet& touchers );
01250      
01252     Model* TestCollision(); 
01253 
01254     void Load( Worldfile* wf, int entity );  
01255     Model* GetModel(){ return mod; };  
01256     const Color& GetColor();        
01257      void Rasterize( uint8_t* data, 
01258                           unsigned int width, unsigned int height,      
01259                           meters_t cellwidth, meters_t cellheight );
01260         
01261   private:
01262     Model* mod; 
01263      std::vector<point_t> mpts; 
01264     size_t pt_count; 
01265      std::vector<point_t> pts; 
01266     Size size;   
01267     Bounds local_z; 
01268     Color color;
01269     bool inherit_color;
01270         bool wheel;
01271 
01272     void DrawTop();
01273     void DrawSides();
01274      
01276     Bounds global_z;     
01277     bool mapped;
01278         
01280         std::vector< std::list<Block*>::iterator > list_entries;
01281         
01284         CellPtrVec rendered_cells[2];
01285         
01286      PointIntVec gpts;
01287     
01290      point_t BlockPointToModelMeters( const point_t& bpt );
01291     
01293      void InvalidateModelPointCache();
01294         
01295   };
01296 
01297     
01298   class BlockGroup
01299   {
01300     friend class Model;
01301         friend class Block;
01302         
01303   private:
01304     int displaylist;
01305         
01306     void BuildDisplayList( Model* mod );
01307         
01308         BlockPtrSet blocks;
01309     Size size;
01310     point3_t offset;
01311     meters_t minx, maxx, miny, maxy;
01312         
01313   public:
01314     BlockGroup();
01315     ~BlockGroup();
01316         
01317     uint32_t GetCount(){ return blocks.size(); };
01318     const Size& GetSize(){ return size; };
01319     const point3_t& GetOffset(){ return offset; };
01320         
01323     void CalcSize();
01324      
01325     void AppendBlock( Block* block );
01326     void CallDisplayList( Model* mod );
01327     void Clear() ; 
01329      void AppendTouchingModels( ModelPtrSet& touchers );
01330      
01333     Model* TestCollision();
01334  
01335     void Map( unsigned int layer );
01336     void UnMap( unsigned int layer );
01337         
01339     void DrawSolid( const Geom &geom); 
01340 
01342      void DrawFootPrint( const Geom &geom);
01343 
01344     void LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfile *wf );
01345     void LoadBlock( Model* mod, Worldfile* wf, int entity );
01346      
01347      void Rasterize( uint8_t* data, 
01348                           unsigned int width, unsigned int height,
01349                           meters_t cellwidth, meters_t cellheight );
01350      
01351      void InvalidateModelPointCache()
01352      {
01353         FOR_EACH( it, blocks )
01354           (*it)->InvalidateModelPointCache();
01355      }
01356 
01357   };
01358 
01359   class Camera 
01360   {
01361   protected:
01362     float _pitch; //left-right (about y)
01363     float _yaw; //up-down (about x)
01364     float _x, _y, _z;
01365     
01366   public:
01367     Camera() : _pitch( 0 ), _yaw( 0 ), _x( 0 ), _y( 0 ), _z( 0 ) { }
01368     virtual ~Camera() { }
01369 
01370     virtual void Draw( void ) const = 0;
01371     virtual void SetProjection( void ) const = 0;
01372 
01373      float yaw( void ) const { return _yaw; }
01374      float pitch( void ) const { return _pitch; }
01375      
01376      float x( void ) const { return _x; }
01377      float y( void ) const { return _y; }
01378      float z( void ) const { return _z; }
01379      
01380     virtual void reset() = 0;
01381     virtual void Load( Worldfile* wf, int sec ) = 0;
01382 
01383     //TODO data should be passed in somehow else. (at least min/max stuff)
01384     //virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const = 0;
01385   };
01386 
01387   class PerspectiveCamera : public Camera
01388   {
01389   private:
01390     float _z_near;
01391     float _z_far;
01392     float _vert_fov;
01393     float _horiz_fov;
01394     float _aspect;
01395 
01396   public:
01397     PerspectiveCamera( void );
01398 
01399     virtual void Draw( void ) const;
01400     virtual void SetProjection( void ) const;
01401     //void SetProjection( float aspect ) const;
01402     void update( void );
01403 
01404     void strafe( float amount );
01405     void forward( float amount );
01406     
01407      void setPose( float x, float y, float z ) { _x = x; _y = y; _z = z; }
01408      void addPose( float x, float y, float z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; }
01409     void move( float x, float y, float z );
01410      void setFov( float horiz_fov, float vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; }
01412      void setAspect( float aspect ) { _aspect = aspect; }
01413      void setYaw( float yaw ) { _yaw = yaw; }
01414      float horizFov( void ) const { return _horiz_fov; }
01415      float vertFov( void ) const { return _vert_fov; }
01416      void addYaw( float yaw ) { _yaw += yaw; }
01417      void setPitch( float pitch ) { _pitch = pitch; }
01418      void addPitch( float pitch ) { _pitch += pitch; if( _pitch < 0 ) _pitch = 0; else if( _pitch > 180 ) _pitch = 180; }
01419     
01420      float realDistance( float z_buf_val ) const {
01421       return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) );
01422     }
01423      void scroll( float dy ) { _z += dy; }
01424      float nearClip( void ) const { return _z_near; }
01425      float farClip( void ) const { return _z_far; }
01426      void setClip( float near, float far ) { _z_far = far; _z_near = near; }
01427     
01428      void reset() { setPitch( 70 ); setYaw( 0 ); }
01429     
01430     void Load( Worldfile* wf, int sec );
01431     void Save( Worldfile* wf, int sec );
01432   };
01433   
01434   class OrthoCamera : public Camera
01435   {
01436   private:
01437     float _scale;
01438     float _pixels_width;
01439     float _pixels_height;
01440     float _y_min;
01441     float _y_max;
01442   
01443   public:
01444     OrthoCamera( void ) : _scale( 15 ) { }
01445     virtual void Draw() const;
01446     virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max );
01447     virtual void SetProjection( void ) const;
01448   
01449     void move( float x, float y );
01450      void setYaw( float yaw ) { _yaw = yaw; }
01451      void setPitch( float pitch ) { _pitch = pitch; }
01452      void addYaw( float yaw ) { _yaw += yaw;    }
01453      void addPitch( float pitch ) {
01454       _pitch += pitch;
01455       if( _pitch > 90 )
01456           _pitch = 90;
01457       else if( _pitch < 0 )
01458           _pitch = 0;
01459     }
01460   
01461      void setScale( float scale ) { _scale = scale; }
01462      void setPose( float x, float y) { _x = x; _y = y; }
01463   
01464     void scale( float scale, float shift_x = 0, float h = 0, float shift_y = 0, float w = 0 );  
01465      void reset( void ) { _pitch = _yaw = 0; }
01466   
01467      float scale() const { return _scale; }
01468   
01469     void Load( Worldfile* wf, int sec );
01470     void Save( Worldfile* wf, int sec );
01471   };
01472 
01473 
01477   class WorldGui : public World, public Fl_Window 
01478   {
01479     friend class Canvas;
01480     friend class ModelCamera;
01481     friend class Model;
01482     friend class Option;
01483 
01484   private:
01485 
01486     Canvas* canvas;
01487     std::vector<Option*> drawOptions;
01488     FileManager* fileMan; 
01489      std::vector<usec_t> interval_log;
01490      
01493      float speedup; 
01494 
01495     Fl_Menu_Bar* mbar;
01496     OptionsDlg* oDlg;
01497     bool pause_time;
01498 
01501     usec_t real_time_interval;
01502      
01504     usec_t real_time_now; 
01505 
01508     usec_t real_time_recorded;
01509      
01511      uint64_t timing_interval;
01512 
01513     // static callback functions
01514     static void windowCb( Fl_Widget* w, WorldGui* wg ); 
01515     static void fileLoadCb( Fl_Widget* w, WorldGui* wg );
01516     static void fileSaveCb( Fl_Widget* w, WorldGui* wg );
01517     static void fileSaveAsCb( Fl_Widget* w, WorldGui* wg );
01518     static void fileExitCb( Fl_Widget* w, WorldGui* wg );
01519     static void viewOptionsCb( OptionsDlg* oDlg, WorldGui* wg );
01520     static void optionsDlgCb( OptionsDlg* oDlg, WorldGui* wg );
01521     static void helpAboutCb( Fl_Widget* w, WorldGui* wg );
01522     static void pauseCb( Fl_Widget* w, WorldGui* wg );
01523     static void onceCb( Fl_Widget* w, WorldGui* wg );
01524     static void fasterCb( Fl_Widget* w, WorldGui* wg );
01525     static void slowerCb( Fl_Widget* w, WorldGui* wg );
01526     static void realtimeCb( Fl_Widget* w, WorldGui* wg );
01527     static void fasttimeCb( Fl_Widget* w, WorldGui* wg );
01528     static void resetViewCb( Fl_Widget* w, WorldGui* wg );
01529     static void moreHelptCb( Fl_Widget* w, WorldGui* wg );
01530     
01531     // GUI functions
01532     bool saveAsDialog();
01533     bool closeWindowQuery();
01534     
01535     virtual void AddModel( Model* mod );
01536     
01537      void SetTimeouts();
01538 
01539   protected:
01540     
01541     virtual void PushColor( Color col );
01542     virtual void PushColor( double r, double g, double b, double a );
01543     virtual void PopColor();
01544     
01545     void DrawOccupancy() const;
01546     void DrawVoxels() const;
01547      
01548   public:
01549     
01550     WorldGui(int W,int H,const char*L=0);
01551     ~WorldGui();
01552 
01554         virtual void Redraw( void );
01555 
01556     virtual std::string ClockString() const;
01557     virtual bool Update();  
01558     virtual void Load( const std::string& filename );
01559     virtual void UnLoad();
01560     virtual bool Save( const char* filename );  
01561     virtual bool IsGUI() const { return true; };    
01562     virtual Model* RecentlySelectedModel() const;
01563 
01564     virtual void Start();
01565     virtual void Stop();
01566     
01567     usec_t RealTimeNow(void) const;
01568  
01569     void DrawBoundingBoxTree();
01570     
01571     Canvas* GetCanvas( void ) const { return canvas; } 
01572 
01574     void Show(); 
01575 
01577     std::string EnergyString( void ) const; 
01578     virtual void RemoveChild( Model* mod );  
01579 
01580      bool IsTopView();
01581   };
01582 
01583 
01584   class StripPlotVis : public Visualizer
01585   {
01586   private:
01587      
01588      Model* mod;
01589      float* data;
01590      size_t len;
01591      size_t count;
01592      unsigned int index;
01593      float x,y,w,h,min,max;
01594      Color fgcolor, bgcolor;
01595      
01596   public:
01597      StripPlotVis( float x, float y, float w, float h, 
01598                         size_t len, 
01599                         Color fgcolor, Color bgcolor,
01600                         const char* name, const char* wfname );
01601      virtual ~StripPlotVis();
01602      virtual void Visualize( Model* mod, Camera* cam );     
01603      void AppendValue( float value );
01604   };
01605 
01606 
01607   class PowerPack
01608   {
01609      friend class WorldGui;
01610      friend class Canvas;
01611      
01612   protected:
01613      
01614      class DissipationVis : public Visualizer
01615      {
01616      private:
01617         unsigned int columns, rows;
01618         meters_t width, height;
01619          
01620          std::vector<joules_t> cells;
01621          
01622          joules_t peak_value;
01623          double cellsize;
01624          
01625         static joules_t global_peak_value; 
01626 
01627      public:
01628         DissipationVis( meters_t width, 
01629                              meters_t height, 
01630                              meters_t cellsize );
01631 
01632         virtual ~DissipationVis();
01633         virtual void Visualize( Model* mod, Camera* cam );      
01634         
01635         void Accumulate( meters_t x, meters_t y, joules_t amount );
01636      } event_vis;
01637      
01638 
01639      StripPlotVis output_vis;
01640      StripPlotVis stored_vis;
01641 
01643      Model* mod;
01644     
01646      joules_t stored;
01647      
01649      joules_t capacity;
01650      
01652      bool charging;
01653      
01655      joules_t dissipated;
01656      
01657      // these are used to visualize the power draw
01658      usec_t last_time;
01659      joules_t last_joules;
01660      watts_t last_watts;
01661 
01662   public:
01663      static joules_t global_stored;
01664      static joules_t global_capacity;
01665      static joules_t global_dissipated;  
01666      static joules_t global_input;
01667 
01668   public:
01669      PowerPack( Model* mod );
01670      ~PowerPack();
01671      
01673      void Visualize( Camera* cam );
01674 
01676      joules_t RemainingCapacity() const;
01677      
01679      void Add( joules_t j );
01680         
01682      void Subtract( joules_t j );
01683         
01685      void TransferTo( PowerPack* dest, joules_t amount );    
01686 
01687      double ProportionRemaining() const
01688      { return( stored / capacity ); }
01689 
01692      void Print( const char* prefix ) const
01693      { 
01694         if( prefix )
01695           printf( "%s", prefix );
01696 
01697         printf( "PowerPack %.2f/%.2f J\n", stored, capacity ); 
01698      }      
01699      
01700      joules_t GetStored() const;
01701      joules_t GetCapacity() const;
01702      joules_t GetDissipated() const;
01703      void SetCapacity( joules_t j );
01704      void SetStored( joules_t j );  
01705 
01707      bool GetCharging() const { return charging; }
01708      
01709      void ChargeStart(){ charging = true; }
01710      void ChargeStop(){ charging = false; }
01711 
01713      void Dissipate( joules_t j );
01714      
01716      void Dissipate( joules_t j, const Pose& p );
01717   };
01718 
01719    
01721   class Model : public Ancestor
01722   {
01723     friend class Ancestor;
01724     friend class World;
01725     friend class World::Event;
01726     friend class WorldGui;
01727     friend class Canvas;
01728     friend class Block;
01729     friend class Region;
01730     friend class BlockGroup;
01731     friend class PowerPack;
01732     friend class Ray;
01733      friend class ModelFiducial;
01734         
01735   private:
01737         static uint32_t count;
01738         static std::map<id_t,Model*> modelsbyid;
01739 
01741         bool mapped;
01742 
01743      std::vector<Option*> drawOptions;
01744      const std::vector<Option*>& getOptions() const { return drawOptions; }
01745      
01746   protected:
01747 
01750      bool alwayson;
01751 
01752      BlockGroup blockgroup;
01754      int blocks_dl;
01755 
01759      int boundary;
01760         
01763   public:
01764      class cb_t
01765      {
01766      public:
01767         model_callback_t callback;
01768         void* arg;
01769             
01770         cb_t( model_callback_t cb, void* arg ) 
01771           : callback(cb), arg(arg) {}
01772             
01773         cb_t( world_callback_t cb, void* arg ) 
01774           : callback(NULL), arg(arg) { (void)cb; }
01775             
01776         cb_t() : callback(NULL), arg(NULL) {}
01777             
01779          bool operator<( const cb_t& other ) const
01780          {
01781              if( callback == other.callback )
01782                  return( arg < other.arg );
01783              //else
01784              return ((void*)(callback)) < ((void*)(other.callback)); 
01785          }
01786          
01788          bool operator==( const cb_t& other ) const
01789          { return( callback == other.callback);  }          
01790      };
01791         
01792      class Flag
01793      {
01794      private:
01795         Color color;
01796         double size;
01797         int displaylist;
01798 
01799      public:
01800          void SetColor( const Color& col );
01801          void SetSize( double sz );
01802          
01803          Color GetColor(){ return color; }
01804          double GetSize(){ return size; }
01805          
01806         Flag( Color color, double size );
01807         Flag* Nibble( double portion );
01808         
01811         void Draw(  GLUquadric* quadric );
01812      };
01813 
01814         typedef enum {
01815             CB_FLAGDECR,
01816             CB_FLAGINCR,
01817             CB_GEOM,
01818             CB_INIT,
01819             CB_LOAD,
01820             CB_PARENT,
01821             CB_POSE,
01822             CB_SAVE,
01823             CB_SHUTDOWN,
01824             CB_STARTUP,
01825             CB_UPDATE,
01826             CB_VELOCITY,
01827             //CB_POSTUPDATE,
01828             __CB_TYPE_COUNT // must be the last entry: counts the number of types
01829         } callback_type_t;
01830         
01831   protected:
01835         std::vector<std::set<cb_t> > callbacks;
01836         
01837 
01839      Color color;
01840         
01844      bool data_fresh;
01845 
01849         bool disabled; 
01850 
01852      std::list<Visualizer*> cv_list;
01853 
01855      std::list<Flag*> flag_list;
01856         
01859      double friction;
01860         
01863         Geom geom;
01864 
01866      class GuiState
01867      {
01868      public:
01869         bool grid;
01870         bool move;
01871         bool nose;
01872         bool outline;
01873         
01874         GuiState();
01875         void Load( Worldfile* wf, int wf_entity );
01876      } gui;
01877      
01878      bool has_default_block;
01879                 
01880   
01882      uint32_t id;   
01883      usec_t interval; 
01884      usec_t interval_energy; 
01885      usec_t interval_pose; 
01886 
01887      usec_t last_update; 
01888      bool log_state; 
01889      meters_t map_resolution;
01890      kg_t mass;
01891 
01893      Model* parent; 
01894 
01897      Pose pose;
01898 
01900      PowerPack* power_pack;
01901 
01904      std::list<PowerPack*> pps_charging;
01905         
01907      class RasterVis : public Visualizer
01908      {
01909      private:
01910         uint8_t* data;
01911         unsigned int width, height;
01912         meters_t cellwidth, cellheight;
01913         std::vector<point_t> pts;
01914       
01915      public:
01916         RasterVis();
01917         virtual ~RasterVis( void ){}
01918         virtual void Visualize( Model* mod, Camera* cam );
01919       
01920         void SetData( uint8_t* data, 
01921                           unsigned int width, 
01922                           unsigned int height,
01923                           meters_t cellwidth,
01924                           meters_t cellheight );
01925       
01926         int subs;     //< the number of subscriptions to this model
01927         int used;     //< the number of connections to this model
01928       
01929         void AddPoint( meters_t x, meters_t y );
01930         void ClearPts();
01931       
01932      } rastervis;
01933      
01934      bool rebuild_displaylist; 
01935      std::string say_string;   
01936         
01937     bool stack_children; 
01938 
01939         bool stall; 
01940      int subs;    
01941 
01945      bool thread_safe;
01946      
01948      class TrailItem 
01949      {                                                                                          
01950      public:
01951         usec_t time;
01952         Pose pose;
01953         Color color;
01954         
01955         TrailItem() 
01956           : time(0), pose(), color(){}
01957          
01958          //TrailItem( usec_t time, Pose pose, Color color ) 
01959          //: time(time), pose(pose), color(color){}
01960      };
01961     
01963         std::vector<TrailItem> trail;
01964 
01966         unsigned int trail_index;
01967 
01971      static unsigned int trail_length;
01972      
01974      static uint64_t trail_interval;
01975      
01977         void UpdateTrail();
01978 
01979      //model_type_t type;  
01980      const std::string type;
01983         unsigned int event_queue_num; 
01984         bool used;   
01985         Velocity velocity;
01986         
01990         bool velocity_enable;
01991         
01992         watts_t watts;
01993      
01996      watts_t watts_give;
01997     
02000      watts_t watts_take;
02001     
02002      Worldfile* wf;
02003      int wf_entity;
02004      World* world; // pointer to the world in which this model exists
02005      WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode
02006 
02007   public:
02008      
02009      const std::string& GetModelType() const {return type;}  
02010      std::string GetSayString(){return std::string(say_string);}
02011      
02014     Model* GetChild( const std::string& name ) const;
02015 
02017         usec_t GetInterval(){ return interval; }
02018         
02019      class Visibility
02020      {
02021      public:
02022          bool blob_return;
02023          int fiducial_key;
02024          int fiducial_return;
02025          bool gripper_return;
02026          bool obstacle_return;
02027          float ranger_return; // 0 - 1
02028         
02029         Visibility();
02030 
02031         void Load( Worldfile* wf, int wf_entity );
02032         void Save( Worldfile* wf, int wf_entity );
02033      } vis;
02034      
02035      usec_t GetUpdateInterval() const { return interval; }
02036      usec_t GetEnergyInterval() const { return interval_energy; }
02037      usec_t GetPoseInterval() const { return interval_pose; }
02038      
02041      void Rasterize( uint8_t* data, 
02042                           unsigned int width, unsigned int height,
02043                           meters_t cellwidth, meters_t cellheight );
02044     
02045   private: 
02048      explicit Model(const Model& original);
02049 
02052      Model& operator=(const Model& original);
02053 
02054   protected:
02055 
02057      void RegisterOption( Option* opt );
02058 
02059      void AppendTouchingModels( ModelPtrSet& touchers );
02060         
02065      Model* TestCollision();
02066   
02067      void CommitTestedPose();
02068 
02069      void Map( unsigned int layer );
02070      void UnMap( unsigned int layer );
02071 
02072      void MapWithChildren( unsigned int layer );
02073      void UnMapWithChildren( unsigned int layer );
02074   
02075      // Find the root model, and map/unmap the whole tree.
02076      void MapFromRoot( unsigned int layer );
02077      void UnMapFromRoot( unsigned int layer );
02078 
02081      RaytraceResult Raytrace( const Pose &pose,
02082                                                 const meters_t range, 
02083                                                 const ray_test_func_t func,
02084                                                 const void* arg,
02085                                                 const bool ztest = true );
02086   
02089      void Raytrace( const Pose &pose,
02090                          const meters_t range, 
02091                          const radians_t fov, 
02092                          const ray_test_func_t func,
02093                          const void* arg,
02094                          RaytraceResult* samples,
02095                          const uint32_t sample_count,
02096                          const bool ztest = true  );
02097   
02098      RaytraceResult Raytrace( const radians_t bearing,           
02099                                                 const meters_t range,
02100                                                 const ray_test_func_t func,
02101                                                 const void* arg,
02102                                                 const bool ztest = true );
02103   
02104      void Raytrace( const radians_t bearing,             
02105                          const meters_t range,
02106                          const radians_t fov,
02107                          const ray_test_func_t func,
02108                          const void* arg,
02109                          RaytraceResult* samples,
02110                          const uint32_t sample_count,
02111                          const bool ztest = true );
02112   
02113         virtual void Startup();
02114         virtual void Shutdown();
02115         virtual void Update();
02116         virtual void Move();
02117         virtual void UpdateCharge();
02118         
02119         static int UpdateWrapper( Model* mod, void* arg ){ mod->Update(); return 0; }
02120         static int MoveWrapper( Model* mod, void* arg ){ mod->Move(); return 0; }
02121 
02123         void CallUpdateCallbacks( void );
02124 
02125      meters_t ModelHeight() const;
02126 
02127      void DrawBlocksTree();
02128      virtual void DrawBlocks();
02129      void DrawBoundingBox();
02130      void DrawBoundingBoxTree();
02131      virtual void DrawStatus( Camera* cam );
02132      void DrawStatusTree( Camera* cam );
02133   
02134      void DrawOriginTree();
02135      void DrawOrigin();
02136   
02137      void PushLocalCoords();
02138      void PopCoords();
02139   
02141      void DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width=1.0, double height=1.0 );
02142     
02143      virtual void DrawPicker();
02144      virtual void DataVisualize( Camera* cam );  
02145      virtual void DrawSelected(void);
02146     
02147      void DrawTrailFootprint();
02148      void DrawTrailBlocks();
02149      void DrawTrailArrows();
02150      void DrawGrid();
02151      // void DrawBlinkenlights();
02152      void DataVisualizeTree( Camera* cam );
02153      void DrawFlagList();
02154      void DrawPose( Pose pose );
02155     
02156   public:
02157      virtual void PushColor( Color col ){ world->PushColor( col ); }    
02158      virtual void PushColor( double r, double g, double b, double a ){ world->PushColor( r,g,b,a ); }   
02159      virtual void PopColor()    { world->PopColor(); }
02160     
02161      PowerPack* FindPowerPack() const;
02162     
02163      //void RecordRenderPoint( GSList** head, GSList* link, 
02164      //                 unsigned int* c1, unsigned int* c2 );
02165 
02166      void PlaceInFreeSpace( meters_t xmin, meters_t xmax, 
02167                                     meters_t ymin, meters_t ymax );
02168     
02170      std::string PoseString()
02171      { return pose.String(); }
02172     
02174      static Model* LookupId( uint32_t id )
02175      { return modelsbyid[id]; }
02176      
02178      Model( World* world, 
02179               Model* parent = NULL, 
02180               const std::string& type = "model" );
02181      
02183      virtual ~Model();
02184         
02186         Model() 
02187             : parent(NULL), world(NULL) 
02188         {}
02189         
02190      void Say( const std::string& str );
02191      
02193      void AddVisualizer( Visualizer* custom_visual, bool on_by_default );
02194 
02196      void RemoveVisualizer( Visualizer* custom_visual );
02197 
02198      void BecomeParentOf( Model* child );
02199 
02200      void Load( Worldfile* wf, int wf_entity )
02201      {
02204         SetWorldfile( wf, wf_entity );
02205         Load(); // call virtual load
02206      }
02207     
02209      void SetWorldfile( Worldfile* wf, int wf_entity )
02210      { this->wf = wf; this->wf_entity = wf_entity; }
02211     
02213      virtual void Load();
02214     
02216      virtual void Save();
02217     
02219      void InitControllers();
02220 
02221      void AddFlag(  Flag* flag );
02222      void RemoveFlag( Flag* flag );
02223     
02224      void PushFlag( Flag* flag );
02225      Flag* PopFlag();
02226     
02227      unsigned int GetFlagCount() const { return flag_list.size(); }
02228     
02233      void Disable(){ disabled = true; };
02234 
02237      void Enable(){ disabled = false; };
02238   
02241      void LoadControllerModule( const char* lib );
02242     
02245      void NeedRedraw();
02246         
02248         void Redraw();
02249 
02252      void LoadBlock( Worldfile* wf, int entity );
02253 
02256         Block*  AddBlockRect( meters_t x, meters_t y, 
02257                                                     meters_t dx, meters_t dy, 
02258                                                     meters_t dz );
02259         
02261      void ClearBlocks();
02262   
02265      Model* Parent() const { return this->parent; }
02266 
02268      World* GetWorld() const { return this->world; }
02269   
02271      Model* Root(){ return(  parent ? parent->Root() : this ); }
02272   
02273      bool IsAntecedent( const Model* testmod ) const;
02274     
02276      bool IsDescendent( const Model* testmod ) const;
02277     
02279      bool IsRelated( const Model* testmod ) const;
02280 
02282      Pose GetGlobalPose() const;
02283     
02285      Velocity GetGlobalVelocity()  const;
02286     
02287      /* set the velocity of a model in the global coordinate system */
02288      void SetGlobalVelocity( const Velocity& gvel );
02289     
02291      void Subscribe();
02292     
02294      void Unsubscribe();
02295     
02297      void SetGlobalPose(  const Pose& gpose );
02298     
02300      void SetVelocity(  const Velocity& vel );
02301     
02303         void VelocityEnable();
02304 
02306         void VelocityDisable();
02307 
02309      void SetPose(  const Pose& pose );
02310     
02312      void AddToPose(  const Pose& pose );
02313     
02315      void AddToPose(  double dx, double dy, double dz, double da );
02316     
02318      void SetGeom(  const Geom& src );
02319   
02322      void SetFiducialReturn(  int fid );
02323   
02325      int GetFiducialReturn()  const { return vis.fiducial_return; }
02326   
02329      void SetFiducialKey(  int key );
02330     
02331      Color GetColor() const { return color; }
02332      
02334      uint32_t GetId()  const { return id; }
02335      
02337      kg_t GetTotalMass() const;
02338      
02340      kg_t GetMassOfChildren() const;
02341 
02343      int SetParent( Model* newparent);
02344     
02347      Geom GetGeom() const { return geom; }
02348     
02351      Pose GetPose() const { return pose; }
02352     
02355      Velocity GetVelocity() const { return velocity; }
02356     
02357      // guess what these do?
02358      void SetColor( Color col );
02359      void SetMass( kg_t mass );
02360      void SetStall( bool stall );
02361      void SetGravityReturn( bool val );
02362      void SetGripperReturn( bool val );
02363      void SetStickyReturn( bool val );
02364      void SetRangerReturn( float val );
02365      void SetObstacleReturn( bool val );
02366      void SetBlobReturn( bool val );
02367      void SetRangerReturn( bool val );
02368      void SetBoundary( bool val );
02369      void SetGuiNose( bool val );
02370      void SetGuiMove( bool val );
02371      void SetGuiGrid( bool val );
02372      void SetGuiOutline( bool val );
02373      void SetWatts( watts_t watts );
02374      void SetMapResolution( meters_t res );
02375      void SetFriction( double friction );
02376     
02377      bool DataIsFresh() const { return this->data_fresh; }
02378         
02379         /* attach callback functions to data members. The function gets
02380              called when the member is changed using SetX() accessor method */
02381         
02388         void AddCallback( callback_type_t type, 
02389                                             model_callback_t cb, 
02390                                             void* user );
02391         
02392         int RemoveCallback( callback_type_t type,
02393                                                 model_callback_t callback );
02394         
02395         int CallCallbacks(  callback_type_t type );
02396         
02397         
02398      virtual void Print( char* prefix ) const;
02399      virtual const char* PrintWithPose() const;
02400     
02403      Pose GlobalToLocal( const Pose& pose ) const;
02404      
02407      Pose LocalToGlobal( const Pose& pose ) const
02408      {  
02409         return( ( GetGlobalPose() + geom.pose ) + pose );
02410      }
02411         
02413         void LocalToPixels( const std::vector<point_t>& local,
02414                                                 std::vector<point_int_t>& pixels) const;
02415         
02418      point_t LocalToGlobal( const point_t& pt) const;       
02419 
02422      Model* GetUnsubscribedModelOfType( const std::string& type ) const;
02423     
02426      Model* GetUnusedModelOfType( const std::string& type );
02427   
02430      bool Stalled() const { return this->stall; }
02431      
02434      unsigned int GetSubscriptionCount() const { return subs; }
02435 
02437      bool HasSubscribers() const { return( subs > 0 ); }     
02438 
02439      static std::map< std::string, creator_t> name_map;  
02440 
02441 //      class Neighbors
02442 //      {
02443 //          Model *left, *right, *up, *down;
02444 //      public:
02445 //          Neighbors() : left(NULL), right(NULL), up(NULL), down(NULL) {}
02446 //      } nbors; // instance
02447 
02448                             
02449   };
02450 
02451 
02452   // BLOBFINDER MODEL --------------------------------------------------------
02453 
02454 
02456   class ModelBlobfinder : public Model
02457   {
02458   public:
02460      class Blob
02461      {
02462      public:
02463         Color color;
02464         uint32_t left, top, right, bottom;
02465         meters_t range;
02466      };
02467 
02468      class Vis : public Visualizer 
02469      {
02470      private:
02471         //static Option showArea;
02472      public:
02473         Vis( World* world );
02474         virtual ~Vis( void ){}
02475         virtual void Visualize( Model* mod, Camera* cam );
02476      } vis;
02477 
02478   private:
02479      std::vector<Blob> blobs;
02480      std::vector<Color> colors;
02481 
02482      // predicate for ray tracing
02483      static bool BlockMatcher( Block* testblock, Model* finder );
02484 
02485   public:
02486      radians_t fov;
02487      radians_t pan;
02488      meters_t range;
02489      unsigned int scan_height;
02490      unsigned int scan_width;
02491      
02492      // constructor
02493      ModelBlobfinder( World* world,
02494                             Model* parent,
02495                             const std::string& type );
02496      // destructor
02497      ~ModelBlobfinder();
02498     
02499      virtual void Startup();
02500      virtual void Shutdown();
02501      virtual void Update();
02502      virtual void Load();
02503         
02504      Blob* GetBlobs( unsigned int* count )
02505      { 
02506         if( count ) *count = blobs.size();
02507         return &blobs[0];
02508      }
02509 
02510      std::vector<Blob> GetBlobs() const { return blobs; }
02511 
02513      void AddColor( Color col );
02514 
02516      void RemoveColor( Color col );
02517 
02520      void RemoveAllColors();
02521   };
02522 
02523 
02524 
02525   
02526 
02527   // Light indicator model
02528   class ModelLightIndicator : public Model
02529   {
02530   public:
02531      ModelLightIndicator( World* world, 
02532                                  Model* parent,
02533                                  const std::string& type );
02534      ~ModelLightIndicator();
02535   
02536      void SetState(bool isOn);
02537 
02538   protected:
02539      virtual void DrawBlocks();
02540 
02541   private:
02542      bool m_IsOn;
02543   };
02544 
02545   // \todo  GRIPPER MODEL --------------------------------------------------------
02546 
02547 
02548   class ModelGripper : public Model
02549   {
02550   public:
02551 
02552      enum paddle_state_t {
02553         PADDLE_OPEN = 0, // default state
02554         PADDLE_CLOSED, 
02555         PADDLE_OPENING,
02556         PADDLE_CLOSING,
02557      };
02558      
02559      enum lift_state_t {
02560         LIFT_DOWN = 0, // default state
02561         LIFT_UP, 
02562         LIFT_UPPING, // verbed these to match the paddle state
02563         LIFT_DOWNING, 
02564      };
02565      
02566      enum cmd_t {
02567         CMD_NOOP = 0, // default state
02568         CMD_OPEN, 
02569         CMD_CLOSE,
02570         CMD_UP, 
02571         CMD_DOWN    
02572      };
02573      
02574      
02577      struct config_t
02578      {
02579         Size paddle_size; 
02580         paddle_state_t paddles;
02581         lift_state_t lift;      
02582         double paddle_position; 
02583         double lift_position; 
02584         Model* gripped;
02585         bool paddles_stalled; // true iff some solid object stopped the paddles closing or opening
02586         double close_limit; 
02587         bool autosnatch; 
02588         double break_beam_inset[2]; 
02589       Model* beam[2]; 
02590       Model* contact[2]; 
02591      };
02592      
02593   private:
02594      virtual void Update();
02595      virtual void DataVisualize( Camera* cam );
02596      
02597      void FixBlocks();
02598      void PositionPaddles();
02599      void UpdateBreakBeams();
02600      void UpdateContacts();
02601 
02602      config_t cfg;
02603      cmd_t cmd;
02604      
02605      Block* paddle_left;
02606      Block* paddle_right;
02607 
02608      static Option showData;
02609 
02610   public:    
02611      static const Size size;
02612 
02613      // constructor
02614      ModelGripper( World* world,
02615                         Model* parent,
02616                         const std::string& type );
02617      // destructor
02618      virtual ~ModelGripper();
02619   
02620      virtual void Load();
02621      virtual void Save();
02622 
02624      void SetConfig( config_t & newcfg ){ this->cfg = newcfg; FixBlocks(); }
02625      
02627      config_t GetConfig(){ return cfg; };
02628      
02630      void SetCommand( cmd_t cmd ) { this->cmd = cmd; }
02632      void CommandClose() { SetCommand( CMD_CLOSE ); }
02634      void CommandOpen() { SetCommand( CMD_OPEN ); }
02636      void CommandUp() { SetCommand( CMD_UP ); }
02638      void CommandDown() { SetCommand( CMD_DOWN ); }
02639   };
02640 
02641 
02642   // \todo BUMPER MODEL --------------------------------------------------------
02643 
02644   //   typedef struct
02645   //   {
02646   //     Pose pose;
02647   //     meters_t length;
02648   //   } bumper_config_t;
02649 
02650   //   typedef struct
02651   //   {
02652   //     Model* hit;
02653   //     point_t hit_point;
02654   //   } bumper_sample_t;
02655 
02656 
02657   // FIDUCIAL MODEL --------------------------------------------------------
02658 
02660   class ModelFiducial : public Model
02661   {
02662   public:  
02664      class Fiducial
02665      {
02666      public:
02667         meters_t range; 
02668         radians_t bearing; 
02669         Pose geom; 
02670          //Pose pose_rel; /// relative pose of the target in local coordinates       
02671         Pose pose; 
02672         Model* mod; 
02673         int id; 
02674      };
02675 
02676   private:
02677      // if neighbor is visible, add him to the fiducial scan
02678      void AddModelIfVisible( Model* him );
02679 
02680      virtual void Update();
02681      virtual void DataVisualize( Camera* cam );
02682 
02683      static Option showData;
02684      static Option showFov;
02685      
02686      std::vector<Fiducial> fiducials;
02687         
02688   public:       
02689      ModelFiducial( World* world, 
02690                          Model* parent,
02691                          const std::string& type );
02692      virtual ~ModelFiducial();
02693     
02694      virtual void Load();
02695      void Shutdown( void );
02696 
02697      meters_t max_range_anon;
02698      meters_t max_range_id; 
02699      meters_t min_range; 
02700      radians_t fov; 
02701      radians_t heading; 
02702      int key; 
02703     bool ignore_zloc;  
02704         
02706      std::vector<Fiducial>& GetFiducials() { return fiducials; }
02707         
02709      Fiducial* GetFiducials( unsigned int* count )
02710      {
02711         if( count ) *count = fiducials.size();
02712         return &fiducials[0];
02713      }
02714   };
02715     
02716     
02717   // RANGER MODEL --------------------------------------------------------
02718     
02720   class ModelRanger : public Model
02721   {
02722   public:
02723   public:
02724         ModelRanger( World* world, Model* parent,
02725                                  const std::string& type );
02726         virtual ~ModelRanger();
02727         
02728         virtual void Load();
02729         virtual void Print( char* prefix ) const;
02730         
02731         class Vis : public Visualizer 
02732         {
02733         public:
02734             static Option showArea;
02735             static Option showStrikes;
02736             static Option showFov;
02737             static Option showBeams;
02738             static Option showTransducers;      
02739             
02740             Vis( World* world );        
02741           virtual ~Vis( void ){} 
02742             virtual void Visualize( Model* mod, Camera* cam );
02743         } vis;
02744         
02745         class Sensor
02746         {       
02747         public:
02748             Pose pose; 
02749             Size size;
02750             Bounds range;
02751             radians_t fov;
02752             unsigned int sample_count;
02753             Color col;
02754             
02755             std::vector<meters_t> ranges;
02756             std::vector<double> intensities;
02757             
02758             Sensor() : pose( 0,0,0,0 ), 
02759                                  size( 0.02, 0.02, 0.02 ), // teeny transducer
02760                                  range( 0.0, 5.0 ),
02761                                  fov( 0.1 ), 
02762                                  sample_count(1),
02763                                  col( 0,1,0,0.3 ),
02764                                  ranges(),
02765                                  intensities()
02766             {}
02767             
02768             void Update( ModelRanger* rgr );            
02769             void Visualize( Vis* vis, ModelRanger* rgr ) const;
02770             std::string String() const;         
02771             void Load( Worldfile* wf, int entity );
02772         };
02773 
02775      const std::vector<Sensor>& GetSensors() const
02776      { return sensors; }
02777      
02780      const std::vector<meters_t>& GetRanges( unsigned int sensor=0) const 
02781      { 
02782         if( sensor < sensors.size() )
02783           return sensors[sensor].ranges;
02784         
02785         PRINT_ERR1( "invalid sensor index specified (%d)", sensor );
02786         exit(-1);
02787      }
02788 
02791      std::vector<meters_t>& GetRangesMutable( unsigned int sensor=0) 
02792      { 
02793         if( sensor < sensors.size() )
02794           return sensors[sensor].ranges;
02795         
02796         PRINT_ERR1( "invalid sensor index specified (%d)", sensor );
02797         exit(-1);
02798      }
02799         
02802         meters_t* GetRangesArr( unsigned int sensor, uint32_t* count )
02803         {
02804             assert(count);
02805             *count = sensors[sensor].ranges.size();
02806             return &sensors[sensor].ranges[0];
02807         }
02808 
02811         meters_t* GetIntensitiesArr( unsigned int sensor, uint32_t* count )
02812         {
02813             assert(count);
02814             *count = sensors[sensor].intensities.size();
02815             return &sensors[sensor].intensities[0];
02816         }
02817         
02820      const std::vector<double>& GetIntensities( unsigned int sensor=0) const 
02821      { 
02822         if( sensor < sensors.size() )
02823           return sensors[sensor].intensities;
02824         
02825         PRINT_ERR1( "invalid sensor index specified (%d)", sensor );
02826         exit(-1);
02827      }
02828      
02829         void LoadSensor( Worldfile* wf, int entity );
02830         
02831   private:
02832         std::vector<Sensor> sensors;        
02833         
02834   protected:
02835         
02836         virtual void Startup();
02837         virtual void Shutdown();
02838         virtual void Update();      
02839   };
02840     
02841   // BLINKENLIGHT MODEL ----------------------------------------------------
02842   class ModelBlinkenlight : public Model
02843   {
02844   private:
02845      double dutycycle;
02846      bool enabled;
02847      msec_t period;
02848      bool on;
02849 
02850      static Option showBlinkenData;
02851   public:
02852      ModelBlinkenlight( World* world,
02853                               Model* parent,
02854                               const std::string& type );
02855 
02856      ~ModelBlinkenlight();
02857     
02858      virtual void Load();
02859      virtual void Update();
02860      virtual void DataVisualize( Camera* cam );
02861   };
02862 
02863     
02864   // CAMERA MODEL ----------------------------------------------------
02865 
02867   class ModelCamera : public Model
02868   {
02869   public:
02870      typedef struct 
02871      {
02872         // GL_V3F
02873         GLfloat x, y, z;
02874      } ColoredVertex;
02875   
02876   private:
02877      Canvas* _canvas;
02878 
02879      GLfloat* _frame_data;  //opengl read buffer
02880      GLubyte* _frame_color_data;  //opengl read buffer
02881 
02882      bool _valid_vertexbuf_cache;
02883      ColoredVertex* _vertexbuf_cache; //cached unit vectors with appropriate rotations (these must be scalled by z-buffer length)
02884     
02885      int _width;         //width of buffer
02886      int _height;        //height of buffer
02887      static const int _depth = 4;
02888     
02889      int _camera_quads_size;
02890      GLfloat* _camera_quads;
02891      GLubyte* _camera_colors;
02892     
02893      static Option showCameraData;
02894     
02895      PerspectiveCamera _camera;
02896      float _yaw_offset; //position camera is mounted at
02897      float _pitch_offset;
02898         
02900      bool GetFrame();
02901     
02902   public:
02903      ModelCamera( World* world,
02904                       Model* parent,
02905                       const std::string& type ); 
02906       
02907      ~ModelCamera();
02908   
02909      virtual void Load();
02910     
02912      virtual void Update();
02913     
02915      //virtual void Draw( uint32_t flags, Canvas* canvas );
02916     
02918      virtual void DataVisualize( Camera* cam );
02919     
02921      int getWidth( void ) const { return _width; }
02922     
02924      int getHeight( void ) const { return _height; }
02925     
02927      const PerspectiveCamera& getCamera( void ) const { return _camera; }
02928     
02930      const GLfloat* FrameDepth() const { return _frame_data; }
02931     
02933      const GLubyte* FrameColor() const { return _frame_color_data; }
02934     
02936      void setPitch( float pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; }
02937     
02939      void setYaw( float yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; }
02940   };
02941 
02942   // POSITION MODEL --------------------------------------------------------
02943 
02945   class ModelPosition : public Model
02946   {
02947      friend class Canvas;
02948 
02949   public:
02951      typedef enum
02952         { CONTROL_VELOCITY, 
02953           CONTROL_POSITION 
02954         } ControlMode;
02955      
02957      typedef enum
02958         { LOCALIZATION_GPS, 
02959           LOCALIZATION_ODOM 
02960         } LocalizationMode;
02961      
02963      typedef enum
02964         { DRIVE_DIFFERENTIAL, 
02965           DRIVE_OMNI, 
02966           DRIVE_CAR 
02967         } DriveMode;
02968      
02969   private:
02970      Pose goal;
02971      ControlMode control_mode;
02972      DriveMode drive_mode;   
02973      LocalizationMode localization_mode; 
02974      Velocity integration_error; 
02975      double wheelbase;
02976      
02977   public:
02978      // constructor
02979      ModelPosition( World* world,
02980                          Model* parent,
02981                          const std::string& type );
02982      // destructor
02983      ~ModelPosition();
02984 
02985      virtual void Startup();
02986      virtual void Shutdown();
02987      virtual void Update();
02988      virtual void Load();
02989         
02992      class Waypoint
02993      {
02994      public:
02995         Waypoint( meters_t x, meters_t y, meters_t z, radians_t a, Color color ) ;
02996         Waypoint( const Pose& pose, Color color ) ;
02997         Waypoint();
02998         void Draw() const;
02999         
03000         Pose pose;
03001         Color color;
03002      };
03003      
03004      std::vector<Waypoint> waypoints;
03005 
03006      class WaypointVis : public Visualizer
03007      {
03008      public:
03009         WaypointVis();
03010         virtual ~WaypointVis( void ){}
03011         virtual void Visualize( Model* mod, Camera* cam );
03012      } wpvis;
03013      
03014      class PoseVis : public Visualizer
03015      {
03016      public:
03017         PoseVis();
03018         virtual ~PoseVis( void ){}
03019         virtual void Visualize( Model* mod, Camera* cam );
03020      } posevis;
03021 
03023      void SetOdom( Pose odom );
03024         
03027      void SetSpeed( double x, double y, double a );
03028      void SetXSpeed( double x );
03029      void SetYSpeed( double y );
03030      void SetZSpeed( double z );
03031      void SetTurnSpeed( double a );
03032      void SetSpeed( Velocity vel );
03034      void Stop();
03035 
03038      void GoTo( double x, double y, double a );
03039      void GoTo( Pose pose );
03040 
03041      // localization state
03042      Pose est_pose; 
03043      Pose est_pose_error; 
03044      Pose est_origin; 
03045   };
03046 
03047 
03048   // ACTUATOR MODEL --------------------------------------------------------
03049 
03051   class ModelActuator : public Model
03052   {
03053   public:
03055      typedef enum
03056         { CONTROL_VELOCITY,
03057           CONTROL_POSITION
03058         } ControlMode;
03059   
03061      typedef enum
03062         { TYPE_LINEAR,
03063           TYPE_ROTATIONAL
03064         } ActuatorType;
03065   
03066   private:
03067      double goal; //< the current velocity or pose to reach, depending on the value of control_mode
03068      double pos;
03069      double max_speed;
03070      double min_position;
03071      double max_position;
03072      double start_position;
03073      double cosa;
03074      double sina;
03075      ControlMode control_mode;
03076      ActuatorType actuator_type;
03077      point3_t axis;
03078   
03079      Pose InitialPose;
03080   public:  
03081      // constructor
03082      ModelActuator( World* world,
03083                          Model* parent,
03084                          const std::string& type );
03085      // destructor
03086      ~ModelActuator();
03087   
03088      virtual void Startup();
03089      virtual void Shutdown();
03090      virtual void Update();
03091      virtual void Load();
03092   
03095      void SetSpeed( double speed );
03096   
03097      double GetSpeed() const {return goal;}
03098   
03101      void GoTo( double pose );
03102   
03103         double GetPosition() const {return pos;};
03104         double GetMaxPosition() const {return max_position;};
03105         double GetMinPosition() const {return min_position;};
03106         
03107         ActuatorType GetType() const { return actuator_type; }
03108         point3_t GetAxis() const { return axis; }
03109   };
03110 
03111 
03112 }; // end namespace stg
03113 
03114 #endif