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   
00091   void Init( int* argc, char** argv[] );
00092 
00094   bool InitDone();
00095   
00098   const char* Version();
00099 
00101   const char COPYRIGHT[] =                     
00102     "Copyright Richard Vaughan and contributors 2000-2009";
00103 
00105   const char AUTHORS[] =                    
00106     "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, Toby Collett, Jeremy Asher, Alex Couture-Beil and contributors.";
00107 
00109   const char WEBSITE[] = "http://playerstage.org";
00110 
00112   const char DESCRIPTION[] =                       
00113     "Robot simulation library\nPart of the Player Project";
00114 
00116   const char LICENSE[] = 
00117     "Stage robot simulation library\n"                  \
00118     "Copyright (C) 2000-2009 Richard Vaughan and contributors\n"    \
00119     "Part of the Player Project [http://playerstage.org]\n"     \
00120     "\n"                                \
00121     "This program is free software; you can redistribute it and/or\n"   \
00122     "modify it under the terms of the GNU General Public License\n" \
00123     "as published by the Free Software Foundation; either version 2\n"  \
00124     "of the License, or (at your option) any later version.\n"      \
00125     "\n"                                \
00126     "This program is distributed in the hope that it will be useful,\n" \
00127     "but WITHOUT ANY WARRANTY; without even the implied warranty of\n"  \
00128     "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n"   \
00129     "GNU General Public License for more details.\n"            \
00130     "\n"                                \
00131     "You should have received a copy of the GNU General Public License\n" \
00132     "along with this program; if not, write to the Free Software\n" \
00133     "Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.\n" \
00134     "\n"                                \
00135     "The text of the license may also be available online at\n"     \
00136     "http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";
00137 
00139   const double thousand = 1e3;
00140 
00142   const double million = 1e6;
00143 
00145   const double billion = 1e9;
00146 
00148   inline double rtod( double r ){ return( r*180.0/M_PI ); }
00149   
00151   inline double dtor( double d ){ return( d*M_PI/180.0 ); }
00152   
00154   inline double normalize( double a )
00155   {
00156     while( a < -M_PI ) a += 2.0*M_PI;
00157     while( a >  M_PI ) a -= 2.0*M_PI;    
00158     return a;
00159   };
00160     
00162   inline int sgn( int a){ return( a<0 ? -1 : 1); }
00163     
00165   inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); }
00166   
00168   enum { FiducialNone = 0 };
00169 
00171   typedef uint32_t id_t;
00172 
00174   typedef double meters_t;
00175 
00177   typedef double radians_t;
00178 
00180   typedef struct timeval time_t;
00181 
00183   typedef unsigned long msec_t;
00184 
00186   typedef uint64_t usec_t;
00187 
00189   typedef double kg_t; // Kilograms (mass)
00190 
00192   typedef double joules_t;
00193 
00195   typedef double watts_t;
00196   
00197   class Color
00198   {
00199   public:
00200     double r,g,b,a;
00201     
00202     Color( double r, double g, double b, double a=1.0 );
00203     
00207     Color( const std::string& name );   
00208     
00209     Color();
00210     
00211     bool operator!=( const Color& other ) const;
00212     bool operator==( const Color& other ) const;
00213     static Color RandomColor();
00214     void Print( const char* prefix ) const;
00215 
00217     static const Color blue, red, green, yellow, magenta, cyan;
00218 
00219     const Color& Load( Worldfile* wf, int entity );
00220 
00221     void GLSet( void ) { glColor4f( r,g,b,a ); }
00222   };
00223   
00225   class Size
00226   {
00227   public:
00228     meters_t x, y, z;
00229     
00230     Size( meters_t x, 
00231       meters_t y, 
00232       meters_t z )
00233       : x(x), y(y), z(z)
00234     {/*empty*/}
00235     
00237     Size() : x( 0.4 ), y( 0.4 ), z( 1.0 )
00238     {/*empty*/} 
00239     
00240     Size& Load( Worldfile* wf, int section, const char* keyword );
00241     void Save( Worldfile* wf, int section, const char* keyword ) const;
00242      
00243     void Zero()
00244     { x=y=z=0.0; }
00245   };
00246   
00248   class Pose
00249   {
00250   public:
00251     meters_t x, y, z;
00252     radians_t a;
00253     
00254     Pose( meters_t x, 
00255       meters_t y, 
00256       meters_t z,
00257       radians_t a ) 
00258       : x(x), y(y), z(z), a(a)
00259     { /*empty*/ }
00260     
00261     Pose() : x(0.0), y(0.0), z(0.0), a(0.0)
00262     { /*empty*/ }        
00263     
00264     virtual ~Pose(){};
00265     
00268     static Pose Random( meters_t xmin, meters_t xmax, 
00269             meters_t ymin, meters_t ymax )
00270     {        
00271       return Pose( xmin + drand48() * (xmax-xmin),
00272            ymin + drand48() * (ymax-ymin),
00273            0, 
00274            normalize( drand48() * (2.0 * M_PI) ));
00275     }
00276     
00280     virtual void Print( const char* prefix ) const
00281     {
00282       printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
00283           prefix, x,y,z,a );
00284     }
00285     
00286     std::string String() const
00287     {
00288       char buf[256];
00289       snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]",
00290         x,y,z,a );
00291       return std::string(buf);
00292     }
00293     
00294     /* returns true iff all components of the velocity are zero. */
00295     bool IsZero() const 
00296     { return( !(x || y || z || a )); };
00297     
00299     void Zero()
00300     { x=y=z=a=0.0; }
00301     
00302     Pose& Load( Worldfile* wf, int section, const char* keyword );
00303     void Save( Worldfile* wf, int section, const char* keyword );
00304     
00305     inline Pose operator+( const Pose& p ) const
00306     {
00307       const double cosa = cos(a);
00308       const double sina = sin(a);
00309       
00310       return Pose( x + p.x * cosa - p.y * sina,
00311            y + p.x * sina + p.y * cosa,
00312            z + p.z,
00313            normalize(a + p.a) );     
00314     }   
00315      
00316     // a < b iff a is closer to the origin than b
00317     bool operator<( const Pose& other ) const
00318     {
00319       return( hypot( y, x ) < hypot( other.y, other.x ));
00320     }
00321      
00322     bool operator==( const Pose& other ) const
00323     {
00324       return(  x==other.x && 
00325            y==other.y && 
00326            z==other.z && 
00327            a==other.a );
00328     }
00329 
00330     bool operator!=( const Pose& other ) const
00331     {
00332       return(  x!=other.x ||
00333            y!=other.y || 
00334            z!=other.z || 
00335            a!=other.a );
00336     }
00337      
00338     meters_t Distance2D( const Pose& other ) const
00339     {
00340       return hypot( x-other.x, y-other.y );
00341     }
00342 
00343   };
00344   
00345   
00348   class Velocity : public Pose
00349   {
00350   public:
00356     Velocity( double x, 
00357           double y, 
00358           double z,
00359           double a ) :
00360       Pose( x, y, z, a )
00361     { /*empty*/ }
00362     
00363     Velocity()
00364     { /*empty*/ }        
00365     
00372     Velocity& Load( Worldfile* wf, int section, const char* keyword )
00373     {
00374       Pose::Load( wf, section, keyword );
00375       return *this;
00376     }
00377     
00378     virtual void Print( const char* prefix ) const
00379     {
00380       if( prefix )
00381     printf( "%s", prefix );
00382       
00383       printf( "velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
00384           x,y,z,a );        
00385     }    
00386   };
00387   
00388  
00391   class Geom
00392   {
00393   public:
00394     Pose pose;
00395     Size size;
00396     
00402     void Print( const char* prefix ) const
00403     {
00404       if( prefix )
00405     printf( "%s", prefix );
00406         
00407       printf( "geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n",
00408           pose.x,
00409           pose.y,
00410           pose.a,
00411           size.x,
00412           size.y );
00413     }
00414      
00416     Geom() : pose(), size() {}
00417 
00419     Geom( const Pose& p, const Size& s ) : pose(p), size(s) {}
00420 
00421     void Zero()
00422     {
00423       pose.Zero();
00424       size.Zero();
00425     }
00426   };
00427   
00429   class Bounds
00430   {
00431   public:
00433     double min;
00435     double max; 
00436     
00437     Bounds() : min(0), max(0) { /* empty*/  }
00438     Bounds( double min, double max ) : min(min), max(max) { /* empty*/  }
00439 
00440     Bounds& Load( Worldfile* wf, int section, const char* keyword );
00441 
00442     // returns value, but no smaller than min and no larger than max.
00443     double Constrain( double value );
00444   };
00445     
00447   class bounds3d_t
00448   {
00449   public:
00451     Bounds x; 
00453     Bounds y; 
00455     Bounds z; 
00456 
00457     bounds3d_t() : x(), y(), z() {}
00458     bounds3d_t( const Bounds& x, const Bounds& y, const Bounds& z) 
00459       : x(x), y(y), z(z) {}
00460   };
00461   
00463   typedef struct
00464   {
00465     Bounds range; 
00466     radians_t angle; 
00467   } fov_t;
00468   
00470   class point_t
00471   {
00472   public:
00473     meters_t x, y;
00474     point_t( meters_t x, meters_t y ) : x(x), y(y){}     
00475     point_t() : x(0.0), y(0.0){}
00476     
00477     bool operator+=( const point_t& other ) 
00478     { return ((x += other.x) && (y += other.y) ); }  
00479   };
00480   
00482   class point3_t
00483   {
00484   public:
00485     meters_t x,y,z;
00486     point3_t( meters_t x, meters_t y, meters_t z ) 
00487       : x(x), y(y), z(z) {}  
00488   
00489     point3_t() : x(0.0), y(0.0), z(0.0) {}
00490   };
00491   
00493   class point_int_t
00494   {
00495   public:
00496     int x,y;
00497     point_int_t( int x, int y ) : x(x), y(y){}   
00498     point_int_t() : x(0), y(0){}
00499         
00501     bool operator<( const point_int_t& other ) const
00502     {
00503       if( x < other.x ) return true;
00504       if( other.x < x ) return false;
00505       return y < other.y;
00506     }
00507         
00508     bool operator==( const point_int_t& other ) const
00509     { return ((x == other.x) && (y == other.y) ); }
00510   };
00511   
00514   point_t* unit_square_points_create();
00515   
00518   namespace Gl
00519   {
00520     void pose_shift( const Pose &pose );
00521     void pose_inverse_shift( const Pose &pose );
00522     void coord_shift( double x, double y, double z, double a  );
00523     void draw_grid( bounds3d_t vol );
00525     void draw_string( float x, float y, float z, const char *string);
00526     void draw_string_multiline( float x, float y, float w, float h, 
00527                 const char *string, Fl_Align align );
00528     void draw_speech_bubble( float x, float y, float z, const char* str );
00529     void draw_octagon( float w, float h, float m );
00530     void draw_octagon( float x, float y, float w, float h, float m );
00531     void draw_vector( double x, double y, double z );
00532     void draw_origin( double len );
00533     void draw_array( float x, float y, float w, float h, 
00534              float* data, size_t len, size_t offset, 
00535              float min, float max );
00536     void draw_array( float x, float y, float w, float h, 
00537              float* data, size_t len, size_t offset );
00539     void draw_centered_rect( float x, float y, float dx, float dy );
00540   } // namespace Gl
00541   
00542   void RegisterModels();
00543   
00544   
00546   class Visualizer {
00547   private:
00548     const std::string menu_name;
00549     const std::string worldfile_name;
00550      
00551   public:
00552     Visualizer( const std::string& menu_name, 
00553         const std::string& worldfile_name ) 
00554       : menu_name( menu_name ),
00555     worldfile_name( worldfile_name )
00556     { }
00557      
00558     virtual ~Visualizer( void ) { }
00559     virtual void Visualize( Model* mod, Camera* cam ) = 0;
00560      
00561     const std::string& GetMenuName() { return menu_name; }
00562     const std::string& GetWorldfileName() { return worldfile_name; }     
00563   };
00564 
00565 
00568   typedef int(*model_callback_t)(Model* mod, void* user );
00569 
00570   typedef int(*world_callback_t)(World* world, void* user );
00571   
00572   // return val, or minval if val < minval, or maxval if val > maxval
00573   double constrain( double val, double minval, double maxval );
00574     
00575   typedef struct 
00576   {
00577     int enabled;
00578     Pose pose;
00579     meters_t size; 
00580     Color color;
00581     msec_t period; 
00582     double duty_cycle; 
00583   } blinkenlight_t;
00584 
00585   
00587   typedef struct
00588   {
00589     Pose pose;
00590     Size size;
00591   } rotrect_t; // rotated rectangle
00592   
00597   int rotrects_from_image_file( const std::string& filename, 
00598                 std::vector<rotrect_t>& rects );
00599   
00603   typedef bool (*ray_test_func_t)(Model* candidate, 
00604                   Model* finder, 
00605                   const void* arg );
00606 
00607   // STL container iterator macros - __typeof is a gcc extension, so
00608   // this could be an issue one day.
00609 #define VAR(V,init) __typeof(init) V=(init)
00610 
00611   //#define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();++I) 
00612 
00613   // NOTE:
00614   // this version assumes the container is not modified in the loop,
00615   // which I think is true everywhere it is used in Stage
00616 #define FOR_EACH(I,C) for(VAR(I,(C).begin()),ite=(C).end();(I)!=ite;++(I))
00617 
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 
00677 
00678   // ANCESTOR CLASS
00680   class Ancestor
00681   {
00682     friend class Canvas; // allow Canvas access to our private members
00683      
00684   protected:
00685 
00687     std::map<std::string,unsigned int> child_type_counts;
00688 
00689     std::vector<Model*> children;
00690 
00691     bool debug;
00692 
00694     std::map<std::string,void*> props;
00695 
00696     std::string token;
00697 
00698     Ancestor& Load( Worldfile* wf, int section );
00699     void Save( Worldfile* wf, int section );     
00700              
00701   public:   
00702     Ancestor();
00703     virtual ~Ancestor();
00704      
00706     std::vector<Model*>& GetChildren(){ return children;}
00707     
00709     void ForEachDescendant( model_callback_t func, void* arg );
00710     
00711     virtual void AddChild( Model* mod );
00712     virtual void RemoveChild( Model* mod );
00713     virtual Pose GetGlobalPose() const;
00714     
00715     const char* Token() const { return token.c_str(); }
00716     const std::string& TokenStr() const { return token; }
00717     
00718     virtual void SetToken( const std::string& str )
00719     { 
00720       //printf( "Ancestor::SetToken( %s )\n", str.c_str() );
00721 
00722       if( str.size() > 0 ) 
00723     token = str;
00724       else
00725     PRINT_ERR( "Ancestor::SetToken() called with zero length string. Ignored." );
00726     } 
00727      
00729     void SetProperty( std::string& key, void* value ){ props[ key ] = value; }
00730      
00732     void* GetProperty( std::string& key )
00733     {
00734       std::map<std::string,void*>::iterator it = props.find( key );     
00735       return( it == props.end() ? NULL : it->second );
00736     }
00737   };
00738 
00741   class RaytraceResult
00742   {
00743   public:
00744     Pose pose; 
00745     meters_t range; 
00746     Model* mod; 
00747     Color color; 
00748      
00749     RaytraceResult() : pose(), range(0), mod(NULL), color() {}
00750     RaytraceResult( const Pose& pose, 
00751             meters_t range ) 
00752       : pose(pose), range(range), mod(NULL), color() {}  
00753   };
00754     
00755   class Ray
00756   {
00757   public:
00758     Ray( const Model* mod, const Pose& origin, const meters_t range, const ray_test_func_t func, const void* arg, const bool ztest ) :
00759       mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest)
00760     {}
00761 
00762     Ray() : mod(NULL), origin(0,0,0,0), range(0), func(NULL), arg(NULL), ztest(true)
00763     {}
00764         
00765     const Model* mod;
00766     Pose origin;
00767     meters_t range;
00768     ray_test_func_t func;
00769     const void* arg;
00770     bool ztest;     
00771   };
00772         
00773 
00774   // defined in stage_internal.hh
00775   class Region;
00776   class SuperRegion;
00777   class BlockGroup;
00778   class PowerPack;
00779 
00780   class LogEntry
00781   {
00782     usec_t timestamp;
00783     Model* mod;
00784     Pose pose;
00785      
00786   public:
00787     LogEntry( usec_t timestamp, Model* mod );
00788      
00790     static std::vector<LogEntry> log;
00791      
00793     static size_t Count(){ return log.size(); }
00794      
00796     static void Clear(){ log.clear(); }
00797 
00799     static void Print();
00800   };
00801 
00802   class CtrlArgs
00803   {
00804   public:
00805     std::string worldfile;
00806     std::string cmdline;
00807 
00808     CtrlArgs( std::string w, std::string c ) : worldfile(w), cmdline(c) {}
00809   };
00810 
00811   class ModelPosition;
00812 
00814   class World : public Ancestor
00815   {
00816   public:
00817     friend class Block;
00818     friend class Model; // allow access to private members
00819     friend class ModelFiducial;
00820     friend class Canvas;
00821     friend class WorkerThread;
00822 
00823   public: 
00826     static std::vector<std::string> args;
00827     static std::string ctrlargs;
00828 
00829   private:
00830     
00831     static std::set<World*> world_set; 
00832     static bool quit_all; 
00833     static void UpdateCb( World* world);
00834     static unsigned int next_id; 
00835      
00836     bool destroy;
00837     bool dirty; 
00838      
00840     std::set<Model*> models;
00841 
00843     std::map<std::string, Model*> models_by_name;       
00844 
00846     std::map<int,Model*> models_by_wfentity;
00847         
00850     std::vector<Model*> models_with_fiducials;
00851         
00852     struct ltx
00853     {
00854       bool operator()(const Model* a, const Model* b) const;
00855     };
00856         
00857     struct lty
00858     {
00859       bool operator()(const Model* a, const Model* b) const;
00860     };
00861         
00864     std::set<Model*,ltx> models_with_fiducials_byx;
00865         
00868     std::set<Model*,lty> models_with_fiducials_byy;
00869                      
00871     void FiducialInsert( Model* mod )
00872     { 
00873       FiducialErase( mod ); // make sure it's not there already
00874       models_with_fiducials.push_back( mod ); 
00875     }
00876      
00878     void FiducialErase( Model* mod )
00879     { 
00880       EraseAll( mod, models_with_fiducials );
00881     }
00882 
00883     double ppm; 
00884     bool quit; 
00885      
00886     bool show_clock; 
00887     unsigned int show_clock_interval; 
00888         
00889     pthread_mutex_t sync_mutex; 
00890     unsigned int threads_working; 
00891     pthread_cond_t threads_start_cond; 
00892     pthread_cond_t threads_done_cond; 
00893     int total_subs; 
00894     unsigned int worker_threads; 
00895     
00896   protected:     
00897 
00898     std::list<std::pair<world_callback_t,void*> > cb_list; 
00899     bounds3d_t extent; 
00900     bool graphics;
00901 
00902     std::set<Option*> option_table; 
00903     std::list<PowerPack*> powerpack_list; 
00904 
00905     usec_t quit_time;
00906     std::list<float*> ray_list;
00907     usec_t sim_time; 
00908     std::map<point_int_t,SuperRegion*> superregions;
00909      
00910     std::vector< std::vector<Model*> > update_lists;  
00911      
00912     uint64_t updates; 
00913     Worldfile* wf; 
00914 
00915     void CallUpdateCallbacks(); 
00916 
00917   public:
00918 
00919     uint64_t UpdateCount(){ return updates; }
00920      
00921     bool paused; 
00922 
00923     virtual void Start(){ paused = false; };
00924     virtual void Stop(){ paused = true; };
00925     virtual void TogglePause(){ paused ? Start() : Stop(); };
00926 
00927     bool Paused() const { return( paused ); };
00928         
00932     virtual void Redraw( void ){ }; // does nothing
00933 
00934     std::vector<point_int_t> rt_cells;
00935     std::vector<point_int_t> rt_candidate_cells;
00936 
00937     static const int DEFAULT_PPM = 50;  // default resolution in pixels per meter
00938 
00941     void AddUpdateCallback( world_callback_t cb, void* user );
00942 
00945     int RemoveUpdateCallback( world_callback_t cb, void* user );
00946 
00948     void Log( Model* mod );
00949 
00951     void NeedRedraw(){ dirty = true; };
00952     
00954     Model* ground;
00955     
00958     virtual std::string ClockString( void ) const;
00959         
00960     Model* CreateModel( Model* parent, const std::string& typestr );     
00961     void LoadModel( Worldfile* wf, int entity );
00962     void LoadBlock( Worldfile* wf, int entity );
00963     void LoadBlockGroup( Worldfile* wf, int entity );
00964     
00965     void LoadSensor( Worldfile* wf, int entity );
00966 
00967     virtual Model* RecentlySelectedModel() const { return NULL; }
00968         
00970     void MapPoly( const std::vector<point_int_t>& poly,
00971           Block* block,
00972           unsigned int layer );
00973 
00974     SuperRegion* AddSuperRegion( const point_int_t& coord );
00975     SuperRegion* GetSuperRegion( const point_int_t& org );
00976     SuperRegion* GetSuperRegionCreate( const point_int_t& org );
00977     //void ExpireSuperRegion( SuperRegion* sr );
00978         
00981     int32_t MetersToPixels( meters_t x ) const
00982     { return (int32_t)floor(x * ppm); };
00983         
00984     point_int_t MetersToPixels( const point_t& pt ) const
00985     { return point_int_t( MetersToPixels(pt.x), MetersToPixels(pt.y)); };
00986         
00987     // dummy implementations to be overloaded by GUI subclasses
00988     virtual void PushColor( Color col ) 
00989     { /* do nothing */  (void)col; };
00990     virtual void PushColor( double r, double g, double b, double a ) 
00991     { /* do nothing */ (void)r; (void)g; (void)b; (void)a; };
00992      
00993     virtual void PopColor(){ /* do nothing */  };
00994         
00995     SuperRegion* CreateSuperRegion( point_int_t origin );
00996     void DestroySuperRegion( SuperRegion* sr );
00997         
00999     RaytraceResult Raytrace( const Ray& ray );
01000 
01001     RaytraceResult Raytrace( const Pose& pose,           
01002                  const meters_t range,
01003                  const ray_test_func_t func,
01004                  const Model* finder,
01005                  const void* arg,
01006                  const bool ztest );
01007         
01008     void Raytrace( const Pose &pose,             
01009            const meters_t range,
01010            const radians_t fov,
01011            const ray_test_func_t func,
01012            const Model* finder,
01013            const void* arg,
01014            RaytraceResult* samples,
01015            const uint32_t sample_count,
01016            const bool ztest );
01017         
01018         
01020     inline void Extend( point3_t pt );
01021   
01022     virtual void AddModel( Model* mod );
01023     virtual void RemoveModel( Model* mod );
01024 
01025     void AddModelName( Model* mod, const std::string& name );
01026         
01027     void AddPowerPack( PowerPack* pp );
01028     void RemovePowerPack( PowerPack* pp );
01029         
01030     void ClearRays();
01031   
01033     void RecordRay( double x1, double y1, double x2, double y2 );
01034         
01037     bool PastQuitTime();
01038                 
01039     static void* update_thread_entry( std::pair<World*,int>* info );
01040     
01041     class Event
01042     {
01043     public:
01044       
01045       Event( usec_t time, Model* mod, model_callback_t cb, void* arg ) 
01046     : time(time), mod(mod), cb(cb), arg(arg) {}
01047             
01048       usec_t time; 
01049       Model* mod; 
01050       model_callback_t cb;
01051       void* arg;
01052             
01055       bool operator<( const Event& other ) const;
01056     };
01057      
01059     std::vector<std::priority_queue<Event> > event_queues;
01060 
01062     std::vector<std::queue<Model*> > pending_update_callbacks;
01063         
01075     void Enqueue( unsigned int queue_num, usec_t delay, Model* mod, model_callback_t cb, void* arg )
01076     {  event_queues[queue_num].push( Event( sim_time + delay, mod, cb, arg ) ); }
01077         
01079     std::set<Model*> active_energy;
01080     void EnableEnergy( Model* m ) { active_energy.insert( m ); };
01081     void DisableEnergy( Model* m ) { active_energy.erase( m ); };
01082     
01084     std::set<ModelPosition*> active_velocity;
01085     
01087     usec_t sim_interval;
01088         
01089     // debug instrumentation - making sure the number of update callbacks
01090     // in each thread is consistent with the number that have been
01091     // registered globally
01092     int update_cb_count;
01093 
01095     void ConsumeQueue( unsigned int queue_num );
01096 
01099     unsigned int GetEventQueue( Model* mod ) const;
01100 
01101   public:
01103     static bool UpdateAll(); 
01104      
01111   static void Run();
01112      
01113     World( const std::string& name = "MyWorld", 
01114        double ppm = DEFAULT_PPM );
01115         
01116     virtual ~World();
01117     
01119     usec_t SimTimeNow(void) const { return sim_time; }
01120         
01123     Worldfile* GetWorldFile()   { return wf; };
01124         
01128     virtual bool IsGUI() const { return false; }
01129      
01135     virtual void Load( const std::string& worldfile_path );
01136 
01137     virtual void UnLoad();
01138 
01139     virtual void Reload();
01140 
01143     virtual bool Save( const char* filename );
01144 
01148     virtual bool Update(void);
01149      
01153     bool TestQuit() const { return( quit || quit_all );  }
01154         
01156     void Quit(){ quit = true; }
01157 
01159     void QuitAll(){ quit_all = true; }
01160 
01162     void CancelQuit(){ quit = false; }
01163 
01165     void CancelQuitAll(){ quit_all = false; }
01166      
01167     void TryCharge( PowerPack* pp, const Pose& pose );
01168 
01171     double Resolution() const { return ppm; };
01172    
01175     Model* GetModel( const std::string& name ) const;
01176 
01178     const std::set<Model*> GetAllModels() const { return models; };
01179   
01181     const bounds3d_t& GetExtent() const { return extent; };
01182   
01184     uint64_t GetUpdateCount() const { return updates; }
01185 
01187     void RegisterOption( Option* opt ); 
01188      
01190     void ShowClock( bool enable ){ show_clock = enable; };
01191 
01193     Model* GetGround() {return ground;};
01194     
01195   };
01196   
01197   class Block
01198   {
01199     friend class BlockGroup;
01200     friend class Model;
01201     friend class SuperRegion;
01202     friend class World;
01203     friend class Canvas;
01204     friend class Cell;
01205   public:
01206         
01210     Block( Model* mod,  
01211        const std::vector<point_t>& pts,
01212        meters_t zmin,
01213        meters_t zmax,
01214        Color color,
01215        bool inherit_color,
01216        bool wheel );
01217         
01219     Block(  Model* mod,  Worldfile* wf, int entity);
01220         
01221     ~Block();
01222      
01224     void Map( unsigned int layer );      
01225         
01227     void UnMap( unsigned int layer );    
01228         
01230     void DrawSolid(bool topview);
01231 
01233     void DrawFootPrint(); 
01234 
01236     void Translate( double x, double y );    
01237 
01239     double CenterX();
01240 
01242     double CenterY();
01243 
01245     void SetCenterX( double y );
01246 
01248     void SetCenterY( double y );
01249 
01251     void SetCenter( double x, double y);     
01252 
01254     void SetZ( double min, double max );
01255         
01256     void AppendTouchingModels( std::set<Model*>& touchers );
01257      
01259     Model* TestCollision(); 
01260 
01261     void Load( Worldfile* wf, int entity );  
01262     Model* GetModel(){ return mod; };  
01263     const Color& GetColor();        
01264     void Rasterize( uint8_t* data, 
01265             unsigned int width, unsigned int height,        
01266             meters_t cellwidth, meters_t cellheight );
01267         
01268   private:
01269     Model* mod; 
01270     std::vector<point_t> mpts; 
01271     size_t pt_count; 
01272     std::vector<point_t> pts; 
01273     Size size;   
01274     Bounds local_z; 
01275     Color color;
01276     bool inherit_color;
01277     bool wheel;
01278 
01279     void DrawTop();
01280     void DrawSides();
01281      
01283     Bounds global_z;     
01284     bool mapped;
01285         
01287     std::vector< std::list<Block*>::iterator > list_entries;
01288         
01291     std::vector<Cell*> rendered_cells[2];
01292         
01295     point_t BlockPointToModelMeters( const point_t& bpt );
01296     
01298     void InvalidateModelPointCache();
01299         
01300   };
01301 
01302     
01303   class BlockGroup
01304   {
01305     friend class Model;
01306     friend class Block;
01307         
01308   private:
01309     int displaylist;
01310         
01311     void BuildDisplayList( Model* mod );
01312         
01313     std::vector<Block*> blocks;
01314     Size size;
01315     point3_t offset;
01316     meters_t minx, maxx, miny, maxy;
01317         
01318   public:
01319     BlockGroup();
01320     ~BlockGroup();
01321         
01322     uint32_t GetCount(){ return blocks.size(); };
01323     const Size& GetSize(){ return size; };
01324     const point3_t& GetOffset(){ return offset; };
01325         
01328     void CalcSize();
01329      
01330     void AppendBlock( Block* block );
01331     void CallDisplayList( Model* mod );
01332     void Clear() ; 
01334     void AppendTouchingModels( std::set<Model*>& touchers );
01335      
01338     Model* TestCollision();
01339  
01340     void Map( unsigned int layer );
01341     void UnMap( unsigned int layer );
01342         
01344     void DrawSolid( const Geom &geom); 
01345 
01347     void DrawFootPrint( const Geom &geom);
01348 
01349     void LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfile *wf );
01350     void LoadBlock( Model* mod, Worldfile* wf, int entity );
01351      
01352     void Rasterize( uint8_t* data, 
01353             unsigned int width, unsigned int height,
01354             meters_t cellwidth, meters_t cellheight );
01355      
01356     void InvalidateModelPointCache()
01357     {
01358       FOR_EACH( it, blocks )
01359     (*it)->InvalidateModelPointCache();
01360     }
01361 
01362   };
01363 
01364   class Camera 
01365   {
01366   protected:
01367     double _pitch; //left-right (about y)
01368     double _yaw; //up-down (about x)
01369     double _x, _y, _z;
01370     
01371   public:
01372     Camera() : _pitch( 0 ), _yaw( 0 ), _x( 0 ), _y( 0 ), _z( 0 ) { }
01373     virtual ~Camera() { }
01374 
01375     virtual void Draw( void ) const = 0;
01376     virtual void SetProjection( void ) const = 0;
01377 
01378     double yaw( void ) const { return _yaw; }
01379     double pitch( void ) const { return _pitch; }
01380      
01381     double x( void ) const { return _x; }
01382     double y( void ) const { return _y; }
01383     double z( void ) const { return _z; }
01384      
01385     virtual void reset() = 0;
01386     virtual void Load( Worldfile* wf, int sec ) = 0;
01387 
01388     //TODO data should be passed in somehow else. (at least min/max stuff)
01389     //virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const = 0;
01390   };
01391 
01392   class PerspectiveCamera : public Camera
01393   {
01394   private:
01395     double _z_near;
01396     double _z_far;
01397     double _vert_fov;
01398     double _horiz_fov;
01399     double _aspect;
01400 
01401   public:
01402     PerspectiveCamera( void );
01403 
01404     virtual void Draw( void ) const;
01405     virtual void SetProjection( void ) const;
01406     //void SetProjection( double aspect ) const;
01407     void update( void );
01408 
01409     void strafe( double amount );
01410     void forward( double amount );
01411     
01412     void setPose( double x, double y, double z ) { _x = x; _y = y; _z = z; }
01413     void addPose( double x, double y, double z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; }
01414     void move( double x, double y, double z );
01415     void setFov( double horiz_fov, double vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; }
01417     void setAspect( double aspect ) { _aspect = aspect; }
01418     void setYaw( double yaw ) { _yaw = yaw; }
01419     double horizFov( void ) const { return _horiz_fov; }
01420     double vertFov( void ) const { return _vert_fov; }
01421     void addYaw( double yaw ) { _yaw += yaw; }
01422     void setPitch( double pitch ) { _pitch = pitch; }
01423     void addPitch( double pitch ) { _pitch += pitch; if( _pitch < 0 ) _pitch = 0; else if( _pitch > 180 ) _pitch = 180; }
01424     
01425     double realDistance( double z_buf_val ) const {
01426       return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) );
01427     }
01428     void scroll( double dy ) { _z += dy; }
01429     double nearClip( void ) const { return _z_near; }
01430     double farClip( void ) const { return _z_far; }
01431     void setClip( double near, double far ) { _z_far = far; _z_near = near; }
01432     
01433     void reset() { setPitch( 70 ); setYaw( 0 ); }
01434     
01435     void Load( Worldfile* wf, int sec );
01436     void Save( Worldfile* wf, int sec );
01437   };
01438   
01439   class OrthoCamera : public Camera
01440   {
01441   private:
01442     double _scale;
01443     double _pixels_width;
01444     double _pixels_height;
01445     double _y_min;
01446     double _y_max;
01447   
01448   public:
01449     OrthoCamera( void ) : 
01450         _scale( 15 ),
01451         _pixels_width(0),
01452         _pixels_height(0),
01453         _y_min(0),
01454         _y_max(0)
01455      { }
01456      
01457     virtual void Draw() const;
01458 
01459     virtual void SetProjection( double pixels_width, 
01460                                           double pixels_height, 
01461                                           double y_min,
01462                                           double y_max );
01463      
01464     virtual void SetProjection( void ) const;
01465      
01466     void move( double x, double y );
01467 
01468     void setYaw( double yaw ) { _yaw = yaw; }
01469 
01470     void setPitch( double pitch ) { _pitch = pitch; }
01471 
01472     void addYaw( double yaw ) { _yaw += yaw;    }
01473 
01474     void addPitch( double pitch ) {
01475       _pitch += pitch;
01476       if( _pitch > 90 )
01477     _pitch = 90;
01478       else if( _pitch < 0 )
01479     _pitch = 0;
01480     }
01481   
01482     void setScale( double scale ) { _scale = scale; }
01483     void setPose( double x, double y) { _x = x; _y = y; }
01484   
01485     void scale( double scale, double shift_x = 0, double h = 0, double shift_y = 0, double w = 0 ); 
01486     void reset( void ) { _pitch = _yaw = 0; }
01487   
01488     double scale() const { return _scale; }
01489   
01490     void Load( Worldfile* wf, int sec );
01491     void Save( Worldfile* wf, int sec );
01492   };
01493 
01494 
01498   class WorldGui : public World, public Fl_Window 
01499   {
01500     friend class Canvas;
01501     friend class ModelCamera;
01502     friend class Model;
01503     friend class Option;
01504 
01505   private:
01506 
01507     Canvas* canvas;
01508     std::vector<Option*> drawOptions;
01509     FileManager* fileMan; 
01510     std::vector<usec_t> interval_log;
01511      
01514     double speedup; 
01515 
01516     Fl_Menu_Bar* mbar;
01517     OptionsDlg* oDlg;
01518     bool pause_time;
01519 
01522     usec_t real_time_interval;
01523      
01525     usec_t real_time_now; 
01526 
01529     usec_t real_time_recorded;
01530      
01532     uint64_t timing_interval;
01533 
01534     // static callback functions
01535     static void windowCb( Fl_Widget* w, WorldGui* wg ); 
01536     static void fileLoadCb( Fl_Widget* w, WorldGui* wg );
01537     static void fileSaveCb( Fl_Widget* w, WorldGui* wg );
01538     static void fileSaveAsCb( Fl_Widget* w, WorldGui* wg );
01539     static void fileExitCb( Fl_Widget* w, WorldGui* wg );
01540     static void viewOptionsCb( OptionsDlg* oDlg, WorldGui* wg );
01541     static void optionsDlgCb( OptionsDlg* oDlg, WorldGui* wg );
01542     static void helpAboutCb( Fl_Widget* w, WorldGui* wg );
01543     static void pauseCb( Fl_Widget* w, WorldGui* wg );
01544     static void onceCb( Fl_Widget* w, WorldGui* wg );
01545     static void fasterCb( Fl_Widget* w, WorldGui* wg );
01546     static void slowerCb( Fl_Widget* w, WorldGui* wg );
01547     static void realtimeCb( Fl_Widget* w, WorldGui* wg );
01548     static void fasttimeCb( Fl_Widget* w, WorldGui* wg );
01549     static void resetViewCb( Fl_Widget* w, WorldGui* wg );
01550     static void moreHelptCb( Fl_Widget* w, WorldGui* wg );
01551     
01552     // GUI functions
01553     bool saveAsDialog();
01554     bool closeWindowQuery();
01555     
01556     virtual void AddModel( Model* mod );
01557     
01558     void SetTimeouts();
01559 
01560   protected:
01561     
01562     virtual void PushColor( Color col );
01563     virtual void PushColor( double r, double g, double b, double a );
01564     virtual void PopColor();
01565     
01566     void DrawOccupancy() const;
01567     void DrawVoxels() const;
01568      
01569   public:
01570     
01571     WorldGui(int W,int H,const char*L=0);
01572     ~WorldGui();
01573 
01575     virtual void Redraw( void );
01576 
01577     virtual std::string ClockString() const;
01578     virtual bool Update();  
01579     virtual void Load( const std::string& filename );
01580     virtual void UnLoad();
01581     virtual bool Save( const char* filename );  
01582     virtual bool IsGUI() const { return true; };    
01583     virtual Model* RecentlySelectedModel() const;
01584 
01585     virtual void Start();
01586     virtual void Stop();
01587     
01588     usec_t RealTimeNow(void) const;
01589  
01590     void DrawBoundingBoxTree();
01591     
01592     Canvas* GetCanvas( void ) const { return canvas; } 
01593 
01595     void Show(); 
01596 
01598     std::string EnergyString( void ) const; 
01599     virtual void RemoveChild( Model* mod );  
01600 
01601     bool IsTopView();
01602   };
01603 
01604 
01605   class StripPlotVis : public Visualizer
01606   {
01607   private:
01608      
01609     Model* mod;
01610     float* data;
01611     size_t len;
01612     size_t count;
01613     unsigned int index;
01614     float x,y,w,h,min,max;
01615     Color fgcolor, bgcolor;
01616      
01617   public:
01618     StripPlotVis( float x, float y, float w, float h, 
01619           size_t len, 
01620           Color fgcolor, Color bgcolor,
01621           const char* name, const char* wfname );
01622     virtual ~StripPlotVis();
01623     virtual void Visualize( Model* mod, Camera* cam );      
01624     void AppendValue( float value );
01625   };
01626 
01627 
01628   class PowerPack
01629   {
01630     friend class WorldGui;
01631     friend class Canvas;
01632      
01633   protected:
01634      
01635     class DissipationVis : public Visualizer
01636     {
01637     private:
01638       unsigned int columns, rows;
01639       meters_t width, height;
01640          
01641       std::vector<joules_t> cells;
01642          
01643       joules_t peak_value;
01644       double cellsize;
01645          
01646       static joules_t global_peak_value; 
01647 
01648     public:
01649       DissipationVis( meters_t width, 
01650               meters_t height, 
01651               meters_t cellsize );
01652 
01653       virtual ~DissipationVis();
01654       virtual void Visualize( Model* mod, Camera* cam );        
01655         
01656       void Accumulate( meters_t x, meters_t y, joules_t amount );
01657     } event_vis;
01658      
01659 
01660     StripPlotVis output_vis;
01661     StripPlotVis stored_vis;
01662 
01664     Model* mod;
01665     
01667     joules_t stored;
01668      
01670     joules_t capacity;
01671      
01673     bool charging;
01674      
01676     joules_t dissipated;
01677      
01678     // these are used to visualize the power draw
01679     usec_t last_time;
01680     joules_t last_joules;
01681     watts_t last_watts;
01682 
01683   public:
01684     static joules_t global_stored;
01685     static joules_t global_capacity;
01686     static joules_t global_dissipated;   
01687     static joules_t global_input;
01688 
01689   public:
01690     PowerPack( Model* mod );
01691     ~PowerPack();
01692      
01694     void Visualize( Camera* cam );
01695 
01697     joules_t RemainingCapacity() const;
01698      
01700     void Add( joules_t j );
01701         
01703     void Subtract( joules_t j );
01704         
01706     void TransferTo( PowerPack* dest, joules_t amount );     
01707 
01708     double ProportionRemaining() const
01709     { return( stored / capacity ); }
01710 
01713     void Print( const char* prefix ) const
01714     { 
01715       if( prefix )
01716     printf( "%s", prefix );
01717 
01718       printf( "PowerPack %.2f/%.2f J\n", stored, capacity ); 
01719     }       
01720      
01721     joules_t GetStored() const;
01722     joules_t GetCapacity() const;
01723     joules_t GetDissipated() const;
01724     void SetCapacity( joules_t j );
01725     void SetStored( joules_t j );   
01726 
01728     bool GetCharging() const { return charging; }
01729      
01730     void ChargeStart(){ charging = true; }
01731     void ChargeStop(){ charging = false; }
01732 
01734     void Dissipate( joules_t j );
01735      
01737     void Dissipate( joules_t j, const Pose& p );
01738   };
01739 
01740    
01742   class Model : public Ancestor
01743   {
01744     friend class Ancestor;
01745     friend class World;
01746     friend class World::Event;
01747     friend class WorldGui;
01748     friend class Canvas;
01749     friend class Block;
01750     friend class Region;
01751     friend class BlockGroup;
01752     friend class PowerPack;
01753     friend class Ray;
01754     friend class ModelFiducial;
01755         
01756   private:
01758     static uint32_t count;
01759     static std::map<id_t,Model*> modelsbyid;
01760 
01762     bool mapped;
01763 
01764     std::vector<Option*> drawOptions;
01765     const std::vector<Option*>& getOptions() const { return drawOptions; }
01766      
01767   protected:
01768 
01771     bool alwayson;
01772 
01773     BlockGroup blockgroup;
01775     int blocks_dl;
01776 
01780     int boundary;
01781         
01784   public:
01785     class cb_t
01786     {
01787     public:
01788       model_callback_t callback;
01789       void* arg;
01790             
01791       cb_t( model_callback_t cb, void* arg ) 
01792     : callback(cb), arg(arg) {}
01793             
01794       cb_t( world_callback_t cb, void* arg ) 
01795     : callback(NULL), arg(arg) { (void)cb; }
01796             
01797       cb_t() : callback(NULL), arg(NULL) {}
01798             
01800       bool operator<( const cb_t& other ) const
01801       {
01802     if( callback == other.callback )
01803       return( arg < other.arg );
01804     //else
01805     return ((void*)(callback)) < ((void*)(other.callback)); 
01806       }
01807          
01809       bool operator==( const cb_t& other ) const
01810       { return( callback == other.callback);  }         
01811     };
01812         
01813     class Flag
01814     {
01815     private:
01816       Color color;
01817       double size;
01818       int displaylist;
01819 
01820     public:
01821       void SetColor( const Color& col );
01822       void SetSize( double sz );
01823          
01824       Color GetColor(){ return color; }
01825       double GetSize(){ return size; }
01826          
01827       Flag( const Color& color, double size );
01828       Flag* Nibble( double portion );
01829         
01832       void Draw(  GLUquadric* quadric );
01833     };
01834 
01835     typedef enum {
01836       CB_FLAGDECR,
01837       CB_FLAGINCR,
01838       CB_GEOM,
01839       CB_INIT,
01840       CB_LOAD,
01841       CB_PARENT,
01842       CB_POSE,
01843       CB_SAVE,
01844       CB_SHUTDOWN,
01845       CB_STARTUP,
01846       CB_UPDATE,
01847       CB_VELOCITY,
01848       //CB_POSTUPDATE,
01849       __CB_TYPE_COUNT // must be the last entry: counts the number of types
01850     } callback_type_t;
01851         
01852   protected:
01856     std::vector<std::set<cb_t> > callbacks;
01857         
01859     Color color;
01860         
01864     bool data_fresh;
01865 
01869     bool disabled; 
01870 
01872     std::list<Visualizer*> cv_list;
01873 
01875     std::list<Flag*> flag_list;
01876         
01879     double friction;
01880         
01883     Geom geom;
01884 
01886     class GuiState
01887     {
01888     public:
01889       bool grid;
01890       bool move;
01891       bool nose;
01892       bool outline;
01893         
01894       GuiState();
01895       GuiState& Load( Worldfile* wf, int wf_entity );
01896     } gui;
01897      
01898     bool has_default_block;
01899                   
01901     uint32_t id;    
01902     usec_t interval; 
01903     usec_t interval_energy; 
01904     //    usec_t interval_pose; ///< time between updates of pose due to velocity in usec
01905 
01906     usec_t last_update; 
01907     bool log_state; 
01908     meters_t map_resolution;
01909     kg_t mass;
01910 
01912     Model* parent; 
01913 
01916     Pose pose;
01917 
01919     PowerPack* power_pack;
01920 
01923     std::list<PowerPack*> pps_charging;
01924         
01926     class RasterVis : public Visualizer
01927     {
01928     private:
01929       uint8_t* data;
01930       unsigned int width, height;
01931       meters_t cellwidth, cellheight;
01932       std::vector<point_t> pts;
01933       
01934     public:
01935       RasterVis();
01936       virtual ~RasterVis( void ){}
01937       virtual void Visualize( Model* mod, Camera* cam );
01938       
01939       void SetData( uint8_t* data, 
01940             unsigned int width, 
01941             unsigned int height,
01942             meters_t cellwidth,
01943             meters_t cellheight );
01944       
01945       int subs;     //< the number of subscriptions to this model
01946       int used;     //< the number of connections to this model
01947       
01948       void AddPoint( meters_t x, meters_t y );
01949       void ClearPts();
01950       
01951     } rastervis;
01952      
01953     bool rebuild_displaylist; 
01954     std::string say_string;   
01955         
01956     bool stack_children; 
01957 
01958     bool stall; 
01959     int subs;    
01960 
01964     bool thread_safe;
01965      
01967     class TrailItem 
01968     {                                                                                           
01969     public:
01970       usec_t time;
01971       Pose pose;
01972       Color color;
01973         
01974       TrailItem() 
01975     : time(0), pose(), color(){}
01976          
01977       //TrailItem( usec_t time, Pose pose, Color color ) 
01978       //: time(time), pose(pose), color(color){}
01979     };
01980     
01982     std::vector<TrailItem> trail;
01983 
01985     unsigned int trail_index;
01986 
01990     static unsigned int trail_length;
01991      
01993     static uint64_t trail_interval;
01994      
01996     void UpdateTrail();
01997 
01998     //model_type_t type;  
01999     const std::string type;
02002     unsigned int event_queue_num; 
02003     bool used;   
02004     
02005     watts_t watts;
02006      
02009     watts_t watts_give;
02010     
02013     watts_t watts_take;
02014     
02015     Worldfile* wf;
02016     int wf_entity;
02017     World* world; // pointer to the world in which this model exists
02018     WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode
02019 
02020   public:
02021     
02022     virtual void SetToken( const std::string& str )
02023     { 
02024       //printf( "Model::SetToken( %s )\n", str.c_str() );
02025 
02026       if( str.size() > 0 ) 
02027     {
02028       world->AddModelName( this, str );
02029       Ancestor::SetToken( str );
02030     }
02031       else
02032     PRINT_ERR( "Model::SetToken() called with zero length string. Ignored." );
02033     } 
02034 
02035      
02036     const std::string& GetModelType() const {return type;}   
02037     std::string GetSayString(){return std::string(say_string);}
02038      
02041     Model* GetChild( const std::string& name ) const;
02042 
02044     usec_t GetInterval(){ return interval; }
02045         
02046     class Visibility
02047     {
02048     public:
02049       bool blob_return;
02050       int fiducial_key;
02051       int fiducial_return;
02052       bool gripper_return;
02053       bool obstacle_return;
02054       double ranger_return; // 0 - 1
02055         
02056       Visibility();
02057 
02058       Visibility& Load( Worldfile* wf, int wf_entity );
02059       void Save( Worldfile* wf, int wf_entity );
02060     } vis;
02061      
02062     usec_t GetUpdateInterval() const { return interval; }
02063     usec_t GetEnergyInterval() const { return interval_energy; }
02064     //    usec_t GetPoseInterval() const { return interval_pose; }
02065      
02068     void Rasterize( uint8_t* data, 
02069             unsigned int width, unsigned int height,
02070             meters_t cellwidth, meters_t cellheight );
02071     
02072   private: 
02075     explicit Model(const Model& original);
02076 
02079     Model& operator=(const Model& original);
02080 
02081   protected:
02082 
02084     void RegisterOption( Option* opt );
02085 
02086     void AppendTouchingModels( std::set<Model*>& touchers );
02087         
02092     Model* TestCollision();
02093   
02094     void CommitTestedPose();
02095 
02096     void Map( unsigned int layer );
02097 
02099     inline void Map(){ Map(0); Map(1); }
02100 
02101     void UnMap( unsigned int layer );
02102 
02104     inline void UnMap(){ UnMap(0); UnMap(1); }
02105 
02106     void MapWithChildren( unsigned int layer );
02107     void UnMapWithChildren( unsigned int layer );
02108   
02109     // Find the root model, and map/unmap the whole tree.
02110     void MapFromRoot( unsigned int layer );
02111     void UnMapFromRoot( unsigned int layer );
02112 
02115     RaytraceResult Raytrace( const Pose &pose,
02116                  const meters_t range, 
02117                  const ray_test_func_t func,
02118                  const void* arg,
02119                  const bool ztest = true );
02120   
02123     void Raytrace( const Pose &pose,
02124            const meters_t range, 
02125            const radians_t fov, 
02126            const ray_test_func_t func,
02127            const void* arg,
02128            RaytraceResult* samples,
02129            const uint32_t sample_count,
02130            const bool ztest = true  );
02131   
02132     RaytraceResult Raytrace( const radians_t bearing,            
02133                  const meters_t range,
02134                  const ray_test_func_t func,
02135                  const void* arg,
02136                  const bool ztest = true );
02137   
02138     void Raytrace( const radians_t bearing,              
02139            const meters_t range,
02140            const radians_t fov,
02141            const ray_test_func_t func,
02142            const void* arg,
02143            RaytraceResult* samples,
02144            const uint32_t sample_count,
02145            const bool ztest = true );
02146   
02147     virtual void Startup();
02148     virtual void Shutdown();
02149     virtual void Update();
02150     virtual void UpdateCharge();
02151         
02152     static int UpdateWrapper( Model* mod, void* arg ){ mod->Update(); return 0; }
02153 
02155     void CallUpdateCallbacks( void );
02156 
02157     meters_t ModelHeight() const;
02158 
02159     void DrawBlocksTree();
02160     virtual void DrawBlocks();
02161     void DrawBoundingBox();
02162     void DrawBoundingBoxTree();
02163     virtual void DrawStatus( Camera* cam );
02164     void DrawStatusTree( Camera* cam );
02165   
02166     void DrawOriginTree();
02167     void DrawOrigin();
02168   
02169     void PushLocalCoords();
02170     void PopCoords();
02171   
02173     void DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width=1.0, double height=1.0 );
02174     
02175     virtual void DrawPicker();
02176     virtual void DataVisualize( Camera* cam );  
02177     virtual void DrawSelected(void);
02178     
02179     void DrawTrailFootprint();
02180     void DrawTrailBlocks();
02181     void DrawTrailArrows();
02182     void DrawGrid();
02183     //  void DrawBlinkenlights();
02184     void DataVisualizeTree( Camera* cam );
02185     void DrawFlagList();
02186     void DrawPose( Pose pose );
02187     
02188   public:
02189     virtual void PushColor( Color col ){ world->PushColor( col ); } 
02190     virtual void PushColor( double r, double g, double b, double a ){ world->PushColor( r,g,b,a ); }    
02191     virtual void PopColor() { world->PopColor(); }
02192     
02193     PowerPack* FindPowerPack() const;
02194     
02195     //void RecordRenderPoint( GSList** head, GSList* link, 
02196     //                  unsigned int* c1, unsigned int* c2 );
02197 
02198     void PlaceInFreeSpace( meters_t xmin, meters_t xmax, 
02199                meters_t ymin, meters_t ymax );
02200     
02202     std::string PoseString()
02203     { return pose.String(); }
02204     
02206     static Model* LookupId( uint32_t id )
02207     { return modelsbyid[id]; }
02208      
02210     Model( World* world, 
02211        Model* parent = NULL, 
02212        const std::string& type = "model",
02213        const std::string& name = "" );
02214      
02216     virtual ~Model();
02217         
02219      Model() 
02220         : mapped(false), alwayson(false), blocks_dl(0),
02221           boundary(false), data_fresh(false), disabled(true), friction(0), has_default_block(false), log_state(false), map_resolution(0), mass(0), parent(NULL), rebuild_displaylist(false), stack_children(true), stall(false), subs(0), thread_safe(false),trail_index(0), event_queue_num(0), used(false), watts(0), watts_give(0),watts_take(0),wf(NULL), wf_entity(0), world(NULL)
02222      {}
02223         
02224     void Say( const std::string& str );
02225      
02227     void AddVisualizer( Visualizer* custom_visual, bool on_by_default );
02228 
02230     void RemoveVisualizer( Visualizer* custom_visual );
02231 
02232     void BecomeParentOf( Model* child );
02233 
02234     void Load( Worldfile* wf, int wf_entity )
02235     {
02238       SetWorldfile( wf, wf_entity );
02239       Load(); // call virtual load
02240     }
02241     
02243     void SetWorldfile( Worldfile* wf, int wf_entity )
02244     { this->wf = wf; this->wf_entity = wf_entity; }
02245     
02247     virtual void Load();
02248     
02250     virtual void Save();
02251     
02253     void InitControllers();
02254 
02255     void AddFlag(  Flag* flag );
02256     void RemoveFlag( Flag* flag );
02257     
02258     void PushFlag( Flag* flag );
02259     Flag* PopFlag();
02260     
02261     unsigned int GetFlagCount() const { return flag_list.size(); }
02262     
02267     void Disable(){ disabled = true; };
02268 
02271     void Enable(){ disabled = false; };
02272   
02275     void LoadControllerModule( const char* lib );
02276     
02279     void NeedRedraw();
02280         
02282     void Redraw();
02283 
02286     void LoadBlock( Worldfile* wf, int entity );
02287 
02290     Block*  AddBlockRect( meters_t x, meters_t y, 
02291                   meters_t dx, meters_t dy, 
02292                   meters_t dz );
02293         
02295     void ClearBlocks();
02296   
02299     Model* Parent() const { return this->parent; }
02300 
02302     World* GetWorld() const { return this->world; }
02303   
02305     Model* Root(){ return(  parent ? parent->Root() : this ); }
02306   
02307     bool IsAntecedent( const Model* testmod ) const;
02308     
02310     bool IsDescendent( const Model* testmod ) const;
02311     
02313     bool IsRelated( const Model* testmod ) const;
02314 
02316     Pose GetGlobalPose() const;
02317     
02319     Velocity GetGlobalVelocity()  const;
02320     
02321     /* set the velocity of a model in the global coordinate system */
02322     void SetGlobalVelocity( const Velocity& gvel );
02323     
02325     void Subscribe();
02326     
02328     void Unsubscribe();
02329     
02331     void SetGlobalPose(  const Pose& gpose );
02332     
02334     //    void VelocityEnable();
02335 
02337     //void VelocityDisable();
02338 
02340     void SetPose(  const Pose& pose );
02341     
02343     void AddToPose(  const Pose& pose );
02344     
02346     void AddToPose(  double dx, double dy, double dz, double da );
02347     
02349     void SetGeom(  const Geom& src );
02350   
02353     void SetFiducialReturn(  int fid );
02354   
02356     int GetFiducialReturn()  const { return vis.fiducial_return; }
02357   
02360     void SetFiducialKey(  int key );
02361     
02362     Color GetColor() const { return color; }
02363      
02365     uint32_t GetId()  const { return id; }
02366      
02368     kg_t GetTotalMass() const;
02369      
02371     kg_t GetMassOfChildren() const;
02372 
02374     int SetParent( Model* newparent);
02375     
02378     Geom GetGeom() const { return geom; }
02379     
02382     Pose GetPose() const { return pose; }
02383     
02384     
02385     // guess what these do?
02386     void SetColor( Color col );
02387     void SetMass( kg_t mass );
02388     void SetStall( bool stall );
02389     void SetGravityReturn( bool val );
02390     void SetGripperReturn( bool val );
02391     void SetStickyReturn( bool val );
02392     void SetRangerReturn( double val );
02393     void SetObstacleReturn( bool val );
02394     void SetBlobReturn( bool val );
02395     void SetRangerReturn( bool val );
02396     void SetBoundary( bool val );
02397     void SetGuiNose( bool val );
02398     void SetGuiMove( bool val );
02399     void SetGuiGrid( bool val );
02400     void SetGuiOutline( bool val );
02401     void SetWatts( watts_t watts );
02402     void SetMapResolution( meters_t res );
02403     void SetFriction( double friction );
02404     
02405     bool DataIsFresh() const { return this->data_fresh; }
02406         
02407     /* attach callback functions to data members. The function gets
02408        called when the member is changed using SetX() accessor method */
02409         
02416     void AddCallback( callback_type_t type, 
02417               model_callback_t cb, 
02418               void* user );
02419         
02420     int RemoveCallback( callback_type_t type,
02421             model_callback_t callback );
02422         
02423     int CallCallbacks(  callback_type_t type );
02424         
02425         
02426     virtual void Print( char* prefix ) const;
02427     virtual const char* PrintWithPose() const;
02428     
02431     Pose GlobalToLocal( const Pose& pose ) const;
02432      
02435     Pose LocalToGlobal( const Pose& pose ) const
02436     {  
02437       return( ( GetGlobalPose() + geom.pose ) + pose );
02438     }
02439     
02441     std::vector<point_int_t>  LocalToPixels( const std::vector<point_t>& local ) const;
02442         
02445     point_t LocalToGlobal( const point_t& pt) const;        
02446 
02449     Model* GetUnsubscribedModelOfType( const std::string& type ) const;
02450     
02453     Model* GetUnusedModelOfType( const std::string& type );
02454   
02457     bool Stalled() const { return this->stall; }
02458      
02461     unsigned int GetSubscriptionCount() const { return subs; }
02462 
02464     bool HasSubscribers() const { return( subs > 0 ); }  
02465 
02466     static std::map< std::string, creator_t> name_map;   
02467 
02468     //      class Neighbors
02469     //      {
02470     //          Model *left, *right, *up, *down;
02471     //      public:
02472     //          Neighbors() : left(NULL), right(NULL), up(NULL), down(NULL) {}
02473     //      } nbors; // instance
02474 
02475                             
02476   };
02477 
02478 
02479   // BLOBFINDER MODEL --------------------------------------------------------
02480 
02481 
02483   class ModelBlobfinder : public Model
02484   {
02485   public:
02487     class Blob
02488     {
02489     public:
02490       Color color;
02491       uint32_t left, top, right, bottom;
02492       meters_t range;
02493     };
02494 
02495     class Vis : public Visualizer 
02496     {
02497     private:
02498       //static Option showArea;
02499     public:
02500       Vis( World* world );
02501       virtual ~Vis( void ){}
02502       virtual void Visualize( Model* mod, Camera* cam );
02503     } vis;
02504 
02505   private:
02506     std::vector<Blob> blobs;
02507     std::vector<Color> colors;
02508 
02509     // predicate for ray tracing
02510     static bool BlockMatcher( Block* testblock, Model* finder );
02511 
02512   public:
02513     radians_t fov;
02514     radians_t pan;
02515     meters_t range;
02516     unsigned int scan_height;
02517     unsigned int scan_width;
02518      
02519     // constructor
02520     ModelBlobfinder( World* world,
02521              Model* parent,
02522              const std::string& type );
02523     // destructor
02524     ~ModelBlobfinder();
02525     
02526     virtual void Startup();
02527     virtual void Shutdown();
02528     virtual void Update();
02529     virtual void Load();
02530         
02531     Blob* GetBlobs( unsigned int* count )
02532     { 
02533       if( count ) *count = blobs.size();
02534       return &blobs[0];
02535     }
02536 
02537     std::vector<Blob> GetBlobs() const { return blobs; }
02538 
02540     void AddColor( Color col );
02541 
02543     void RemoveColor( Color col );
02544 
02547     void RemoveAllColors();
02548   };
02549 
02550 
02551 
02552   
02553 
02554   // Light indicator model
02555   class ModelLightIndicator : public Model
02556   {
02557   public:
02558     ModelLightIndicator( World* world, 
02559              Model* parent,
02560              const std::string& type );
02561     ~ModelLightIndicator();
02562   
02563     void SetState(bool isOn);
02564 
02565   protected:
02566     virtual void DrawBlocks();
02567 
02568   private:
02569     bool m_IsOn;
02570   };
02571 
02572   // \todo  GRIPPER MODEL --------------------------------------------------------
02573 
02574 
02575   class ModelGripper : public Model
02576   {
02577   public:
02578 
02579     enum paddle_state_t {
02580       PADDLE_OPEN = 0, // default state
02581       PADDLE_CLOSED, 
02582       PADDLE_OPENING,
02583       PADDLE_CLOSING,
02584     };
02585      
02586     enum lift_state_t {
02587       LIFT_DOWN = 0, // default state
02588       LIFT_UP, 
02589       LIFT_UPPING, // verbed these to match the paddle state
02590       LIFT_DOWNING, 
02591     };
02592      
02593     enum cmd_t {
02594       CMD_NOOP = 0, // default state
02595       CMD_OPEN, 
02596       CMD_CLOSE,
02597       CMD_UP, 
02598       CMD_DOWN    
02599     };
02600      
02601      
02604     struct config_t
02605     {
02606       Size paddle_size; 
02607       paddle_state_t paddles;
02608       lift_state_t lift;        
02609       double paddle_position; 
02610       double lift_position; 
02611       Model* gripped;
02612       bool paddles_stalled; // true iff some solid object stopped the paddles closing or opening
02613       double close_limit; 
02614       bool autosnatch; 
02615       double break_beam_inset[2]; 
02616       Model* beam[2]; 
02617       Model* contact[2]; 
02618     };
02619      
02620   private:
02621     virtual void Update();
02622     virtual void DataVisualize( Camera* cam );
02623      
02624     void FixBlocks();
02625     void PositionPaddles();
02626     void UpdateBreakBeams();
02627     void UpdateContacts();
02628 
02629     config_t cfg;
02630     cmd_t cmd;
02631      
02632     Block* paddle_left;
02633     Block* paddle_right;
02634 
02635     static Option showData;
02636 
02637   public:    
02638     static const Size size;
02639 
02640     // constructor
02641     ModelGripper( World* world,
02642           Model* parent,
02643           const std::string& type );
02644     // destructor
02645     virtual ~ModelGripper();
02646   
02647     virtual void Load();
02648     virtual void Save();
02649 
02651     void SetConfig( config_t & newcfg ){ this->cfg = newcfg; FixBlocks(); }
02652      
02654     config_t GetConfig(){ return cfg; };
02655      
02657     void SetCommand( cmd_t cmd ) { this->cmd = cmd; }
02659     void CommandClose() { SetCommand( CMD_CLOSE ); }
02661     void CommandOpen() { SetCommand( CMD_OPEN ); }
02663     void CommandUp() { SetCommand( CMD_UP ); }
02665     void CommandDown() { SetCommand( CMD_DOWN ); }
02666   };
02667 
02668 
02669   // \todo BUMPER MODEL --------------------------------------------------------
02670 
02671   //   typedef struct
02672   //   {
02673   //     Pose pose;
02674   //     meters_t length;
02675   //   } bumper_config_t;
02676 
02677   //   typedef struct
02678   //   {
02679   //     Model* hit;
02680   //     point_t hit_point;
02681   //   } bumper_sample_t;
02682 
02683 
02684   // FIDUCIAL MODEL --------------------------------------------------------
02685 
02687   class ModelFiducial : public Model
02688   {
02689   public:  
02691     class Fiducial
02692     {
02693     public:
02694       meters_t range; 
02695       radians_t bearing; 
02696       Pose geom; 
02697          //Pose pose_rel; /// relative pose of the target in local coordinates       
02698       Pose pose; 
02699       Model* mod; 
02700       int id; 
02701     };
02702 
02703   private:
02704     // if neighbor is visible, add him to the fiducial scan
02705     void AddModelIfVisible( Model* him );
02706 
02707     virtual void Update();
02708     virtual void DataVisualize( Camera* cam );
02709 
02710     static Option showData;
02711     static Option showFov;
02712      
02713     std::vector<Fiducial> fiducials;
02714         
02715   public:       
02716     ModelFiducial( World* world, 
02717            Model* parent,
02718            const std::string& type );
02719     virtual ~ModelFiducial();
02720     
02721     virtual void Load();
02722     void Shutdown( void );
02723 
02724     meters_t max_range_anon;
02725     meters_t max_range_id; 
02726     meters_t min_range; 
02727     radians_t fov; 
02728     radians_t heading; 
02729     int key; 
02730     bool ignore_zloc;  
02731         
02733     std::vector<Fiducial>& GetFiducials() { return fiducials; }
02734         
02736     Fiducial* GetFiducials( unsigned int* count )
02737     {
02738       if( count ) *count = fiducials.size();
02739       return &fiducials[0];
02740     }
02741   };
02742     
02743     
02744   // RANGER MODEL --------------------------------------------------------
02745     
02747   class ModelRanger : public Model
02748   {
02749   public:
02750   public:
02751     ModelRanger( World* world, Model* parent,
02752          const std::string& type );
02753     virtual ~ModelRanger();
02754         
02755     virtual void Load();
02756     virtual void Print( char* prefix ) const;
02757         
02758     class Vis : public Visualizer 
02759     {
02760     public:
02761       static Option showArea;
02762       static Option showStrikes;
02763       static Option showFov;
02764       static Option showBeams;
02765       static Option showTransducers;        
02766             
02767       Vis( World* world );      
02768       virtual ~Vis( void ){} 
02769       virtual void Visualize( Model* mod, Camera* cam );
02770     } vis;
02771         
02772     class Sensor
02773     {       
02774     public:
02775       Pose pose; 
02776       Size size;
02777       Bounds range;
02778       radians_t fov;
02779       unsigned int sample_count;
02780       Color col;
02781             
02782       std::vector<meters_t> ranges;
02783       std::vector<double> intensities;
02784       std::vector<double> bearings;
02785             
02786       Sensor() : pose( 0,0,0,0 ), 
02787          size( 0.02, 0.02, 0.02 ), // teeny transducer
02788          range( 0.0, 5.0 ),
02789          fov( 0.1 ), 
02790          sample_count(1),
02791          col( 0,1,0,0.3 ),
02792          ranges(),
02793          intensities(),
02794          bearings()
02795       {}
02796             
02797       void Update( ModelRanger* rgr );          
02798       void Visualize( Vis* vis, ModelRanger* rgr ) const;
02799       std::string String() const;           
02800       void Load( Worldfile* wf, int entity );
02801     };
02802 
02804     const std::vector<Sensor>& GetSensors() const
02805     { return sensors; }
02806 
02808     std::vector<Sensor>& GetSensorsMutable() 
02809     { return sensors; }
02810      
02811     void LoadSensor( Worldfile* wf, int entity );
02812         
02813   private:
02814     std::vector<Sensor> sensors;        
02815     
02816   protected:
02817         
02818     virtual void Startup();
02819     virtual void Shutdown();
02820     virtual void Update();      
02821   };
02822     
02823   // BLINKENLIGHT MODEL ----------------------------------------------------
02824   class ModelBlinkenlight : public Model
02825   {
02826   private:
02827     double dutycycle;
02828     bool enabled;
02829     msec_t period;
02830     bool on;
02831 
02832     static Option showBlinkenData;
02833   public:
02834     ModelBlinkenlight( World* world,
02835                Model* parent,
02836                const std::string& type );
02837 
02838     ~ModelBlinkenlight();
02839     
02840     virtual void Load();
02841     virtual void Update();
02842     virtual void DataVisualize( Camera* cam );
02843   };
02844 
02845     
02846   // CAMERA MODEL ----------------------------------------------------
02847 
02849   class ModelCamera : public Model
02850   {
02851   public:
02852     typedef struct 
02853     {
02854       // GL_V3F
02855       GLfloat x, y, z;
02856     } ColoredVertex;
02857   
02858   private:
02859     Canvas* _canvas;
02860 
02861     GLfloat* _frame_data;  //opengl read buffer
02862     GLubyte* _frame_color_data;  //opengl read buffer
02863 
02864     bool _valid_vertexbuf_cache;
02865     ColoredVertex* _vertexbuf_cache; //cached unit vectors with appropriate rotations (these must be scalled by z-buffer length)
02866     
02867     int _width;         //width of buffer
02868     int _height;        //height of buffer
02869     static const int _depth = 4;
02870     
02871     int _camera_quads_size;
02872     GLfloat* _camera_quads;
02873     GLubyte* _camera_colors;
02874     
02875     static Option showCameraData;
02876     
02877     PerspectiveCamera _camera;
02878     double _yaw_offset; //position camera is mounted at
02879     double _pitch_offset;
02880         
02882     bool GetFrame();
02883     
02884   public:
02885     ModelCamera( World* world,
02886          Model* parent,
02887          const std::string& type ); 
02888       
02889     ~ModelCamera();
02890   
02891     virtual void Load();
02892     
02894     virtual void Update();
02895     
02897     //virtual void Draw( uint32_t flags, Canvas* canvas );
02898     
02900     virtual void DataVisualize( Camera* cam );
02901     
02903     int getWidth( void ) const { return _width; }
02904     
02906     int getHeight( void ) const { return _height; }
02907     
02909     const PerspectiveCamera& getCamera( void ) const { return _camera; }
02910     
02912     const GLfloat* FrameDepth() const { return _frame_data; }
02913     
02915     const GLubyte* FrameColor() const { return _frame_color_data; }
02916     
02918     void setPitch( double pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; }
02919     
02921     void setYaw( double yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; }
02922   };
02923 
02924   // POSITION MODEL --------------------------------------------------------
02925 
02927   class ModelPosition : public Model
02928   {
02929     friend class Canvas;
02930 
02931   public:
02933     typedef enum
02934       { CONTROL_ACCELERATION,
02935     CONTROL_VELOCITY, 
02936     CONTROL_POSITION 
02937       } ControlMode;
02938      
02940     typedef enum
02941       { LOCALIZATION_GPS, 
02942     LOCALIZATION_ODOM 
02943       } LocalizationMode;
02944      
02946     typedef enum
02947       { DRIVE_DIFFERENTIAL, 
02948     DRIVE_OMNI, 
02949     DRIVE_CAR 
02950       } DriveMode;
02951      
02952   private:
02953     Velocity velocity;
02954     Pose goal;
02955     ControlMode control_mode;
02956     DriveMode drive_mode;    
02957     LocalizationMode localization_mode; 
02958     Velocity integration_error; 
02959     double wheelbase;
02960     
02961   public:
02963     Bounds acceleration_bounds[4];
02964     
02966     Bounds velocity_bounds[4];
02967 
02968   public:
02969     // constructor
02970     ModelPosition( World* world,
02971            Model* parent,
02972            const std::string& type );
02973     // destructor
02974     ~ModelPosition();
02975 
02976     virtual void Move();
02977     virtual void Startup();
02978     virtual void Shutdown();
02979     virtual void Update();
02980     virtual void Load();
02981 
02984     Velocity GetVelocity() const { return velocity; }
02985     void SetVelocity( const Velocity& val );
02986 
02989     class Waypoint
02990     {
02991     public:
02992       Waypoint( meters_t x, meters_t y, meters_t z, radians_t a, Color color ) ;
02993       Waypoint( const Pose& pose, Color color ) ;
02994       Waypoint();
02995       void Draw() const;
02996         
02997       Pose pose;
02998       Color color;
02999     };
03000      
03001     std::vector<Waypoint> waypoints;
03002 
03003     class WaypointVis : public Visualizer
03004     {
03005     public:
03006       WaypointVis();
03007       virtual ~WaypointVis( void ){}
03008       virtual void Visualize( Model* mod, Camera* cam );
03009     } wpvis;
03010      
03011     class PoseVis : public Visualizer
03012     {
03013     public:
03014       PoseVis();
03015       virtual ~PoseVis( void ){}
03016       virtual void Visualize( Model* mod, Camera* cam );
03017     } posevis;
03018 
03020     void SetOdom( Pose odom );
03021         
03024     void SetSpeed( double x, double y, double a );
03025     void SetXSpeed( double x );
03026     void SetYSpeed( double y );
03027     void SetZSpeed( double z );
03028     void SetTurnSpeed( double a );
03029     void SetSpeed( Velocity vel );
03031     void Stop();
03032 
03035     void GoTo( double x, double y, double a );
03036     void GoTo( Pose pose );
03037     
03041     void SetAcceleration( double x, double y, double a );
03042 
03043     // localization state
03044     Pose est_pose; 
03045     Pose est_pose_error; 
03046     Pose est_origin; 
03047   };
03048 
03049 
03050   // ACTUATOR MODEL --------------------------------------------------------
03051 
03053   class ModelActuator : public Model
03054   {
03055   public:
03057     typedef enum
03058       { CONTROL_VELOCITY,
03059     CONTROL_POSITION
03060       } ControlMode;
03061   
03063     typedef enum
03064       { TYPE_LINEAR,
03065     TYPE_ROTATIONAL
03066       } ActuatorType;
03067   
03068   private:
03069     double goal; //< the current velocity or pose to reach depending on the value of control_mode
03070     double pos;
03071     double max_speed;
03072     double min_position;
03073     double max_position;
03074     double start_position;
03075     double cosa;
03076     double sina;
03077     ControlMode control_mode;
03078     ActuatorType actuator_type;
03079     point3_t axis;
03080   
03081     Pose InitialPose;
03082   public:  
03083     // constructor
03084     ModelActuator( World* world,
03085            Model* parent,
03086            const std::string& type );
03087     // destructor
03088     ~ModelActuator();
03089   
03090     virtual void Startup();
03091     virtual void Shutdown();
03092     virtual void Update();
03093     virtual void Load();
03094   
03097     void SetSpeed( double speed );
03098   
03099     double GetSpeed() const {return goal;}
03100   
03103     void GoTo( double pose );
03104   
03105     double GetPosition() const {return pos;};
03106     double GetMaxPosition() const {return max_position;};
03107     double GetMinPosition() const {return min_position;};
03108         
03109     ActuatorType GetType() const { return actuator_type; }
03110     point3_t GetAxis() const { return axis; }
03111   };
03112 
03113 
03114 }; // end namespace stg
03115 
03116 #endif