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