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