stage.hh
Go to the documentation of this file.
00001 #ifndef STG_H 00002 #define STG_H 00003 /* 00004 * Stage : a multi-robot simulator. Part of the Player Project. 00005 * 00006 * Copyright (C) 2001-2009 Richard Vaughan, Brian Gerkey, Andrew 00007 * Howard, Toby Collett, Reed Hedges, Alex Couture-Beil, Jeremy 00008 * Asher, Pooya Karimian 00009 * 00010 * This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU General Public License for more details. 00019 * 00020 * You should have received a copy of the GNU General Public License 00021 * along with this program; if not, write to the Free Software 00022 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00023 * 00024 */ 00025 00033 // C libs 00034 #include <unistd.h> 00035 #include <stdint.h> // for portable int types eg. uint32_t 00036 #include <assert.h> 00037 #include <stdlib.h> 00038 #include <stdio.h> 00039 #include <libgen.h> 00040 #include <string.h> 00041 #include <sys/types.h> 00042 #include <sys/time.h> 00043 #include <pthread.h> 00044 00045 // C++ libs 00046 #include <cmath> 00047 #include <iostream> 00048 #include <vector> 00049 #include <list> 00050 #include <map> 00051 #include <set> 00052 #include <queue> 00053 #include <algorithm> 00054 00055 // FLTK Gui includes 00056 #include <FL/Fl.H> 00057 #include <FL/Fl_Box.H> 00058 #include <FL/Fl_Gl_Window.H> 00059 #include <FL/Fl_Menu_Bar.H> 00060 #include <FL/Fl_Window.H> 00061 #include <FL/fl_draw.H> 00062 #include <FL/gl.h> // FLTK takes care of platform-specific GL stuff 00063 // except GLU 00064 #ifdef __APPLE__ 00065 #include <OpenGL/glu.h> 00066 #else 00067 #include <GL/glu.h> 00068 #endif 00069 00071 namespace Stg 00072 { 00073 // forward declare 00074 class Block; 00075 class Canvas; 00076 class Cell; 00077 class Worldfile; 00078 class World; 00079 class WorldGui; 00080 class Model; 00081 class OptionsDlg; 00082 class Camera; 00083 class FileManager; 00084 class Option; 00085 00086 typedef Model* (*creator_t)( World*, Model*, const std::string& type ); 00087 00089 typedef std::set<Model*> ModelPtrSet; 00090 00092 typedef std::vector<Model*> ModelPtrVec; 00093 00095 typedef std::set<Block*> BlockPtrSet; 00096 00098 typedef std::vector<Cell*> CellPtrVec; 00099 00101 void Init( int* argc, char** argv[] ); 00102 00104 bool InitDone(); 00105 00108 const char* Version(); 00109 00111 const char COPYRIGHT[] = 00112 "Copyright Richard Vaughan and contributors 2000-2009"; 00113 00115 const char AUTHORS[] = 00116 "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, Toby Collett, Jeremy Asher, Alex Couture-Beil and contributors."; 00117 00119 const char WEBSITE[] = "http://playerstage.org"; 00120 00122 const char DESCRIPTION[] = 00123 "Robot simulation library\nPart of the Player Project"; 00124 00126 const char LICENSE[] = 00127 "Stage robot simulation library\n" \ 00128 "Copyright (C) 2000-2009 Richard Vaughan and contributors\n" \ 00129 "Part of the Player Project [http://playerstage.org]\n" \ 00130 "\n" \ 00131 "This program is free software; you can redistribute it and/or\n" \ 00132 "modify it under the terms of the GNU General Public License\n" \ 00133 "as published by the Free Software Foundation; either version 2\n" \ 00134 "of the License, or (at your option) any later version.\n" \ 00135 "\n" \ 00136 "This program is distributed in the hope that it will be useful,\n" \ 00137 "but WITHOUT ANY WARRANTY; without even the implied warranty of\n" \ 00138 "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n" \ 00139 "GNU General Public License for more details.\n" \ 00140 "\n" \ 00141 "You should have received a copy of the GNU General Public License\n" \ 00142 "along with this program; if not, write to the Free Software\n" \ 00143 "Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.\n" \ 00144 "\n" \ 00145 "The text of the license may also be available online at\n" \ 00146 "http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n"; 00147 00149 const double thousand = 1e3; 00150 00152 const double million = 1e6; 00153 00155 const double billion = 1e9; 00156 00158 inline double rtod( double r ){ return( r*180.0/M_PI ); } 00159 00161 inline double dtor( double d ){ return( d*M_PI/180.0 ); } 00162 00164 inline double normalize( double a ) 00165 { 00166 while( a < -M_PI ) a += 2.0*M_PI; 00167 while( a > M_PI ) a -= 2.0*M_PI; 00168 return a; 00169 }; 00170 00172 inline int sgn( int a){ return( a<0 ? -1 : 1); } 00173 00175 inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); } 00176 00178 enum { FiducialNone = 0 }; 00179 00181 typedef uint32_t stg_id_t; 00182 00184 typedef double stg_meters_t; 00185 00187 typedef double stg_radians_t; 00188 00190 typedef struct timeval stg_time_t; 00191 00193 typedef unsigned long stg_msec_t; 00194 00196 typedef uint64_t stg_usec_t; 00197 00199 typedef double stg_kg_t; // Kilograms (mass) 00200 00202 typedef double stg_joules_t; 00203 00205 typedef double stg_watts_t; 00206 00208 typedef bool stg_bool_t; 00209 00210 class Color 00211 { 00212 public: 00213 float r,g,b,a; 00214 00215 Color( float r, float g, float b, float a=1.0 ); 00216 00220 Color( const std::string& name ); 00221 00222 Color(); 00223 00224 bool operator!=( const Color& other ); 00225 bool operator==( const Color& other ); 00226 static Color RandomColor(); 00227 void Print( const char* prefix ); 00228 }; 00229 00231 class Size 00232 { 00233 public: 00234 stg_meters_t x, y, z; 00235 00236 Size( stg_meters_t x, 00237 stg_meters_t y, 00238 stg_meters_t z ) 00239 : x(x), y(y), z(z) 00240 {/*empty*/} 00241 00243 Size() : x( 0.4 ), y( 0.4 ), z( 1.0 ) 00244 {/*empty*/} 00245 00246 void Load( Worldfile* wf, int section, const char* keyword ); 00247 void Save( Worldfile* wf, int section, const char* keyword ); 00248 00249 void Zero() 00250 { x=y=z=0.0; } 00251 }; 00252 00254 class Pose 00255 { 00256 public: 00257 stg_meters_t x, y, z; 00258 stg_radians_t a; 00259 00260 Pose( stg_meters_t x, 00261 stg_meters_t y, 00262 stg_meters_t z, 00263 stg_radians_t a ) 00264 : x(x), y(y), z(z), a(a) 00265 { /*empty*/ } 00266 00267 Pose() : x(0.0), y(0.0), z(0.0), a(0.0) 00268 { /*empty*/ } 00269 00270 virtual ~Pose(){}; 00271 00274 static Pose Random( stg_meters_t xmin, stg_meters_t xmax, 00275 stg_meters_t ymin, stg_meters_t ymax ) 00276 { 00277 return Pose( xmin + drand48() * (xmax-xmin), 00278 ymin + drand48() * (ymax-ymin), 00279 0, 00280 normalize( drand48() * (2.0 * M_PI) )); 00281 } 00282 00286 virtual void Print( const char* prefix ) 00287 { 00288 printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n", 00289 prefix, x,y,z,a ); 00290 } 00291 00292 std::string String() 00293 { 00294 char buf[256]; 00295 snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]", 00296 x,y,z,a ); 00297 return std::string(buf); 00298 } 00299 00300 /* returns true iff all components of the velocity are zero. */ 00301 bool IsZero() const { return( !(x || y || z || a )); }; 00302 00304 void Zero(){ x=y=z=a=0.0; } 00305 00306 void Load( Worldfile* wf, int section, const char* keyword ); 00307 void Save( Worldfile* wf, int section, const char* keyword ); 00308 00309 inline Pose operator+( const Pose& p ) 00310 { 00311 const double cosa = cos(a); 00312 const double sina = sin(a); 00313 00314 return Pose( x + p.x * cosa - p.y * sina, 00315 y + p.x * sina + p.y * cosa, 00316 z + p.z, 00317 normalize(a + p.a) ); 00318 } 00319 }; 00320 00321 00323 class Velocity : public Pose 00324 { 00325 public: 00331 Velocity( stg_meters_t x, 00332 stg_meters_t y, 00333 stg_meters_t z, 00334 stg_radians_t a ) : 00335 Pose( x, y, z, a ) 00336 { /*empty*/ } 00337 00338 Velocity() 00339 { /*empty*/ } 00340 00344 virtual void Print( const char* prefix ) 00345 { 00346 printf( "%s velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n", 00347 prefix, x,y,z,a ); 00348 } 00349 }; 00350 00353 class Geom 00354 { 00355 public: 00356 Pose pose; 00357 Size size; 00358 00359 void Print( const char* prefix ) 00360 { 00361 printf( "%s geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n", 00362 prefix, 00363 pose.x, 00364 pose.y, 00365 pose.a, 00366 size.x, 00367 size.y ); 00368 } 00369 00371 Geom() : pose(), size() {} 00372 00374 Geom( const Pose& p, const Size& s ) : pose(p), size(s) {} 00375 00376 void Zero() 00377 { 00378 pose.Zero(); 00379 size.Zero(); 00380 } 00381 }; 00382 00384 class Bounds 00385 { 00386 public: 00388 double min; 00390 double max; 00391 00392 Bounds() : min(0), max(0) { /* empty*/ } 00393 Bounds( double min, double max ) : min(min), max(max) { /* empty*/ } 00394 }; 00395 00397 class stg_bounds3d_t 00398 { 00399 public: 00401 Bounds x; 00403 Bounds y; 00405 Bounds z; 00406 00407 stg_bounds3d_t() : x(), y(), z() {} 00408 stg_bounds3d_t( const Bounds& x, const Bounds& y, const Bounds& z) 00409 : x(x), y(y), z(z) {} 00410 }; 00411 00413 typedef struct 00414 { 00415 Bounds range; 00416 stg_radians_t angle; 00417 } stg_fov_t; 00418 00420 class stg_point_t 00421 { 00422 public: 00423 stg_meters_t x, y; 00424 stg_point_t( stg_meters_t x, stg_meters_t y ) : x(x), y(y){} 00425 stg_point_t() : x(0.0), y(0.0){} 00426 00427 bool operator+=( const stg_point_t& other ) 00428 { return ((x += other.x) && (y += other.y) ); } 00429 }; 00430 00432 class stg_point3_t 00433 { 00434 public: 00435 stg_meters_t x,y,z; 00436 stg_point3_t( stg_meters_t x, stg_meters_t y, stg_meters_t z ) 00437 : x(x), y(y), z(z) {} 00438 00439 stg_point3_t() : x(0.0), y(0.0), z(0.0) {} 00440 }; 00441 00443 class stg_point_int_t 00444 { 00445 public: 00446 int x,y; 00447 stg_point_int_t( int x, int y ) : x(x), y(y){} 00448 stg_point_int_t() : x(0), y(0){} 00449 00451 bool operator<( const stg_point_int_t& other ) const 00452 { 00453 if( x < other.x ) return true; 00454 if( other.x < x ) return false; 00455 return y < other.y; 00456 } 00457 00458 bool operator==( const stg_point_int_t& other ) const 00459 { return ((x == other.x) && (y == other.y) ); } 00460 }; 00461 00462 typedef std::vector<stg_point_int_t> PointIntVec; 00463 00466 stg_point_t* stg_unit_square_points_create(); 00467 00470 typedef enum 00471 { 00472 LaserTransparent=0, 00473 LaserVisible, 00474 LaserBright 00475 } stg_laser_return_t; 00476 00479 namespace Gl 00480 { 00481 void pose_shift( const Pose &pose ); 00482 void pose_inverse_shift( const Pose &pose ); 00483 void coord_shift( double x, double y, double z, double a ); 00484 void draw_grid( stg_bounds3d_t vol ); 00486 void draw_string( float x, float y, float z, const char *string); 00487 void draw_string_multiline( float x, float y, float w, float h, 00488 const char *string, Fl_Align align ); 00489 void draw_speech_bubble( float x, float y, float z, const char* str ); 00490 void draw_octagon( float w, float h, float m ); 00491 void draw_octagon( float x, float y, float w, float h, float m ); 00492 void draw_vector( double x, double y, double z ); 00493 void draw_origin( double len ); 00494 void draw_array( float x, float y, float w, float h, 00495 float* data, size_t len, size_t offset, 00496 float min, float max ); 00497 void draw_array( float x, float y, float w, float h, 00498 float* data, size_t len, size_t offset ); 00500 void draw_centered_rect( float x, float y, float dx, float dy ); 00501 } // namespace Gl 00502 00503 void RegisterModels(); 00504 00505 00507 class Visualizer { 00508 private: 00509 const std::string menu_name; 00510 const std::string worldfile_name; 00511 00512 public: 00513 Visualizer( const std::string& menu_name, 00514 const std::string& worldfile_name ) 00515 : menu_name( menu_name ), 00516 worldfile_name( worldfile_name ) 00517 { } 00518 00519 virtual ~Visualizer( void ) { } 00520 virtual void Visualize( Model* mod, Camera* cam ) = 0; 00521 00522 const std::string& GetMenuName() { return menu_name; } 00523 const std::string& GetWorldfileName() { return worldfile_name; } 00524 }; 00525 00526 00527 typedef int(*stg_model_callback_t)(Model* mod, void* user ); 00528 typedef int(*stg_world_callback_t)(World* world, void* user ); 00529 00530 // return val, or minval if val < minval, or maxval if val > maxval 00531 double constrain( double val, double minval, double maxval ); 00532 00533 typedef struct 00534 { 00535 int enabled; 00536 Pose pose; 00537 stg_meters_t size; 00538 Color color; 00539 stg_msec_t period; 00540 double duty_cycle; 00541 } stg_blinkenlight_t; 00542 00543 00545 typedef struct 00546 { 00547 Pose pose; 00548 Size size; 00549 } stg_rotrect_t; // rotated rectangle 00550 00556 int stg_rotrects_from_image_file( const char* filename, 00557 stg_rotrect_t** rects, 00558 unsigned int* rect_count, 00559 unsigned int* widthp, 00560 unsigned int* heightp ); 00561 00562 00565 typedef bool (*stg_ray_test_func_t)(Model* candidate, 00566 Model* finder, 00567 const void* arg ); 00568 00569 // STL container iterator macros 00570 #define VAR(V,init) __typeof(init) V=(init) 00571 #define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();I++) 00572 00574 template <class T, class C> 00575 void EraseAll( T thing, C& cont ) 00576 { cont.erase( std::remove( cont.begin(), cont.end(), thing ), cont.end() ); } 00577 00578 // Error macros - output goes to stderr 00579 #define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__) 00580 #define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__) 00581 #define PRINT_ERR2(m,a,b) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 00582 #define PRINT_ERR3(m,a,b,c) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__) 00583 #define PRINT_ERR4(m,a,b,c,d) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__) 00584 #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__) 00585 00586 // Warning macros 00587 #define PRINT_WARN(m) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__) 00588 #define PRINT_WARN1(m,a) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__) 00589 #define PRINT_WARN2(m,a,b) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 00590 #define PRINT_WARN3(m,a,b,c) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__) 00591 #define PRINT_WARN4(m,a,b,c,d) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__) 00592 #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__) 00593 00594 // Message macros 00595 #ifdef DEBUG 00596 #define PRINT_MSG(m) printf( "Stage: "m" (%s %s)\n", __FILE__, __FUNCTION__) 00597 #define PRINT_MSG1(m,a) printf( "Stage: "m" (%s %s)\n", a, __FILE__, __FUNCTION__) 00598 #define PRINT_MSG2(m,a,b) printf( "Stage: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 00599 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__) 00600 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__) 00601 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m" (%s %s)\n", a, b, c, d, e,__FILE__, __FUNCTION__) 00602 #else 00603 #define PRINT_MSG(m) printf( "Stage: "m"\n" ) 00604 #define PRINT_MSG1(m,a) printf( "Stage: "m"\n", a) 00605 #define PRINT_MSG2(m,a,b) printf( "Stage: "m"\n,", a, b ) 00606 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m"\n", a, b, c ) 00607 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m"\n", a, b, c, d ) 00608 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m"\n", a, b, c, d, e ) 00609 #endif 00610 00611 // DEBUG macros 00612 #ifdef DEBUG 00613 #define PRINT_DEBUG(m) printf( "debug: "m" (%s %s)\n", __FILE__, __FUNCTION__) 00614 #define PRINT_DEBUG1(m,a) printf( "debug: "m" (%s %s)\n", a, __FILE__, __FUNCTION__) 00615 #define PRINT_DEBUG2(m,a,b) printf( "debug: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 00616 #define PRINT_DEBUG3(m,a,b,c) printf( "debug: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__) 00617 #define PRINT_DEBUG4(m,a,b,c,d) printf( "debug: "m" (%s %s)\n", a, b, c ,d, __FILE__, __FUNCTION__) 00618 #define PRINT_DEBUG5(m,a,b,c,d,e) printf( "debug: "m" (%s %s)\n", a, b, c ,d, e, __FILE__, __FUNCTION__) 00619 #else 00620 #define PRINT_DEBUG(m) 00621 #define PRINT_DEBUG1(m,a) 00622 #define PRINT_DEBUG2(m,a,b) 00623 #define PRINT_DEBUG3(m,a,b,c) 00624 #define PRINT_DEBUG4(m,a,b,c,d) 00625 #define PRINT_DEBUG5(m,a,b,c,d,e) 00626 #endif 00627 00628 class Block; 00629 class Model; 00630 00633 typedef int (*stg_model_callback_t)( Model* mod, void* user ); 00634 00635 // ANCESTOR CLASS 00637 class Ancestor 00638 { 00639 friend class Canvas; // allow Canvas access to our private members 00640 00641 protected: 00642 ModelPtrVec children; 00643 bool debug; 00644 std::string token; 00645 pthread_mutex_t access_mutex; 00646 00647 void Load( Worldfile* wf, int section ); 00648 void Save( Worldfile* wf, int section ); 00649 00650 public: 00651 /* The maximum length of a Stage model identifier string */ 00652 //static const uint32_t TOKEN_MAX = 64; 00653 00655 ModelPtrVec& GetChildren(){ return children;} 00656 00658 void ForEachDescendant( stg_model_callback_t func, void* arg ); 00659 00661 std::map<std::string,unsigned int> child_type_counts; 00662 00663 Ancestor(); 00664 virtual ~Ancestor(); 00665 00666 virtual void AddChild( Model* mod ); 00667 virtual void RemoveChild( Model* mod ); 00668 virtual Pose GetGlobalPose(); 00669 00670 const char* Token() 00671 { return token.c_str(); } 00672 00673 void SetToken( const std::string& str ) 00674 { token = str; } 00675 00676 void Lock(){ pthread_mutex_lock( &access_mutex ); } 00677 void Unlock(){ pthread_mutex_unlock( &access_mutex ); } 00678 }; 00679 00682 class RaytraceResult 00683 { 00684 public: 00685 Pose pose; 00686 stg_meters_t range; 00687 Model* mod; 00688 Color color; 00689 00690 RaytraceResult() : pose(), range(0), mod(NULL), color() {} 00691 RaytraceResult( const Pose& pose, 00692 stg_meters_t range ) 00693 : pose(pose), range(range), mod(NULL), color() {} 00694 }; 00695 00696 typedef RaytraceResult stg_raytrace_result_t; 00697 00698 class Ray 00699 { 00700 public: 00701 Ray( const Model* mod, const Pose& origin, const stg_meters_t range, const stg_ray_test_func_t func, const void* arg, const bool ztest ) : 00702 mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest) 00703 {} 00704 00705 Ray() : mod(NULL), origin(0,0,0,0), range(0), func(NULL), arg(NULL), ztest(true) 00706 {} 00707 00708 const Model* mod; 00709 Pose origin; 00710 stg_meters_t range; 00711 stg_ray_test_func_t func; 00712 const void* arg; 00713 bool ztest; 00714 }; 00715 00716 00717 // defined in stage_internal.hh 00718 class Region; 00719 class SuperRegion; 00720 class BlockGroup; 00721 class PowerPack; 00722 00723 class LogEntry 00724 { 00725 stg_usec_t timestamp; 00726 Model* mod; 00727 Pose pose; 00728 00729 public: 00730 LogEntry( stg_usec_t timestamp, Model* mod ); 00731 00733 static std::vector<LogEntry> log; 00734 00736 static size_t Count(){ return log.size(); } 00737 00739 static void Clear(){ log.clear(); } 00740 00742 static void Print(); 00743 }; 00744 00745 class CtrlArgs 00746 { 00747 public: 00748 std::string worldfile; 00749 std::string cmdline; 00750 00751 CtrlArgs( std::string w, std::string c ) : worldfile(w), cmdline(c) {} 00752 }; 00753 00755 class World : public Ancestor 00756 { 00757 friend class Block; 00758 friend class Model; // allow access to private members 00759 friend class ModelFiducial; 00760 friend class Canvas; 00761 00762 public: 00765 static std::vector<std::string> args; 00766 static std::string ctrlargs; 00767 00768 private: 00769 00770 static std::set<World*> world_set; 00771 static bool quit_all; 00772 static void UpdateCb( World* world); 00773 static unsigned int next_id; 00774 00775 bool destroy; 00776 bool dirty; 00777 00779 std::set<Model*> models; 00780 00782 std::map<std::string, Model*> models_by_name; 00783 00785 std::map<int,Model*> models_by_wfentity; 00786 00789 ModelPtrVec models_with_fiducials; 00790 00792 void FiducialInsert( Model* mod ) 00793 { 00794 FiducialErase( mod ); // make sure it's not there already 00795 models_with_fiducials.push_back( mod ); 00796 } 00797 00799 void FiducialErase( Model* mod ) 00800 { 00801 EraseAll( mod, models_with_fiducials ); 00802 } 00803 00804 double ppm; 00805 bool quit; 00806 00807 bool show_clock; 00808 unsigned int show_clock_interval; 00809 00810 pthread_mutex_t thread_mutex; 00811 unsigned int threads_working; 00812 pthread_cond_t threads_start_cond; 00813 pthread_cond_t threads_done_cond; 00814 int total_subs; 00815 unsigned int worker_threads; 00816 00817 protected: 00818 00819 std::list<std::pair<stg_world_callback_t,void*> > cb_list; 00820 stg_bounds3d_t extent; 00821 bool graphics; 00822 00823 std::set<Option*> option_table; 00824 std::list<PowerPack*> powerpack_list; 00825 00826 stg_usec_t quit_time; 00827 std::list<float*> ray_list; 00828 stg_usec_t sim_time; 00829 std::map<stg_point_int_t,SuperRegion*> superregions; 00830 SuperRegion* sr_cached; 00831 00832 std::vector<ModelPtrVec> update_lists; 00833 00834 uint64_t updates; 00835 Worldfile* wf; 00836 00837 void CallUpdateCallbacks(); 00838 00839 public: 00840 00841 bool paused; 00842 00843 virtual void Start(){ paused = false; }; 00844 virtual void Stop(){ paused = true; }; 00845 virtual void TogglePause(){ paused ? Start() : Stop(); }; 00846 00847 bool Paused(){ return( paused ); }; 00848 00849 PointIntVec rt_cells; 00850 PointIntVec rt_candidate_cells; 00851 00852 static const int DEFAULT_PPM = 50; // default resolution in pixels per meter 00853 00856 void AddUpdateCallback( stg_world_callback_t cb, void* user ); 00857 00860 int RemoveUpdateCallback( stg_world_callback_t cb, void* user ); 00862 void Log( Model* mod ); 00863 00865 void NeedRedraw(){ dirty = true; }; 00866 00868 Model* ground; 00869 00872 virtual std::string ClockString( void ) const; 00873 00874 Model* CreateModel( Model* parent, const std::string& typestr ); 00875 void LoadModel( Worldfile* wf, int entity ); 00876 void LoadBlock( Worldfile* wf, int entity ); 00877 void LoadBlockGroup( Worldfile* wf, int entity ); 00878 00879 virtual Model* RecentlySelectedModel(){ return NULL; } 00880 00881 SuperRegion* AddSuperRegion( const stg_point_int_t& coord ); 00882 SuperRegion* GetSuperRegion( const stg_point_int_t& coord ); 00883 SuperRegion* GetSuperRegionCached( const stg_point_int_t& coord); 00884 SuperRegion* GetSuperRegionCached( int32_t x, int32_t y ); 00885 void ExpireSuperRegion( SuperRegion* sr ); 00886 00889 void ForEachCellInLine( const stg_point_int_t& pt1, 00890 const stg_point_int_t& pt2, 00891 CellPtrVec& cells ); 00892 00895 int32_t MetersToPixels( stg_meters_t x ) 00896 { return (int32_t)floor(x * ppm); }; 00897 00898 stg_point_int_t MetersToPixels( const stg_point_t& pt ) 00899 { return stg_point_int_t( MetersToPixels(pt.x), MetersToPixels(pt.y)); }; 00900 00901 // dummy implementations to be overloaded by GUI subclasses 00902 virtual void PushColor( Color col ) 00903 { /* do nothing */ (void)col; }; 00904 virtual void PushColor( double r, double g, double b, double a ) 00905 { /* do nothing */ (void)r; (void)g; (void)b; (void)a; }; 00906 00907 virtual void PopColor(){ /* do nothing */ }; 00908 00909 SuperRegion* CreateSuperRegion( stg_point_int_t origin ); 00910 void DestroySuperRegion( SuperRegion* sr ); 00911 00913 stg_raytrace_result_t Raytrace( const Ray& ray ); 00914 00915 stg_raytrace_result_t Raytrace( const Pose& pose, 00916 const stg_meters_t range, 00917 const stg_ray_test_func_t func, 00918 const Model* finder, 00919 const void* arg, 00920 const bool ztest ); 00921 00922 void Raytrace( const Pose &pose, 00923 const stg_meters_t range, 00924 const stg_radians_t fov, 00925 const stg_ray_test_func_t func, 00926 const Model* finder, 00927 const void* arg, 00928 stg_raytrace_result_t* samples, 00929 const uint32_t sample_count, 00930 const bool ztest ); 00931 00932 00934 void Extend( stg_point3_t pt ); 00935 00936 virtual void AddModel( Model* mod ); 00937 virtual void RemoveModel( Model* mod ); 00938 00939 void AddModelName( Model* mod, const std::string& name ); 00940 00941 void AddPowerPack( PowerPack* pp ); 00942 void RemovePowerPack( PowerPack* pp ); 00943 00944 void ClearRays(); 00945 00947 void RecordRay( double x1, double y1, double x2, double y2 ); 00948 00951 bool PastQuitTime(); 00952 00953 static void* update_thread_entry( std::pair<World*,int>* info ); 00954 00955 class Event 00956 { 00957 public: 00958 00959 Event( stg_usec_t time, Model* mod ) 00960 : time(time), mod(mod) {} 00961 00962 stg_usec_t time; // time that event occurs 00963 Model* mod; // model to update 00964 00965 bool operator<( const Event& other ) const; 00966 }; 00967 00968 std::vector<std::priority_queue<Event> > event_queues; 00969 void Enqueue( unsigned int queue_num, stg_usec_t delay, Model* mod ); 00970 00971 std::set<Model*> active_energy; 00972 std::set<Model*> active_velocity; 00973 00975 stg_usec_t sim_interval; 00976 00978 void ConsumeQueue( unsigned int queue_num ); 00979 00982 unsigned int GetEventQueue( Model* mod ); 00983 00984 public: 00986 static bool UpdateAll(); 00987 00988 World( const std::string& name = "MyWorld", 00989 double ppm = DEFAULT_PPM ); 00990 00991 virtual ~World(); 00992 00993 stg_usec_t SimTimeNow(void); 00994 00995 Worldfile* GetWorldFile(){ return wf; }; 00996 00997 virtual bool IsGUI() const { return false; } 00998 00999 virtual void Load( const char* worldfile_path ); 01000 virtual void UnLoad(); 01001 virtual void Reload(); 01002 virtual bool Save( const char* filename ); 01003 virtual bool Update(void); 01004 01005 bool TestQuit(){ return( quit || quit_all ); } 01006 void Quit(){ quit = true; } 01007 void QuitAll(){ quit_all = true; } 01008 void CancelQuit(){ quit = false; } 01009 void CancelQuitAll(){ quit_all = false; } 01010 01011 void TryCharge( PowerPack* pp, const Pose& pose ); 01012 01015 double Resolution(){ return ppm; }; 01016 01019 Model* GetModel( const std::string& name ) const; 01020 01022 const stg_bounds3d_t& GetExtent(){ return extent; }; 01023 01025 uint64_t GetUpdateCount() { return updates; } 01026 01028 void RegisterOption( Option* opt ); 01029 01031 void ShowClock( bool enable ){ show_clock = enable; }; 01032 01034 Model* GetGround() {return ground;}; 01035 01036 }; 01037 01038 class Block 01039 { 01040 friend class BlockGroup; 01041 friend class Model; 01042 friend class SuperRegion; 01043 friend class World; 01044 friend class Canvas; 01045 public: 01046 01050 Block( Model* mod, 01051 stg_point_t* pts, 01052 size_t pt_count, 01053 stg_meters_t zmin, 01054 stg_meters_t zmax, 01055 Color color, 01056 bool inherit_color ); 01057 01059 Block( Model* mod, Worldfile* wf, int entity); 01060 01061 ~Block(); 01062 01064 void Map(); 01065 01067 void UnMap(); 01068 01070 void DrawSolid(); 01071 01073 void DrawFootPrint(); 01074 01076 void Translate( double x, double y ); 01077 01079 double CenterX(); 01080 01082 double CenterY(); 01083 01085 void SetCenterX( double y ); 01086 01088 void SetCenterY( double y ); 01089 01091 void SetCenter( double x, double y); 01092 01094 void SetZ( double min, double max ); 01095 01096 stg_point_t* Points( unsigned int *count ) 01097 { if( count ) *count = pt_count; return &pts[0]; }; 01098 01099 std::vector<stg_point_t>& Points() 01100 { return pts; }; 01101 01102 inline void RemoveFromCellArray( CellPtrVec* blocks ); 01103 inline void GenerateCandidateCells(); 01104 01105 void AppendTouchingModels( ModelPtrSet& touchers ); 01106 01108 Model* TestCollision(); 01109 void SwitchToTestedCells(); 01110 void Load( Worldfile* wf, int entity ); 01111 Model* GetModel(){ return mod; }; 01112 const Color& GetColor(); 01113 void Rasterize( uint8_t* data, 01114 unsigned int width, unsigned int height, 01115 stg_meters_t cellwidth, stg_meters_t cellheight ); 01116 01117 private: 01118 Model* mod; 01119 std::vector<stg_point_t> mpts; 01120 size_t pt_count; 01121 std::vector<stg_point_t> pts; 01122 Size size; 01123 Bounds local_z; 01124 Color color; 01125 bool inherit_color; 01126 01128 double glow; 01129 01130 void DrawTop(); 01131 void DrawSides(); 01132 01134 Bounds global_z; 01135 bool mapped; 01136 01138 std::vector< std::list<Block*>::iterator > list_entries; 01139 01142 CellPtrVec * rendered_cells; 01143 01150 CellPtrVec * candidate_cells; 01151 01152 PointIntVec gpts; 01153 01156 stg_point_t BlockPointToModelMeters( const stg_point_t& bpt ); 01157 01159 void InvalidateModelPointCache(); 01160 }; 01161 01162 01163 class BlockGroup 01164 { 01165 friend class Model; 01166 friend class Block; 01167 01168 private: 01169 int displaylist; 01170 01171 void BuildDisplayList( Model* mod ); 01172 01173 BlockPtrSet blocks; 01174 Size size; 01175 stg_point3_t offset; 01176 stg_meters_t minx, maxx, miny, maxy; 01177 01178 public: 01179 BlockGroup(); 01180 ~BlockGroup(); 01181 01182 uint32_t GetCount(){ return blocks.size(); }; 01183 const Size& GetSize(){ return size; }; 01184 const stg_point3_t& GetOffset(){ return offset; }; 01185 01188 void CalcSize(); 01189 01190 void AppendBlock( Block* block ); 01191 void CallDisplayList( Model* mod ); 01192 void Clear() ; 01194 void AppendTouchingModels( ModelPtrSet& touchers ); 01195 01198 Model* TestCollision(); 01199 01200 void SwitchToTestedCells(); 01201 01202 void Map(); 01203 void UnMap(); 01204 01206 void DrawSolid( const Geom &geom); 01207 01209 void DrawFootPrint( const Geom &geom); 01210 01211 void LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfile *wf ); 01212 void LoadBlock( Model* mod, Worldfile* wf, int entity ); 01213 01214 void Rasterize( uint8_t* data, 01215 unsigned int width, unsigned int height, 01216 stg_meters_t cellwidth, stg_meters_t cellheight ); 01217 01218 void InvalidateModelPointCache() 01219 { 01220 FOR_EACH( it, blocks ) 01221 (*it)->InvalidateModelPointCache(); 01222 } 01223 01224 }; 01225 01226 class Camera 01227 { 01228 protected: 01229 float _pitch; //left-right (about y) 01230 float _yaw; //up-down (about x) 01231 float _x, _y, _z; 01232 01233 public: 01234 Camera() : _pitch( 0 ), _yaw( 0 ), _x( 0 ), _y( 0 ), _z( 0 ) { } 01235 virtual ~Camera() { } 01236 01237 virtual void Draw( void ) const = 0; 01238 virtual void SetProjection( void ) const = 0; 01239 01240 float yaw( void ) const { return _yaw; } 01241 float pitch( void ) const { return _pitch; } 01242 01243 float x( void ) const { return _x; } 01244 float y( void ) const { return _y; } 01245 float z( void ) const { return _z; } 01246 01247 virtual void reset() = 0; 01248 virtual void Load( Worldfile* wf, int sec ) = 0; 01249 01250 //TODO data should be passed in somehow else. (at least min/max stuff) 01251 //virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const = 0; 01252 }; 01253 01254 class PerspectiveCamera : public Camera 01255 { 01256 private: 01257 float _z_near; 01258 float _z_far; 01259 float _vert_fov; 01260 float _horiz_fov; 01261 float _aspect; 01262 01263 public: 01264 PerspectiveCamera( void ); 01265 01266 virtual void Draw( void ) const; 01267 virtual void SetProjection( void ) const; 01268 //void SetProjection( float aspect ) const; 01269 void update( void ); 01270 01271 void strafe( float amount ); 01272 void forward( float amount ); 01273 01274 void setPose( float x, float y, float z ) { _x = x; _y = y; _z = z; } 01275 void addPose( float x, float y, float z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; } 01276 void move( float x, float y, float z ); 01277 void setFov( float horiz_fov, float vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; } 01279 void setAspect( float aspect ) { _aspect = aspect; } 01280 void setYaw( float yaw ) { _yaw = yaw; } 01281 float horizFov( void ) const { return _horiz_fov; } 01282 float vertFov( void ) const { return _vert_fov; } 01283 void addYaw( float yaw ) { _yaw += yaw; } 01284 void setPitch( float pitch ) { _pitch = pitch; } 01285 void addPitch( float pitch ) { _pitch += pitch; if( _pitch < 0 ) _pitch = 0; else if( _pitch > 180 ) _pitch = 180; } 01286 01287 float realDistance( float z_buf_val ) const { 01288 return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) ); 01289 } 01290 void scroll( float dy ) { _z += dy; } 01291 float nearClip( void ) const { return _z_near; } 01292 float farClip( void ) const { return _z_far; } 01293 void setClip( float near, float far ) { _z_far = far; _z_near = near; } 01294 01295 void reset() { setPitch( 70 ); setYaw( 0 ); } 01296 01297 void Load( Worldfile* wf, int sec ); 01298 void Save( Worldfile* wf, int sec ); 01299 }; 01300 01301 class OrthoCamera : public Camera 01302 { 01303 private: 01304 float _scale; 01305 float _pixels_width; 01306 float _pixels_height; 01307 float _y_min; 01308 float _y_max; 01309 01310 public: 01311 OrthoCamera( void ) : _scale( 15 ) { } 01312 virtual void Draw() const; 01313 virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ); 01314 virtual void SetProjection( void ) const; 01315 01316 void move( float x, float y ); 01317 void setYaw( float yaw ) { _yaw = yaw; } 01318 void setPitch( float pitch ) { _pitch = pitch; } 01319 void addYaw( float yaw ) { _yaw += yaw; } 01320 void addPitch( float pitch ) { 01321 _pitch += pitch; 01322 if( _pitch > 90 ) 01323 _pitch = 90; 01324 else if( _pitch < 0 ) 01325 _pitch = 0; 01326 } 01327 01328 void setScale( float scale ) { _scale = scale; } 01329 void setPose( float x, float y) { _x = x; _y = y; } 01330 01331 void scale( float scale, float shift_x = 0, float h = 0, float shift_y = 0, float w = 0 ); 01332 void reset( void ) { _pitch = _yaw = 0; } 01333 01334 float scale() const { return _scale; } 01335 01336 void Load( Worldfile* wf, int sec ); 01337 void Save( Worldfile* wf, int sec ); 01338 }; 01339 01340 01344 class WorldGui : public World, public Fl_Window 01345 { 01346 friend class Canvas; 01347 friend class ModelCamera; 01348 friend class Model; 01349 friend class Option; 01350 01351 private: 01352 01353 Canvas* canvas; 01354 std::vector<Option*> drawOptions; 01355 FileManager* fileMan; 01356 std::vector<stg_usec_t> interval_log; 01357 01360 float speedup; 01361 01362 Fl_Menu_Bar* mbar; 01363 OptionsDlg* oDlg; 01364 bool pause_time; 01365 01368 stg_usec_t real_time_interval; 01369 01371 stg_usec_t real_time_now; 01372 01375 stg_usec_t real_time_recorded; 01376 01378 uint64_t timing_interval; 01379 01380 // static callback functions 01381 static void windowCb( Fl_Widget* w, WorldGui* wg ); 01382 static void fileLoadCb( Fl_Widget* w, WorldGui* wg ); 01383 static void fileSaveCb( Fl_Widget* w, WorldGui* wg ); 01384 static void fileSaveAsCb( Fl_Widget* w, WorldGui* wg ); 01385 static void fileExitCb( Fl_Widget* w, WorldGui* wg ); 01386 static void viewOptionsCb( OptionsDlg* oDlg, WorldGui* wg ); 01387 static void optionsDlgCb( OptionsDlg* oDlg, WorldGui* wg ); 01388 static void helpAboutCb( Fl_Widget* w, WorldGui* wg ); 01389 static void pauseCb( Fl_Widget* w, WorldGui* wg ); 01390 static void onceCb( Fl_Widget* w, WorldGui* wg ); 01391 static void fasterCb( Fl_Widget* w, WorldGui* wg ); 01392 static void slowerCb( Fl_Widget* w, WorldGui* wg ); 01393 static void realtimeCb( Fl_Widget* w, WorldGui* wg ); 01394 static void fasttimeCb( Fl_Widget* w, WorldGui* wg ); 01395 static void resetViewCb( Fl_Widget* w, WorldGui* wg ); 01396 static void moreHelptCb( Fl_Widget* w, WorldGui* wg ); 01397 01398 // GUI functions 01399 bool saveAsDialog(); 01400 bool closeWindowQuery(); 01401 01402 virtual void AddModel( Model* mod ); 01403 01404 void SetTimeouts(); 01405 01406 protected: 01407 01408 virtual void PushColor( Color col ); 01409 virtual void PushColor( double r, double g, double b, double a ); 01410 virtual void PopColor(); 01411 01412 void DrawOccupancy(); 01413 void DrawVoxels(); 01414 01415 public: 01416 01417 WorldGui(int W,int H,const char*L=0); 01418 ~WorldGui(); 01419 01420 virtual std::string ClockString() const; 01421 virtual bool Update(); 01422 virtual void Load( const char* filename ); 01423 virtual void UnLoad(); 01424 virtual bool Save( const char* filename ); 01425 virtual bool IsGUI() const { return true; }; 01426 virtual Model* RecentlySelectedModel() const; 01427 01428 virtual void Start(); 01429 virtual void Stop(); 01430 01431 stg_usec_t RealTimeNow(void) const; 01432 01433 void DrawBoundingBoxTree(); 01434 01435 Canvas* GetCanvas( void ) const { return canvas; } 01436 01438 void Show(); 01439 01441 std::string EnergyString( void ); 01442 virtual void RemoveChild( Model* mod ); 01443 }; 01444 01445 01446 class StripPlotVis : public Visualizer 01447 { 01448 private: 01449 01450 Model* mod; 01451 float* data; 01452 size_t len; 01453 size_t count; 01454 unsigned int index; 01455 float x,y,w,h,min,max; 01456 Color fgcolor, bgcolor; 01457 01458 public: 01459 StripPlotVis( float x, float y, float w, float h, 01460 size_t len, 01461 Color fgcolor, Color bgcolor, 01462 const char* name, const char* wfname ); 01463 virtual ~StripPlotVis(); 01464 virtual void Visualize( Model* mod, Camera* cam ); 01465 void AppendValue( float value ); 01466 }; 01467 01468 01469 class PowerPack 01470 { 01471 friend class WorldGui; 01472 friend class Canvas; 01473 01474 protected: 01475 01476 class DissipationVis : public Visualizer 01477 { 01478 private: 01479 unsigned int columns, rows; 01480 stg_meters_t width, height; 01481 01482 std::vector<stg_joules_t> cells; 01483 01484 stg_joules_t peak_value; 01485 double cellsize; 01486 01487 static stg_joules_t global_peak_value; 01488 01489 public: 01490 DissipationVis( stg_meters_t width, 01491 stg_meters_t height, 01492 stg_meters_t cellsize ); 01493 01494 virtual ~DissipationVis(); 01495 virtual void Visualize( Model* mod, Camera* cam ); 01496 01497 void Accumulate( stg_meters_t x, stg_meters_t y, stg_joules_t amount ); 01498 } event_vis; 01499 01500 01501 StripPlotVis output_vis; 01502 StripPlotVis stored_vis; 01503 01505 Model* mod; 01506 01508 stg_joules_t stored; 01509 01511 stg_joules_t capacity; 01512 01514 bool charging; 01515 01517 stg_joules_t dissipated; 01518 01519 // these are used to visualize the power draw 01520 stg_usec_t last_time; 01521 stg_joules_t last_joules; 01522 stg_watts_t last_watts; 01523 01524 static stg_joules_t global_stored; 01525 static stg_joules_t global_capacity; 01526 static stg_joules_t global_dissipated; 01527 static stg_joules_t global_input; 01528 01529 public: 01530 PowerPack( Model* mod ); 01531 ~PowerPack(); 01532 01534 void Visualize( Camera* cam ); 01535 01538 void Print( char* prefix ) const; 01539 01541 stg_joules_t RemainingCapacity() const; 01542 01544 void Add( stg_joules_t j ); 01545 01547 void Subtract( stg_joules_t j ); 01548 01550 void TransferTo( PowerPack* dest, stg_joules_t amount ); 01551 01552 double ProportionRemaining() const 01553 { return( stored / capacity ); } 01554 01555 void Print( const char* prefix ) const 01556 { printf( "%s PowerPack %.2f/%.2f J\n", prefix, stored, capacity ); } 01557 01558 stg_joules_t GetStored() const; 01559 stg_joules_t GetCapacity() const; 01560 stg_joules_t GetDissipated() const; 01561 void SetCapacity( stg_joules_t j ); 01562 void SetStored( stg_joules_t j ); 01563 01565 bool GetCharging() const { return charging; } 01566 01567 void ChargeStart(){ charging = true; } 01568 void ChargeStop(){ charging = false; } 01569 01571 void Dissipate( stg_joules_t j ); 01572 01574 void Dissipate( stg_joules_t j, const Pose& p ); 01575 }; 01576 01577 01579 class Model : public Ancestor 01580 { 01581 friend class Ancestor; 01582 friend class World; 01583 friend class World::Event; 01584 friend class WorldGui; 01585 friend class Canvas; 01586 friend class Block; 01587 friend class Region; 01588 friend class BlockGroup; 01589 friend class PowerPack; 01590 friend class Ray; 01591 01592 private: 01594 static uint32_t count; 01595 static std::map<stg_id_t,Model*> modelsbyid; 01596 std::vector<Option*> drawOptions; 01597 const std::vector<Option*>& getOptions() const { return drawOptions; } 01598 01599 protected: 01600 pthread_mutex_t access_mutex; 01601 01604 bool alwayson; 01605 01606 BlockGroup blockgroup; 01608 int blocks_dl; 01609 01613 int boundary; 01614 01617 public: 01618 class stg_cb_t 01619 { 01620 public: 01621 stg_model_callback_t callback; 01622 void* arg; 01623 01624 stg_cb_t( stg_model_callback_t cb, void* arg ) 01625 : callback(cb), arg(arg) {} 01626 01627 stg_cb_t( stg_world_callback_t cb, void* arg ) 01628 : callback(NULL), arg(arg) { (void)cb; } 01629 01630 stg_cb_t() : callback(NULL), arg(NULL) {} 01631 01633 bool operator<( const stg_cb_t& other ) const 01634 { return ((void*)(callback)) < ((void*)(other.callback)); } 01635 01637 bool operator==( const stg_cb_t& other ) const 01638 { return( callback == other.callback); } 01639 }; 01640 01641 class Flag 01642 { 01643 public: 01644 Color color; 01645 double size; 01646 static int displaylist; 01647 01648 Flag( Color color, double size ); 01649 Flag* Nibble( double portion ); 01650 01653 void Draw( GLUquadric* quadric ); 01654 }; 01655 01656 protected: 01660 static std::map<void*, std::set<stg_cb_t> > callbacks; 01661 01663 Color color; 01664 double friction; 01665 01669 bool data_fresh; 01670 stg_bool_t disabled; 01671 std::list<Visualizer*> cv_list; 01672 std::list<Flag*> flag_list; 01673 Geom geom; 01674 01676 class GuiState 01677 { 01678 public: 01679 bool grid; 01680 bool move; 01681 bool nose; 01682 bool outline; 01683 01684 GuiState(); 01685 void Load( Worldfile* wf, int wf_entity ); 01686 } gui; 01687 01688 bool has_default_block; 01689 01690 /* Hooks for attaching special callback functions (not used as 01691 variables - we just need unique addresses for them.) */ 01692 class CallbackHooks 01693 { 01694 public: 01695 int flag_incr; 01696 int flag_decr; 01697 int init; 01698 int load; 01699 int save; 01700 int shutdown; 01701 int startup; 01702 int update; 01703 int update_done; 01704 01705 /* optimization: record the number of attached callbacks for pose 01706 and velocity, so we can cheaply determine whether we need to 01707 call a callback for SetPose() and SetVelocity(), which happen 01708 very frequently. */ 01709 int attached_velocity; 01710 int attached_pose; 01711 int attached_update; 01712 01713 CallbackHooks() : 01714 attached_velocity(0), 01715 attached_pose(0), 01716 attached_update(0) 01717 {} 01718 01719 } hooks; 01720 01722 uint32_t id; 01723 stg_usec_t interval; 01724 stg_usec_t interval_energy; 01725 stg_usec_t interval_pose; 01726 01727 stg_usec_t last_update; 01728 bool log_state; 01729 stg_meters_t map_resolution; 01730 stg_kg_t mass; 01731 01733 Model* parent; 01734 01737 Pose pose; 01738 01740 PowerPack* power_pack; 01741 01744 std::list<PowerPack*> pps_charging; 01745 01749 std::map<std::string,const void*> props; 01750 01752 class RasterVis : public Visualizer 01753 { 01754 private: 01755 uint8_t* data; 01756 unsigned int width, height; 01757 stg_meters_t cellwidth, cellheight; 01758 std::vector<stg_point_t> pts; 01759 01760 public: 01761 RasterVis(); 01762 virtual ~RasterVis( void ){} 01763 virtual void Visualize( Model* mod, Camera* cam ); 01764 01765 void SetData( uint8_t* data, 01766 unsigned int width, 01767 unsigned int height, 01768 stg_meters_t cellwidth, 01769 stg_meters_t cellheight ); 01770 01771 int subs; //< the number of subscriptions to this model 01772 int used; //< the number of connections to this model 01773 01774 void AddPoint( stg_meters_t x, stg_meters_t y ); 01775 void ClearPts(); 01776 01777 } rastervis; 01778 01779 bool rebuild_displaylist; 01780 std::string say_string; 01781 01782 stg_bool_t stall; 01783 int subs; 01784 01786 bool thread_safe; 01787 01789 class TrailItem 01790 { 01791 public: 01792 stg_usec_t time; 01793 Pose pose; 01794 Color color; 01795 01796 TrailItem() 01797 : time(0), pose(), color(){} 01798 01799 //TrailItem( stg_usec_t time, Pose pose, Color color ) 01800 //: time(time), pose(pose), color(color){} 01801 }; 01802 01804 std::vector<TrailItem> trail; 01805 01807 unsigned int trail_index; 01808 01812 static unsigned int trail_length; 01813 01815 static uint64_t trail_interval; 01816 01818 void UpdateTrail(); 01819 01820 //stg_model_type_t type; 01821 const std::string type; 01824 unsigned int event_queue_num; 01825 bool used; 01826 Velocity velocity; 01827 01831 bool velocity_enable; 01832 01833 stg_watts_t watts; 01834 01837 stg_watts_t watts_give; 01838 01841 stg_watts_t watts_take; 01842 01843 Worldfile* wf; 01844 int wf_entity; 01845 World* world; // pointer to the world in which this model exists 01846 WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode 01847 01848 public: 01849 01850 const std::string& GetModelType() const {return type;} 01851 std::string GetSayString(){return std::string(say_string);} 01852 01855 Model* GetChild( const std::string& name ) const; 01856 01857 class Visibility 01858 { 01859 public: 01860 bool blob_return; 01861 int fiducial_key; 01862 int fiducial_return; 01863 bool gripper_return; 01864 stg_laser_return_t laser_return; 01865 bool obstacle_return; 01866 bool ranger_return; 01867 bool gravity_return; 01868 bool sticky_return; 01869 01870 Visibility(); 01871 void Load( Worldfile* wf, int wf_entity ); 01872 } vis; 01873 01874 stg_usec_t GetUpdateInterval(){ return interval; } 01875 stg_usec_t GetEnergyInterval(){ return interval_energy; } 01876 stg_usec_t GetPoseInterval(){ return interval_pose; } 01877 01880 void Rasterize( uint8_t* data, 01881 unsigned int width, unsigned int height, 01882 stg_meters_t cellwidth, stg_meters_t cellheight ); 01883 01884 private: 01887 explicit Model(const Model& original); 01888 01891 Model& operator=(const Model& original); 01892 01893 protected: 01894 01896 void RegisterOption( Option* opt ); 01897 01898 void AppendTouchingModels( ModelPtrSet& touchers ); 01899 01903 Model* TestCollision(); 01904 01907 Model* TestCollisionTree(); 01908 01909 void CommitTestedPose(); 01910 01911 void Map(); 01912 void UnMap(); 01913 01914 void MapWithChildren(); 01915 void UnMapWithChildren(); 01916 01917 // Find the root model, and map/unmap the whole tree. 01918 void MapFromRoot(); 01919 void UnMapFromRoot(); 01920 01923 stg_raytrace_result_t Raytrace( const Pose &pose, 01924 const stg_meters_t range, 01925 const stg_ray_test_func_t func, 01926 const void* arg, 01927 const bool ztest = true ); 01928 01931 void Raytrace( const Pose &pose, 01932 const stg_meters_t range, 01933 const stg_radians_t fov, 01934 const stg_ray_test_func_t func, 01935 const void* arg, 01936 stg_raytrace_result_t* samples, 01937 const uint32_t sample_count, 01938 const bool ztest = true ); 01939 01940 stg_raytrace_result_t Raytrace( const stg_radians_t bearing, 01941 const stg_meters_t range, 01942 const stg_ray_test_func_t func, 01943 const void* arg, 01944 const bool ztest = true ); 01945 01946 void Raytrace( const stg_radians_t bearing, 01947 const stg_meters_t range, 01948 const stg_radians_t fov, 01949 const stg_ray_test_func_t func, 01950 const void* arg, 01951 stg_raytrace_result_t* samples, 01952 const uint32_t sample_count, 01953 const bool ztest = true ); 01954 01955 01959 void GPoseDirtyTree(); 01960 01961 virtual void Startup(); 01962 virtual void Shutdown(); 01963 virtual void Update(); 01964 virtual void UpdatePose(); 01965 virtual void UpdateCharge(); 01966 01967 Model* ConditionalMove( const Pose& newpose ); 01968 01969 stg_meters_t ModelHeight() const; 01970 01971 void DrawBlocksTree(); 01972 virtual void DrawBlocks(); 01973 void DrawBoundingBox(); 01974 void DrawBoundingBoxTree(); 01975 virtual void DrawStatus( Camera* cam ); 01976 void DrawStatusTree( Camera* cam ); 01977 01978 void DrawOriginTree(); 01979 void DrawOrigin(); 01980 01981 void PushLocalCoords(); 01982 void PopCoords(); 01983 01985 void DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width=1.0, double height=1.0 ); 01986 01987 virtual void DrawPicker(); 01988 virtual void DataVisualize( Camera* cam ); 01989 virtual void DrawSelected(void); 01990 01991 void DrawTrailFootprint(); 01992 void DrawTrailBlocks(); 01993 void DrawTrailArrows(); 01994 void DrawGrid(); 01995 // void DrawBlinkenlights(); 01996 void DataVisualizeTree( Camera* cam ); 01997 void DrawFlagList(); 01998 void DrawPose( Pose pose ); 01999 02000 public: 02001 virtual void PushColor( Color col ){ world->PushColor( col ); } 02002 virtual void PushColor( double r, double g, double b, double a ){ world->PushColor( r,g,b,a ); } 02003 virtual void PopColor() { world->PopColor(); } 02004 02005 PowerPack* FindPowerPack() const; 02006 02007 //void RecordRenderPoint( GSList** head, GSList* link, 02008 // unsigned int* c1, unsigned int* c2 ); 02009 02010 void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, 02011 stg_meters_t ymin, stg_meters_t ymax ); 02012 02014 std::string PoseString() 02015 { return pose.String(); } 02016 02018 static Model* LookupId( uint32_t id ) 02019 { return modelsbyid[id]; } 02020 02022 Model( World* world, 02023 Model* parent = NULL, 02024 const std::string& type = "model" ); 02025 02027 virtual ~Model(); 02028 02029 void Say( const std::string& str ); 02030 02032 void AddVisualizer( Visualizer* custom_visual, bool on_by_default ); 02033 02035 void RemoveVisualizer( Visualizer* custom_visual ); 02036 02037 void BecomeParentOf( Model* child ); 02038 02039 void Load( Worldfile* wf, int wf_entity ) 02040 { 02043 SetWorldfile( wf, wf_entity ); 02044 Load(); // call virtual load 02045 } 02046 02048 void SetWorldfile( Worldfile* wf, int wf_entity ) 02049 { this->wf = wf; this->wf_entity = wf_entity; } 02050 02052 virtual void Load(); 02053 02055 virtual void Save(); 02056 02058 void InitControllers(); 02059 02060 void AddFlag( Flag* flag ); 02061 void RemoveFlag( Flag* flag ); 02062 02063 void PushFlag( Flag* flag ); 02064 Flag* PopFlag(); 02065 02066 int GetFlagCount() const { return flag_list.size(); } 02067 02072 void Disable(){ disabled = true; }; 02073 02076 void Enable(){ disabled = false; }; 02077 02080 void LoadControllerModule( const char* lib ); 02081 02084 void NeedRedraw(); 02085 02088 void LoadBlock( Worldfile* wf, int entity ); 02089 02092 Block* AddBlockRect( stg_meters_t x, stg_meters_t y, 02093 stg_meters_t dx, stg_meters_t dy, 02094 stg_meters_t dz ); 02095 02097 void ClearBlocks(); 02098 02101 Model* Parent() const { return this->parent; } 02102 02104 World* GetWorld() const { return this->world; } 02105 02107 Model* Root(){ return( parent ? parent->Root() : this ); } 02108 02109 bool IsAntecedent( const Model* testmod ) const; 02110 02112 bool IsDescendent( const Model* testmod ) const; 02113 02115 bool IsRelated( const Model* testmod ) const; 02116 02118 Pose GetGlobalPose() const; 02119 02121 Velocity GetGlobalVelocity() const; 02122 02123 /* set the velocity of a model in the global coordinate system */ 02124 void SetGlobalVelocity( const Velocity& gvel ); 02125 02127 void Subscribe(); 02128 02130 void Unsubscribe(); 02131 02133 void SetGlobalPose( const Pose& gpose ); 02134 02136 void SetVelocity( const Velocity& vel ); 02137 02139 void VelocityEnable(); 02140 02142 void VelocityDisable(); 02143 02145 void SetPose( const Pose& pose ); 02146 02148 void AddToPose( const Pose& pose ); 02149 02151 void AddToPose( double dx, double dy, double dz, double da ); 02152 02154 void SetGeom( const Geom& src ); 02155 02158 void SetFiducialReturn( int fid ); 02159 02161 int GetFiducialReturn() const { return vis.fiducial_return; } 02162 02165 void SetFiducialKey( int key ); 02166 02167 Color GetColor() const { return color; } 02168 02170 uint32_t GetId() const { return id; } 02171 02173 stg_kg_t GetTotalMass(); 02174 02176 stg_kg_t GetMassOfChildren(); 02177 02179 int SetParent( Model* newparent); 02180 02183 Geom GetGeom() const { return geom; } 02184 02187 Pose GetPose() const { return pose; } 02188 02191 Velocity GetVelocity() const { return velocity; } 02192 02193 // guess what these do? 02194 void SetColor( Color col ); 02195 void SetMass( stg_kg_t mass ); 02196 void SetStall( stg_bool_t stall ); 02197 void SetGravityReturn( int val ); 02198 void SetGripperReturn( int val ); 02199 void SetStickyReturn( int val ); 02200 void SetLaserReturn( stg_laser_return_t val ); 02201 void SetObstacleReturn( int val ); 02202 void SetBlobReturn( int val ); 02203 void SetRangerReturn( int val ); 02204 void SetBoundary( int val ); 02205 void SetGuiNose( int val ); 02206 void SetGuiMove( int val ); 02207 void SetGuiGrid( int val ); 02208 void SetGuiOutline( int val ); 02209 void SetWatts( stg_watts_t watts ); 02210 void SetMapResolution( stg_meters_t res ); 02211 void SetFriction( double friction ); 02212 02213 bool DataIsFresh() const { return this->data_fresh; } 02214 02215 /* attach callback functions to data members. The function gets 02216 called when the member is changed using SetX() accessor method */ 02217 02218 void AddCallback( void* address, 02219 stg_model_callback_t cb, 02220 void* user ); 02221 02222 int RemoveCallback( void* member, 02223 stg_model_callback_t callback ); 02224 02225 int CallCallbacks( void* address ); 02226 02227 /* wrappers for the generic callback add & remove functions that hide 02228 some implementation detail */ 02229 02230 void AddStartupCallback( stg_model_callback_t cb, void* user ) 02231 { AddCallback( &hooks.startup, cb, user ); }; 02232 02233 void RemoveStartupCallback( stg_model_callback_t cb ) 02234 { RemoveCallback( &hooks.startup, cb ); }; 02235 02236 void AddShutdownCallback( stg_model_callback_t cb, void* user ) 02237 { AddCallback( &hooks.shutdown, cb, user ); }; 02238 02239 void RemoveShutdownCallback( stg_model_callback_t cb ) 02240 { RemoveCallback( &hooks.shutdown, cb ); } 02241 02242 void AddLoadCallback( stg_model_callback_t cb, void* user ) 02243 { AddCallback( &hooks.load, cb, user ); } 02244 02245 void RemoveLoadCallback( stg_model_callback_t cb ) 02246 { RemoveCallback( &hooks.load, cb ); } 02247 02248 void AddSaveCallback( stg_model_callback_t cb, void* user ) 02249 { AddCallback( &hooks.save, cb, user ); } 02250 02251 void RemoveSaveCallback( stg_model_callback_t cb ) 02252 { RemoveCallback( &hooks.save, cb ); } 02253 02254 void AddUpdateCallback( stg_model_callback_t cb, void* user ) 02255 { AddCallback( &hooks.update, cb, user ); } 02256 02257 void RemoveUpdateCallback( stg_model_callback_t cb ) 02258 { RemoveCallback( &hooks.update, cb ); } 02259 02260 void AddFlagIncrCallback( stg_model_callback_t cb, void* user ) 02261 { AddCallback( &hooks.flag_incr, cb, user ); } 02262 02263 void RemoveFlagIncrCallback( stg_model_callback_t cb ) 02264 { RemoveCallback( &hooks.flag_incr, cb ); } 02265 02266 void AddFlagDecrCallback( stg_model_callback_t cb, void* user ) 02267 { AddCallback( &hooks.flag_decr, cb, user ); } 02268 02269 void RemoveFlagDecrCallback( stg_model_callback_t cb ) 02270 { RemoveCallback( &hooks.flag_decr, cb ); } 02271 02272 02273 virtual void Print( char* prefix ) const; 02274 virtual const char* PrintWithPose() const; 02275 02278 Pose GlobalToLocal( const Pose& pose ) const; 02279 02282 Pose LocalToGlobal( const Pose& pose ) const 02283 { 02284 return( ( GetGlobalPose() + geom.pose ) + pose ); 02285 } 02286 02288 void LocalToPixels( const std::vector<stg_point_t>& local, 02289 std::vector<stg_point_int_t>& pixels) const; 02290 02293 stg_point_t LocalToGlobal( const stg_point_t& pt) const; 02294 02297 Model* GetUnsubscribedModelOfType( const std::string& type ) const; 02298 02301 Model* GetUnusedModelOfType( const std::string& type ); 02302 02305 bool Stalled() const { return this->stall; } 02306 02309 unsigned int GetSubscriptionCount() const { return subs; } 02310 02312 bool HasSubscribers() const { return( subs > 0 ); } 02313 02314 static std::map< std::string, creator_t> name_map; 02315 }; 02316 02317 02318 // BLOBFINDER MODEL -------------------------------------------------------- 02319 02320 02322 class ModelBlobfinder : public Model 02323 { 02324 public: 02326 class Blob 02327 { 02328 public: 02329 Color color; 02330 uint32_t left, top, right, bottom; 02331 stg_meters_t range; 02332 }; 02333 02334 class Vis : public Visualizer 02335 { 02336 private: 02337 //static Option showArea; 02338 public: 02339 Vis( World* world ); 02340 virtual ~Vis( void ){} 02341 virtual void Visualize( Model* mod, Camera* cam ); 02342 } vis; 02343 02344 private: 02345 std::vector<Blob> blobs; 02346 std::vector<Color> colors; 02347 02348 // predicate for ray tracing 02349 static bool BlockMatcher( Block* testblock, Model* finder ); 02350 02351 public: 02352 stg_radians_t fov; 02353 stg_radians_t pan; 02354 stg_meters_t range; 02355 unsigned int scan_height; 02356 unsigned int scan_width; 02357 02358 // constructor 02359 ModelBlobfinder( World* world, 02360 Model* parent, 02361 const std::string& type ); 02362 // destructor 02363 ~ModelBlobfinder(); 02364 02365 virtual void Startup(); 02366 virtual void Shutdown(); 02367 virtual void Update(); 02368 virtual void Load(); 02369 02370 Blob* GetBlobs( unsigned int* count ) 02371 { 02372 if( count ) *count = blobs.size(); 02373 return &blobs[0]; 02374 } 02375 02377 void AddColor( Color col ); 02378 02380 void RemoveColor( Color col ); 02381 02384 void RemoveAllColors(); 02385 }; 02386 02387 02388 02389 02390 // LASER MODEL -------------------------------------------------------- 02391 02393 class ModelLaser : public Model 02394 { 02395 public: 02397 class Sample 02398 { 02399 public: 02400 stg_meters_t range; 02401 double reflectance; 02402 }; 02403 02405 class Config 02406 { 02407 public: 02408 uint32_t sample_count; 02409 uint32_t resolution; 02410 Bounds range_bounds; 02411 stg_radians_t fov; 02412 stg_usec_t interval; 02413 }; 02414 02415 private: 02416 class Vis : public Visualizer 02417 { 02418 private: 02419 static Option showArea; 02420 static Option showStrikes; 02421 static Option showFov; 02422 static Option showBeams; 02423 02424 public: 02425 Vis( World* world ); virtual ~Vis( void ){} 02426 virtual void Visualize( Model* mod, Camera* cam ); 02427 } vis; 02428 02429 unsigned int sample_count; 02430 std::vector<Sample> samples; 02431 02432 stg_meters_t range_max; 02433 stg_radians_t fov; 02434 uint32_t resolution; 02435 02436 // set up data buffers after the config changes 02437 void SampleConfig(); 02438 02439 public: 02440 // constructor 02441 ModelLaser( World* world, 02442 Model* parent, 02443 const std::string& type ); 02444 02445 // destructor 02446 ~ModelLaser(); 02447 02448 virtual void Startup(); 02449 virtual void Shutdown(); 02450 virtual void Update(); 02451 virtual void Load(); 02452 virtual void Print( char* prefix ); 02453 02455 Sample* GetSamples( uint32_t* count ); 02456 02458 const std::vector<Sample>& GetSamples(); 02459 02461 Config GetConfig( ); 02462 02464 void SetConfig( Config& cfg ); 02465 }; 02466 02467 02468 // Light indicator model 02469 class ModelLightIndicator : public Model 02470 { 02471 public: 02472 ModelLightIndicator( World* world, 02473 Model* parent, 02474 const std::string& type ); 02475 ~ModelLightIndicator(); 02476 02477 void SetState(bool isOn); 02478 02479 protected: 02480 virtual void DrawBlocks(); 02481 02482 private: 02483 bool m_IsOn; 02484 }; 02485 02486 // \todo GRIPPER MODEL -------------------------------------------------------- 02487 02488 02489 class ModelGripper : public Model 02490 { 02491 public: 02492 02493 enum paddle_state_t { 02494 PADDLE_OPEN = 0, // default state 02495 PADDLE_CLOSED, 02496 PADDLE_OPENING, 02497 PADDLE_CLOSING, 02498 }; 02499 02500 enum lift_state_t { 02501 LIFT_DOWN = 0, // default state 02502 LIFT_UP, 02503 LIFT_UPPING, // verbed these to match the paddle state 02504 LIFT_DOWNING, 02505 }; 02506 02507 enum cmd_t { 02508 CMD_NOOP = 0, // default state 02509 CMD_OPEN, 02510 CMD_CLOSE, 02511 CMD_UP, 02512 CMD_DOWN 02513 }; 02514 02515 02518 struct config_t 02519 { 02520 Size paddle_size; 02521 paddle_state_t paddles; 02522 lift_state_t lift; 02523 double paddle_position; 02524 double lift_position; 02525 Model* gripped; 02526 bool paddles_stalled; // true iff some solid object stopped the paddles closing or opening 02527 double close_limit; 02528 bool autosnatch; 02529 double break_beam_inset[2]; 02530 Model* beam[2]; 02531 Model* contact[2]; 02532 }; 02533 02534 private: 02535 virtual void Update(); 02536 virtual void DataVisualize( Camera* cam ); 02537 02538 void FixBlocks(); 02539 void PositionPaddles(); 02540 void UpdateBreakBeams(); 02541 void UpdateContacts(); 02542 02543 config_t cfg; 02544 cmd_t cmd; 02545 02546 Block* paddle_left; 02547 Block* paddle_right; 02548 02549 static Option showData; 02550 02551 public: 02552 static const Size size; 02553 02554 // constructor 02555 ModelGripper( World* world, 02556 Model* parent, 02557 const std::string& type ); 02558 // destructor 02559 virtual ~ModelGripper(); 02560 02561 virtual void Load(); 02562 virtual void Save(); 02563 02565 void SetConfig( config_t & newcfg ){ this->cfg = newcfg; FixBlocks(); } 02566 02568 config_t GetConfig(){ return cfg; }; 02569 02571 void SetCommand( cmd_t cmd ) { this->cmd = cmd; } 02573 void CommandClose() { SetCommand( CMD_CLOSE ); } 02575 void CommandOpen() { SetCommand( CMD_OPEN ); } 02577 void CommandUp() { SetCommand( CMD_UP ); } 02579 void CommandDown() { SetCommand( CMD_DOWN ); } 02580 }; 02581 02582 02583 // \todo BUMPER MODEL -------------------------------------------------------- 02584 02585 // typedef struct 02586 // { 02587 // Pose pose; 02588 // stg_meters_t length; 02589 // } stg_bumper_config_t; 02590 02591 // typedef struct 02592 // { 02593 // Model* hit; 02594 // stg_point_t hit_point; 02595 // } stg_bumper_sample_t; 02596 02597 02598 // FIDUCIAL MODEL -------------------------------------------------------- 02599 02601 class ModelFiducial : public Model 02602 { 02603 public: 02605 class Fiducial 02606 { 02607 public: 02608 stg_meters_t range; 02609 stg_radians_t bearing; 02610 Pose geom; 02611 Pose pose; 02612 Model* mod; 02613 int id; 02614 }; 02615 02616 private: 02617 // if neighbor is visible, add him to the fiducial scan 02618 void AddModelIfVisible( Model* him ); 02619 02620 virtual void Update(); 02621 virtual void DataVisualize( Camera* cam ); 02622 02623 static Option showData; 02624 static Option showFov; 02625 02626 std::vector<Fiducial> fiducials; 02627 02628 public: 02629 ModelFiducial( World* world, 02630 Model* parent, 02631 const std::string& type ); 02632 virtual ~ModelFiducial(); 02633 02634 virtual void Load(); 02635 void Shutdown( void ); 02636 02637 stg_meters_t max_range_anon; 02638 stg_meters_t max_range_id; 02639 stg_meters_t min_range; 02640 stg_radians_t fov; 02641 stg_radians_t heading; 02642 int key; 02643 02644 02646 std::vector<Fiducial>& GetFiducials() { return fiducials; } 02647 02649 Fiducial* GetFiducials( unsigned int* count ) 02650 { 02651 if( count ) *count = fiducials.size(); 02652 return &fiducials[0]; 02653 } 02654 }; 02655 02656 02657 // RANGER MODEL -------------------------------------------------------- 02658 02660 class ModelRanger : public Model 02661 { 02662 public: 02663 class Sensor 02664 { 02665 public: 02666 Pose pose; 02667 Size size; 02668 Bounds bounds_range; 02669 stg_radians_t fov; 02670 int ray_count; 02671 stg_meters_t range; 02672 }; 02673 02674 protected: 02675 02676 virtual void Startup(); 02677 virtual void Shutdown(); 02678 virtual void Update(); 02679 virtual void DataVisualize( Camera* cam ); 02680 02681 public: 02682 ModelRanger( World* world, Model* parent, 02683 const std::string& type ); 02684 virtual ~ModelRanger(); 02685 02686 virtual void Load(); 02687 virtual void Print( char* prefix ); 02688 02689 std::vector<Sensor> sensors; 02690 02691 private: 02692 static Option showRangerData; 02693 static Option showRangerTransducers; 02694 }; 02695 02696 // BLINKENLIGHT MODEL ---------------------------------------------------- 02697 class ModelBlinkenlight : public Model 02698 { 02699 private: 02700 double dutycycle; 02701 bool enabled; 02702 stg_msec_t period; 02703 bool on; 02704 02705 static Option showBlinkenData; 02706 public: 02707 ModelBlinkenlight( World* world, 02708 Model* parent, 02709 const std::string& type ); 02710 02711 ~ModelBlinkenlight(); 02712 02713 virtual void Load(); 02714 virtual void Update(); 02715 virtual void DataVisualize( Camera* cam ); 02716 }; 02717 02718 02719 // CAMERA MODEL ---------------------------------------------------- 02720 02722 class ModelCamera : public Model 02723 { 02724 public: 02725 typedef struct 02726 { 02727 // GL_V3F 02728 GLfloat x, y, z; 02729 } ColoredVertex; 02730 02731 private: 02732 Canvas* _canvas; 02733 02734 GLfloat* _frame_data; //opengl read buffer 02735 GLubyte* _frame_color_data; //opengl read buffer 02736 02737 bool _valid_vertexbuf_cache; 02738 ColoredVertex* _vertexbuf_cache; //cached unit vectors with appropriate rotations (these must be scalled by z-buffer length) 02739 02740 int _width; //width of buffer 02741 int _height; //height of buffer 02742 static const int _depth = 4; 02743 02744 int _camera_quads_size; 02745 GLfloat* _camera_quads; 02746 GLubyte* _camera_colors; 02747 02748 static Option showCameraData; 02749 02750 PerspectiveCamera _camera; 02751 float _yaw_offset; //position camera is mounted at 02752 float _pitch_offset; 02753 02755 bool GetFrame(); 02756 02757 public: 02758 ModelCamera( World* world, 02759 Model* parent, 02760 const std::string& type ); 02761 02762 ~ModelCamera(); 02763 02764 virtual void Load(); 02765 02767 virtual void Update(); 02768 02770 //virtual void Draw( uint32_t flags, Canvas* canvas ); 02771 02773 virtual void DataVisualize( Camera* cam ); 02774 02776 int getWidth( void ) const { return _width; } 02777 02779 int getHeight( void ) const { return _height; } 02780 02782 const PerspectiveCamera& getCamera( void ) const { return _camera; } 02783 02785 const GLfloat* FrameDepth() const { return _frame_data; } 02786 02788 const GLubyte* FrameColor() const { return _frame_color_data; } 02789 02791 void setPitch( float pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; } 02792 02794 void setYaw( float yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; } 02795 }; 02796 02797 // POSITION MODEL -------------------------------------------------------- 02798 02800 class ModelPosition : public Model 02801 { 02802 friend class Canvas; 02803 02804 public: 02806 typedef enum 02807 { CONTROL_VELOCITY, 02808 CONTROL_POSITION 02809 } ControlMode; 02810 02812 typedef enum 02813 { LOCALIZATION_GPS, 02814 LOCALIZATION_ODOM 02815 } LocalizationMode; 02816 02818 typedef enum 02819 { DRIVE_DIFFERENTIAL, 02820 DRIVE_OMNI, 02821 DRIVE_CAR 02822 } DriveMode; 02823 02824 private: 02825 Pose goal; 02826 ControlMode control_mode; 02827 DriveMode drive_mode; 02828 LocalizationMode localization_mode; 02829 Velocity integration_error; 02830 02831 02832 public: 02833 // constructor 02834 ModelPosition( World* world, 02835 Model* parent, 02836 const std::string& type ); 02837 // destructor 02838 ~ModelPosition(); 02839 02840 virtual void Startup(); 02841 virtual void Shutdown(); 02842 virtual void Update(); 02843 virtual void Load(); 02844 02847 class Waypoint 02848 { 02849 public: 02850 Waypoint( stg_meters_t x, stg_meters_t y, stg_meters_t z, stg_radians_t a, Color color ) ; 02851 Waypoint( const Pose& pose, Color color ) ; 02852 Waypoint(); 02853 void Draw() const; 02854 02855 Pose pose; 02856 Color color; 02857 }; 02858 02859 std::vector<Waypoint> waypoints; 02860 02861 class WaypointVis : public Visualizer 02862 { 02863 public: 02864 WaypointVis(); 02865 virtual ~WaypointVis( void ){} 02866 virtual void Visualize( Model* mod, Camera* cam ); 02867 } wpvis; 02868 02869 class PoseVis : public Visualizer 02870 { 02871 public: 02872 PoseVis(); 02873 virtual ~PoseVis( void ){} 02874 virtual void Visualize( Model* mod, Camera* cam ); 02875 } posevis; 02876 02878 void SetOdom( Pose odom ); 02879 02882 void SetSpeed( double x, double y, double a ); 02883 void SetXSpeed( double x ); 02884 void SetYSpeed( double y ); 02885 void SetZSpeed( double z ); 02886 void SetTurnSpeed( double a ); 02887 void SetSpeed( Velocity vel ); 02889 void Stop(); 02890 02893 void GoTo( double x, double y, double a ); 02894 void GoTo( Pose pose ); 02895 02896 // localization state 02897 Pose est_pose; 02898 Pose est_pose_error; 02899 Pose est_origin; 02900 }; 02901 02902 02903 // ACTUATOR MODEL -------------------------------------------------------- 02904 02906 class ModelActuator : public Model 02907 { 02908 public: 02910 typedef enum 02911 { CONTROL_VELOCITY, 02912 CONTROL_POSITION 02913 } ControlMode; 02914 02916 typedef enum 02917 { TYPE_LINEAR, 02918 TYPE_ROTATIONAL 02919 } ActuatorType; 02920 02921 private: 02922 double goal; //< the current velocity or pose to reach, depending on the value of control_mode 02923 double pos; 02924 double max_speed; 02925 double min_position; 02926 double max_position; 02927 ControlMode control_mode; 02928 ActuatorType actuator_type; 02929 stg_point3_t axis; 02930 02931 Pose InitialPose; 02932 public: 02933 // constructor 02934 ModelActuator( World* world, 02935 Model* parent, 02936 const std::string& type ); 02937 // destructor 02938 ~ModelActuator(); 02939 02940 virtual void Startup(); 02941 virtual void Shutdown(); 02942 virtual void Update(); 02943 virtual void Load(); 02944 02947 void SetSpeed( double speed ); 02948 02949 double GetSpeed() const {return goal;} 02950 02953 void GoTo( double pose ); 02954 02955 double GetPosition() const {return pos;}; 02956 double GetMaxPosition() const {return max_position;}; 02957 double GetMinPosition() const {return min_position;}; 02958 02959 }; 02960 02961 02962 }; // end namespace stg 02963 02964 #endif