42 #include <sys/types.h> 58 #include <FL/Fl_Box.H> 59 #include <FL/Fl_Gl_Window.H> 60 #include <FL/Fl_Menu_Bar.H> 61 #include <FL/Fl_Window.H> 62 #include <FL/fl_draw.H> 66 #include <OpenGL/glu.h> 102 "Copyright Richard Vaughan and contributors 2000-2009";
106 "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, Toby Collett, Jeremy Asher, Alex Couture-Beil and contributors.";
109 const char WEBSITE[] =
"http://playerstage.org";
113 "Robot simulation library\nPart of the Player Project";
117 "Stage robot simulation library\n" \
118 "Copyright (C) 2000-2009 Richard Vaughan and contributors\n" \
119 "Part of the Player Project [http://playerstage.org]\n" \
121 "This program is free software; you can redistribute it and/or\n" \
122 "modify it under the terms of the GNU General Public License\n" \
123 "as published by the Free Software Foundation; either version 2\n" \
124 "of the License, or (at your option) any later version.\n" \
126 "This program is distributed in the hope that it will be useful,\n" \
127 "but WITHOUT ANY WARRANTY; without even the implied warranty of\n" \
128 "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n" \
129 "GNU General Public License for more details.\n" \
131 "You should have received a copy of the GNU General Public License\n" \
132 "along with this program; if not, write to the Free Software\n" \
133 "Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.\n" \
135 "The text of the license may also be available online at\n" \
136 "http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";
148 inline double rtod(
double r ){
return( r*180.0/M_PI ); }
151 inline double dtor(
double d ){
return( d*M_PI/180.0 ); }
156 while( a < -M_PI ) a += 2.0*M_PI;
157 while( a > M_PI ) a -= 2.0*M_PI;
162 inline int sgn(
int a){
return( a<0 ? -1 : 1); }
165 inline double sgn(
double a){
return( a<0 ? -1.0 : 1.0); }
202 Color(
double r,
double g,
double b,
double a=1.0 );
207 Color(
const std::string& name );
214 void Print(
const char* prefix )
const;
221 void GLSet(
void ) { glColor4f( r,g,b,a ); }
237 Size() : x( 0.4 ), y( 0.4 ), z( 1.0 )
241 void Save(
Worldfile* wf,
int section,
const char* keyword )
const;
258 : x(x), y(y), z(z), a(a)
261 Pose() : x(0.0), y(0.0), z(0.0), a(0.0)
269 meters_t ymin, meters_t ymax )
271 return Pose( xmin + drand48() * (xmax-xmin),
272 ymin + drand48() * (ymax-ymin),
280 virtual void Print(
const char* prefix )
const 282 printf(
"%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
289 snprintf( buf, 256,
"[ %.3f %.3f %.3f %.3f ]",
291 return std::string(buf);
296 {
return( !(x || y || z || a )); };
303 void Save(
Worldfile* wf,
int section,
const char* keyword );
307 const double cosa = cos(a);
308 const double sina = sin(a);
310 return Pose( x + p.
x * cosa - p.
y * sina,
311 y + p.
x * sina + p.
y * cosa,
319 return( hypot( y, x ) < hypot( other.
y, other.
x ));
324 return( x==other.
x &&
332 return( x!=other.
x ||
340 return hypot( x-other.
x, y-other.
y );
378 virtual void Print(
const char* prefix )
const 381 printf(
"%s", prefix );
383 printf(
"velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
402 void Print(
const char* prefix )
const 405 printf(
"%s", prefix );
407 printf(
"geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n",
438 Bounds(
double min,
double max ) : min(min), max(max) { }
443 double Constrain(
double value );
459 : x(x), y(y), z(z) {}
474 point_t( meters_t x, meters_t y ) : x(x), y(y){}
478 {
return ((x += other.
x) && (y += other.
y) ); }
487 : x(x), y(y), z(z) {}
503 if( x < other.
x )
return true;
504 if( other.
x < x )
return false;
509 {
return ((x == other.
x) && (y == other.
y) ); }
522 void coord_shift(
double x,
double y,
double z,
double a );
525 void draw_string(
float x,
float y,
float z,
const char *
string);
527 const char *
string, Fl_Align align );
530 void draw_octagon(
float x,
float y,
float w,
float h,
float m );
533 void draw_array(
float x,
float y,
float w,
float h,
534 float* data,
size_t len,
size_t offset,
535 float min,
float max );
536 void draw_array(
float x,
float y,
float w,
float h,
537 float* data,
size_t len,
size_t offset );
548 const std::string menu_name;
549 const std::string worldfile_name;
553 const std::string& worldfile_name )
554 : menu_name( menu_name ),
555 worldfile_name( worldfile_name )
559 virtual void Visualize( Model* mod,
Camera* cam ) = 0;
573 double constrain(
double val,
double minval,
double maxval );
598 std::vector<rotrect_t>& rects );
609 #define VAR(V,init) __typeof(init) V=(init) 616 #define FOR_EACH(I,C) for(VAR(I,(C).begin()),ite=(C).end();(I)!=ite;++(I)) 620 template <
class T,
class C>
622 { cont.erase( std::remove( cont.begin(), cont.end(), thing ), cont.end() ); }
625 #define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: " m " (%s %s)\n", __FILE__, __FUNCTION__) 626 #define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, __FILE__, __FUNCTION__) 627 #define PRINT_ERR2(m,a,b) fprintf( stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, b, __FILE__, __FUNCTION__) 628 #define PRINT_ERR3(m,a,b,c) fprintf( stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, b, c, __FILE__, __FUNCTION__) 629 #define PRINT_ERR4(m,a,b,c,d) fprintf( stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__) 630 #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__) 633 #define PRINT_WARN(m) printf( "\033[44mwarn\033[0m: " m " (%s %s)\n", __FILE__, __FUNCTION__) 634 #define PRINT_WARN1(m,a) printf( "\033[44mwarn\033[0m: " m " (%s %s)\n", a, __FILE__, __FUNCTION__) 635 #define PRINT_WARN2(m,a,b) printf( "\033[44mwarn\033[0m: " m " (%s %s)\n", a, b, __FILE__, __FUNCTION__) 636 #define PRINT_WARN3(m,a,b,c) printf( "\033[44mwarn\033[0m: " m " (%s %s)\n", a, b, c, __FILE__, __FUNCTION__) 637 #define PRINT_WARN4(m,a,b,c,d) printf( "\033[44mwarn\033[0m: " m " (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__) 638 #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__) 642 #define PRINT_MSG(m) printf( "Stage: " m " (%s %s)\n", __FILE__, __FUNCTION__) 643 #define PRINT_MSG1(m,a) printf( "Stage: " m " (%s %s)\n", a, __FILE__, __FUNCTION__) 644 #define PRINT_MSG2(m,a,b) printf( "Stage: " m " (%s %s)\n", a, b, __FILE__, __FUNCTION__) 645 #define PRINT_MSG3(m,a,b,c) printf( "Stage: " m " (%s %s)\n", a, b, c, __FILE__, __FUNCTION__) 646 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: " m " (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__) 647 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: " m " (%s %s)\n", a, b, c, d, e,__FILE__, __FUNCTION__) 649 #define PRINT_MSG(m) printf( "Stage: " m "\n" ) 650 #define PRINT_MSG1(m,a) printf( "Stage: " m "\n", a) 651 #define PRINT_MSG2(m,a,b) printf( "Stage: " m "\n,", a, b ) 652 #define PRINT_MSG3(m,a,b,c) printf( "Stage: " m "\n", a, b, c ) 653 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: " m "\n", a, b, c, d ) 654 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: " m "\n", a, b, c, d, e ) 659 #define PRINT_DEBUG(m) printf( "debug: " m " (%s %s)\n", __FILE__, __FUNCTION__) 660 #define PRINT_DEBUG1(m,a) printf( "debug: " m " (%s %s)\n", a, __FILE__, __FUNCTION__) 661 #define PRINT_DEBUG2(m,a,b) printf( "debug: " m " (%s %s)\n", a, b, __FILE__, __FUNCTION__) 662 #define PRINT_DEBUG3(m,a,b,c) printf( "debug: " m " (%s %s)\n", a, b, c, __FILE__, __FUNCTION__) 663 #define PRINT_DEBUG4(m,a,b,c,d) printf( "debug: " m " (%s %s)\n", a, b, c ,d, __FILE__, __FUNCTION__) 664 #define PRINT_DEBUG5(m,a,b,c,d,e) printf( "debug: " m " (%s %s)\n", a, b, c ,d, e, __FILE__, __FUNCTION__) 666 #define PRINT_DEBUG(m) 667 #define PRINT_DEBUG1(m,a) 668 #define PRINT_DEBUG2(m,a,b) 669 #define PRINT_DEBUG3(m,a,b,c) 670 #define PRINT_DEBUG4(m,a,b,c,d) 671 #define PRINT_DEBUG5(m,a,b,c,d,e) 711 virtual void AddChild( Model* mod );
712 virtual void RemoveChild( Model* mod );
713 virtual Pose GetGlobalPose()
const;
715 const char*
Token()
const {
return token.c_str(); }
716 const std::string&
TokenStr()
const {
return token; }
725 PRINT_ERR(
"Ancestor::SetToken() called with zero length string. Ignored." );
729 void SetProperty( std::string& key,
void* value ){ props[ key ] = value; }
734 std::map<std::string,void*>::iterator it = props.find( key );
735 return( it == props.end() ? NULL : it->second );
752 : pose(pose), range(range), mod(NULL), color() {}
758 Ray(
const Model* mod,
const Pose& origin,
const meters_t range,
const ray_test_func_t func,
const void* arg,
const bool ztest ) :
759 mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest)
762 Ray() : mod(NULL), origin(0,0,0,0), range(0), func(NULL), arg(NULL), ztest(true)
787 LogEntry( usec_t timestamp, Model* mod );
790 static std::vector<LogEntry>
log;
793 static size_t Count(){
return log.size(); }
808 CtrlArgs( std::string w, std::string c ) : worldfile(w), cmdline(c) {}
821 friend class WorkerThread;
826 static std::vector<std::string>
args;
831 static std::set<World*> world_set;
832 static bool quit_all;
833 static void UpdateCb( World* world);
834 static unsigned int next_id;
840 std::set<Model*> models;
843 std::map<std::string, Model*> models_by_name;
846 std::map<int,Model*> models_by_wfentity;
850 std::vector<Model*> models_with_fiducials;
854 bool operator()(
const Model*
a,
const Model*
b)
const;
859 bool operator()(
const Model*
a,
const Model*
b)
const;
864 std::set<Model*,ltx> models_with_fiducials_byx;
868 std::set<Model*,lty> models_with_fiducials_byy;
871 void FiducialInsert( Model* mod )
873 FiducialErase( mod );
874 models_with_fiducials.push_back( mod );
878 void FiducialErase( Model* mod )
880 EraseAll( mod, models_with_fiducials );
887 unsigned int show_clock_interval;
889 pthread_mutex_t sync_mutex;
890 unsigned int threads_working;
891 pthread_cond_t threads_start_cond;
892 pthread_cond_t threads_done_cond;
894 unsigned int worker_threads;
898 std::list<std::pair<world_callback_t,void*> >
cb_list;
915 void CallUpdateCallbacks();
923 virtual void Start(){ paused =
false; };
924 virtual void Stop(){ paused =
true; };
927 bool Paused()
const {
return( paused ); };
932 virtual void Redraw(
void ){ };
937 static const int DEFAULT_PPM = 50;
948 void Log( Model* mod );
958 virtual std::string ClockString(
void )
const;
960 Model* CreateModel( Model* parent,
const std::string& typestr );
961 void LoadModel(
Worldfile* wf,
int entity );
962 void LoadBlock(
Worldfile* wf,
int entity );
963 void LoadBlockGroup(
Worldfile* wf,
int entity );
965 void LoadSensor(
Worldfile* wf,
int entity );
970 void MapPoly(
const std::vector<point_int_t>& poly,
972 unsigned int layer );
982 {
return (int32_t)floor(x * ppm); };
985 {
return point_int_t( MetersToPixels(pt.
x), MetersToPixels(pt.
y)); };
991 { (void)r; (void)g; (void)b; (void)a; };
1002 const meters_t range,
1004 const Model* finder,
1008 void Raytrace(
const Pose &pose,
1009 const meters_t range,
1010 const radians_t fov,
1012 const Model* finder,
1015 const uint32_t sample_count,
1022 virtual void AddModel( Model* mod );
1023 virtual void RemoveModel( Model* mod );
1025 void AddModelName( Model* mod,
const std::string& name );
1033 void RecordRay(
double x1,
double y1,
double x2,
double y2 );
1037 bool PastQuitTime();
1039 static void* update_thread_entry( std::pair<World*,int>* info );
1046 : time(time), mod(mod), cb(cb), arg(arg) {}
1055 bool operator<(
const Event& other )
const;
1076 { event_queues[queue_num].push(
Event( sim_time + delay, mod, cb, arg ) ); }
1084 std::set<ModelPosition*> active_velocity;
1095 void ConsumeQueue(
unsigned int queue_num );
1099 unsigned int GetEventQueue( Model* mod )
const;
1103 static bool UpdateAll();
1113 World(
const std::string& name =
"MyWorld",
1114 double ppm = DEFAULT_PPM );
1128 virtual bool IsGUI()
const {
return false; }
1135 virtual void Load(
const std::string& worldfile_path );
1137 virtual void UnLoad();
1139 virtual void Reload();
1143 virtual bool Save(
const char* filename );
1148 virtual bool Update(
void);
1175 Model* GetModel(
const std::string& name )
const;
1187 void RegisterOption(
Option* opt );
1211 const std::vector<point_t>& pts,
1224 void Map(
unsigned int layer );
1227 void UnMap(
unsigned int layer );
1230 void DrawSolid(
bool topview);
1233 void DrawFootPrint();
1236 void Translate(
double x,
double y );
1245 void SetCenterX(
double y );
1248 void SetCenterY(
double y );
1251 void SetCenter(
double x,
double y);
1254 void SetZ(
double min,
double max );
1256 void AppendTouchingModels( std::set<Model*>& touchers );
1259 Model* TestCollision();
1263 const Color& GetColor();
1264 void Rasterize( uint8_t* data,
1265 unsigned int width,
unsigned int height,
1266 meters_t cellwidth, meters_t cellheight );
1270 std::vector<point_t> mpts;
1272 std::vector<point_t> pts;
1287 std::vector< std::list<Block*>::iterator > list_entries;
1291 std::vector<Cell*> rendered_cells[2];
1298 void InvalidateModelPointCache();
1311 void BuildDisplayList( Model* mod );
1313 std::vector<Block*> blocks;
1316 meters_t minx, maxx, miny, maxy;
1330 void AppendBlock(
Block* block );
1331 void CallDisplayList( Model* mod );
1334 void AppendTouchingModels( std::set<Model*>& touchers );
1338 Model* TestCollision();
1340 void Map(
unsigned int layer );
1341 void UnMap(
unsigned int layer );
1344 void DrawSolid(
const Geom &geom);
1347 void DrawFootPrint(
const Geom &geom);
1349 void LoadBitmap( Model* mod,
const std::string& bitmapfile,
Worldfile *wf );
1350 void LoadBlock( Model* mod,
Worldfile* wf,
int entity );
1352 void Rasterize( uint8_t* data,
1353 unsigned int width,
unsigned int height,
1354 meters_t cellwidth, meters_t cellheight );
1359 (*it)->InvalidateModelPointCache();
1372 Camera() : _pitch( 0 ), _yaw( 0 ), _x( 0 ), _y( 0 ), _z( 0 ) { }
1375 virtual void Draw(
void )
const = 0;
1376 virtual void SetProjection(
void )
const = 0;
1378 double yaw(
void )
const {
return _yaw; }
1379 double pitch(
void )
const {
return _pitch; }
1381 double x(
void )
const {
return _x; }
1382 double y(
void )
const {
return _y; }
1383 double z(
void )
const {
return _z; }
1385 virtual void reset() = 0;
1404 virtual void Draw(
void )
const;
1405 virtual void SetProjection(
void )
const;
1407 void update(
void );
1409 void strafe(
double amount );
1410 void forward(
double amount );
1412 void setPose(
double x,
double y,
double z ) { _x = x; _y = y; _z = z; }
1413 void addPose(
double x,
double y,
double z ) { _x += x; _y += y; _z += z;
if( _z < 0.1 ) _z = 0.1; }
1414 void move(
double x,
double y,
double z );
1415 void setFov(
double horiz_fov,
double vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; }
1420 double vertFov(
void )
const {
return _vert_fov; }
1423 void addPitch(
double pitch ) { _pitch += pitch;
if( _pitch < 0 ) _pitch = 0;
else if( _pitch > 180 ) _pitch = 180; }
1426 return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) );
1431 void setClip(
double near,
double far ) { _z_far = far; _z_near = near; }
1433 void reset() { setPitch( 70 ); setYaw( 0 ); }
1443 double _pixels_width;
1444 double _pixels_height;
1457 virtual void Draw()
const;
1459 virtual void SetProjection(
double pixels_width,
1460 double pixels_height,
1464 virtual void SetProjection(
void )
const;
1466 void move(
double x,
double y );
1478 else if( _pitch < 0 )
1483 void setPose(
double x,
double y) { _x = x; _y = y; }
1485 void scale(
double scale,
double shift_x = 0,
double h = 0,
double shift_y = 0,
double w = 0 );
1486 void reset(
void ) { _pitch = _yaw = 0; }
1508 std::vector<Option*> drawOptions;
1510 std::vector<usec_t> interval_log;
1522 usec_t real_time_interval;
1525 usec_t real_time_now;
1529 usec_t real_time_recorded;
1532 uint64_t timing_interval;
1535 static void windowCb( Fl_Widget* w,
WorldGui* wg );
1536 static void fileLoadCb( Fl_Widget* w,
WorldGui* wg );
1537 static void fileSaveCb( Fl_Widget* w,
WorldGui* wg );
1538 static void fileSaveAsCb( Fl_Widget* w,
WorldGui* wg );
1539 static void fileExitCb( Fl_Widget* w,
WorldGui* wg );
1542 static void helpAboutCb( Fl_Widget* w,
WorldGui* wg );
1543 static void pauseCb( Fl_Widget* w,
WorldGui* wg );
1544 static void onceCb( Fl_Widget* w,
WorldGui* wg );
1545 static void fasterCb( Fl_Widget* w,
WorldGui* wg );
1546 static void slowerCb( Fl_Widget* w,
WorldGui* wg );
1547 static void realtimeCb( Fl_Widget* w,
WorldGui* wg );
1548 static void fasttimeCb( Fl_Widget* w,
WorldGui* wg );
1549 static void resetViewCb( Fl_Widget* w,
WorldGui* wg );
1550 static void moreHelptCb( Fl_Widget* w,
WorldGui* wg );
1553 bool saveAsDialog();
1554 bool closeWindowQuery();
1556 virtual void AddModel( Model* mod );
1562 virtual void PushColor(
Color col );
1563 virtual void PushColor(
double r,
double g,
double b,
double a );
1564 virtual void PopColor();
1566 void DrawOccupancy()
const;
1567 void DrawVoxels()
const;
1571 WorldGui(
int W,
int H,
const char*L=0);
1575 virtual void Redraw(
void );
1577 virtual std::string ClockString()
const;
1578 virtual bool Update();
1579 virtual void Load(
const std::string& filename );
1580 virtual void UnLoad();
1581 virtual bool Save(
const char* filename );
1582 virtual bool IsGUI()
const {
return true; };
1583 virtual Model* RecentlySelectedModel()
const;
1585 virtual void Start();
1586 virtual void Stop();
1588 usec_t RealTimeNow(
void)
const;
1590 void DrawBoundingBoxTree();
1598 std::string EnergyString(
void )
const;
1599 virtual void RemoveChild( Model* mod );
1614 float x,y,w,h,min,max;
1615 Color fgcolor, bgcolor;
1621 const char* name,
const char* wfname );
1623 virtual void Visualize( Model* mod,
Camera* cam );
1624 void AppendValue(
float value );
1638 unsigned int columns, rows;
1639 meters_t width, height;
1641 std::vector<joules_t> cells;
1643 joules_t peak_value;
1646 static joules_t global_peak_value;
1651 meters_t cellsize );
1654 virtual void Visualize( Model* mod,
Camera* cam );
1656 void Accumulate( meters_t x, meters_t y, joules_t amount );
1694 void Visualize(
Camera* cam );
1697 joules_t RemainingCapacity()
const;
1700 void Add( joules_t j );
1703 void Subtract( joules_t j );
1706 void TransferTo(
PowerPack* dest, joules_t amount );
1709 {
return( stored / capacity ); }
1716 printf(
"%s", prefix );
1718 printf(
"PowerPack %.2f/%.2f J\n", stored, capacity );
1721 joules_t GetStored()
const;
1722 joules_t GetCapacity()
const;
1723 joules_t GetDissipated()
const;
1724 void SetCapacity( joules_t j );
1725 void SetStored( joules_t j );
1734 void Dissipate( joules_t j );
1737 void Dissipate( joules_t j,
const Pose& p );
1758 static uint32_t count;
1759 static std::map<id_t,Model*> modelsbyid;
1764 std::vector<Option*> drawOptions;
1765 const std::vector<Option*>& getOptions()
const {
return drawOptions; }
1792 : callback(cb), arg(arg) {}
1795 : callback(NULL), arg(arg) { (void)cb; }
1797 cb_t() : callback(NULL), arg(NULL) {}
1803 return( arg < other.
arg );
1805 return ((
void*)(callback)) < ((
void*)(other.
callback));
1810 {
return( callback == other.
callback); }
1821 void SetColor(
const Color& col );
1822 void SetSize(
double sz );
1827 Flag(
const Color& color,
double size );
1828 Flag* Nibble(
double portion );
1832 void Draw( GLUquadric* quadric );
1930 unsigned int width, height;
1931 meters_t cellwidth, cellheight;
1932 std::vector<point_t> pts;
1937 virtual void Visualize( Model* mod,
Camera* cam );
1939 void SetData( uint8_t* data,
1941 unsigned int height,
1943 meters_t cellheight );
1948 void AddPoint( meters_t x, meters_t y );
1975 : time(0), pose(), color(){}
2026 if( str.size() > 0 )
2032 PRINT_ERR(
"Model::SetToken() called with zero length string. Ignored." );
2041 Model* GetChild(
const std::string& name )
const;
2059 void Save(
Worldfile* wf,
int wf_entity );
2068 void Rasterize( uint8_t* data,
2069 unsigned int width,
unsigned int height,
2070 meters_t cellwidth, meters_t cellheight );
2075 explicit Model(
const Model& original);
2079 Model& operator=(
const Model& original);
2084 void RegisterOption(
Option* opt );
2086 void AppendTouchingModels( std::set<Model*>& touchers );
2092 Model* TestCollision();
2094 void CommitTestedPose();
2096 void Map(
unsigned int layer );
2101 void UnMap(
unsigned int layer );
2106 void MapWithChildren(
unsigned int layer );
2107 void UnMapWithChildren(
unsigned int layer );
2110 void MapFromRoot(
unsigned int layer );
2111 void UnMapFromRoot(
unsigned int layer );
2116 const meters_t range,
2119 const bool ztest =
true );
2123 void Raytrace(
const Pose &pose,
2124 const meters_t range,
2125 const radians_t fov,
2129 const uint32_t sample_count,
2130 const bool ztest =
true );
2133 const meters_t range,
2136 const bool ztest =
true );
2138 void Raytrace(
const radians_t bearing,
2139 const meters_t range,
2140 const radians_t fov,
2144 const uint32_t sample_count,
2145 const bool ztest =
true );
2147 virtual void Startup();
2148 virtual void Shutdown();
2149 virtual void Update();
2150 virtual void UpdateCharge();
2155 void CallUpdateCallbacks(
void );
2157 meters_t ModelHeight()
const;
2159 void DrawBlocksTree();
2160 virtual void DrawBlocks();
2161 void DrawBoundingBox();
2162 void DrawBoundingBoxTree();
2163 virtual void DrawStatus(
Camera* cam );
2164 void DrawStatusTree(
Camera* cam );
2166 void DrawOriginTree();
2169 void PushLocalCoords();
2173 void DrawImage( uint32_t texture_id,
Camera* cam,
float alpha,
double width=1.0,
double height=1.0 );
2175 virtual void DrawPicker();
2176 virtual void DataVisualize(
Camera* cam );
2177 virtual void DrawSelected(
void);
2179 void DrawTrailFootprint();
2180 void DrawTrailBlocks();
2181 void DrawTrailArrows();
2184 void DataVisualizeTree(
Camera* cam );
2185 void DrawFlagList();
2186 void DrawPose(
Pose pose );
2198 void PlaceInFreeSpace( meters_t xmin, meters_t xmax,
2199 meters_t ymin, meters_t ymax );
2203 {
return pose.
String(); }
2207 {
return modelsbyid[id]; }
2210 Model( World* world,
2211 Model* parent = NULL,
2212 const std::string& type =
"model",
2213 const std::string& name =
"" );
2220 : mapped(false), alwayson(false), blocks_dl(0),
2221 boundary(false), data_fresh(false), disabled(true), friction(0), has_default_block(false), log_state(false), map_resolution(0), mass(0), parent(NULL), rebuild_displaylist(false), stack_children(true), stall(false), subs(0), thread_safe(false),trail_index(0), event_queue_num(0), used(false), watts(0), watts_give(0),watts_take(0),wf(NULL), wf_entity(0), world(NULL)
2224 void Say(
const std::string& str );
2227 void AddVisualizer(
Visualizer* custom_visual,
bool on_by_default );
2230 void RemoveVisualizer(
Visualizer* custom_visual );
2232 void BecomeParentOf( Model* child );
2238 SetWorldfile( wf, wf_entity );
2244 { this->wf = wf; this->wf_entity = wf_entity; }
2247 virtual void Load();
2250 virtual void Save();
2253 void InitControllers();
2255 void AddFlag(
Flag* flag );
2256 void RemoveFlag(
Flag* flag );
2258 void PushFlag(
Flag* flag );
2275 void LoadControllerModule(
const char* lib );
2286 void LoadBlock(
Worldfile* wf,
int entity );
2290 Block* AddBlockRect( meters_t x, meters_t y,
2291 meters_t dx, meters_t dy,
2299 Model*
Parent()
const {
return this->parent; }
2305 Model*
Root(){
return( parent ? parent->
Root() : this ); }
2307 bool IsAntecedent(
const Model* testmod )
const;
2310 bool IsDescendent(
const Model* testmod )
const;
2313 bool IsRelated(
const Model* testmod )
const;
2316 Pose GetGlobalPose()
const;
2319 Velocity GetGlobalVelocity()
const;
2322 void SetGlobalVelocity(
const Velocity& gvel );
2331 void SetGlobalPose(
const Pose& gpose );
2340 void SetPose(
const Pose& pose );
2343 void AddToPose(
const Pose& pose );
2346 void AddToPose(
double dx,
double dy,
double dz,
double da );
2349 void SetGeom(
const Geom& src );
2353 void SetFiducialReturn(
int fid );
2360 void SetFiducialKey(
int key );
2368 kg_t GetTotalMass()
const;
2371 kg_t GetMassOfChildren()
const;
2374 int SetParent( Model* newparent);
2386 void SetColor(
Color col );
2387 void SetMass( kg_t mass );
2388 void SetStall(
bool stall );
2389 void SetGravityReturn(
bool val );
2390 void SetGripperReturn(
bool val );
2391 void SetStickyReturn(
bool val );
2392 void SetRangerReturn(
double val );
2393 void SetObstacleReturn(
bool val );
2394 void SetBlobReturn(
bool val );
2395 void SetRangerReturn(
bool val );
2396 void SetBoundary(
bool val );
2397 void SetGuiNose(
bool val );
2398 void SetGuiMove(
bool val );
2399 void SetGuiGrid(
bool val );
2400 void SetGuiOutline(
bool val );
2401 void SetWatts( watts_t watts );
2402 void SetMapResolution( meters_t res );
2403 void SetFriction(
double friction );
2416 void AddCallback( callback_type_t type,
2420 int RemoveCallback( callback_type_t type,
2423 int CallCallbacks( callback_type_t type );
2426 virtual void Print(
char* prefix )
const;
2427 virtual const char* PrintWithPose()
const;
2431 Pose GlobalToLocal(
const Pose& pose )
const;
2437 return( ( GetGlobalPose() + geom.
pose ) + pose );
2441 std::vector<point_int_t> LocalToPixels(
const std::vector<point_t>& local )
const;
2449 Model* GetUnsubscribedModelOfType(
const std::string& type )
const;
2453 Model* GetUnusedModelOfType(
const std::string& type );
2491 uint32_t left,
top, right, bottom;
2500 Vis( World* world );
2502 virtual void Visualize( Model* mod,
Camera* cam );
2506 std::vector<Blob> blobs;
2507 std::vector<Color> colors;
2510 static bool BlockMatcher(
Block* testblock, Model* finder );
2522 const std::string& type );
2526 virtual void Startup();
2527 virtual void Shutdown();
2528 virtual void Update();
2529 virtual void Load();
2533 if( count ) *count = blobs.size();
2540 void AddColor(
Color col );
2543 void RemoveColor(
Color col );
2547 void RemoveAllColors();
2560 const std::string& type );
2563 void SetState(
bool isOn);
2566 virtual void DrawBlocks();
2615 double break_beam_inset[2];
2621 virtual void Update();
2622 virtual void DataVisualize(
Camera* cam );
2625 void PositionPaddles();
2626 void UpdateBreakBeams();
2627 void UpdateContacts();
2633 Block* paddle_right;
2643 const std::string& type );
2647 virtual void Load();
2648 virtual void Save();
2705 void AddModelIfVisible( Model* him );
2707 virtual void Update();
2708 virtual void DataVisualize(
Camera* cam );
2713 std::vector<Fiducial> fiducials;
2718 const std::string& type );
2721 virtual void Load();
2722 void Shutdown(
void );
2738 if( count ) *count = fiducials.size();
2739 return &fiducials[0];
2752 const std::string& type );
2755 virtual void Load();
2756 virtual void Print(
char* prefix )
const;
2767 Vis( World* world );
2769 virtual void Visualize( Model* mod,
Camera* cam );
2787 size( 0.02, 0.02, 0.02 ),
2799 std::string String()
const;
2811 void LoadSensor(
Worldfile* wf,
int entity );
2814 std::vector<Sensor> sensors;
2818 virtual void Startup();
2819 virtual void Shutdown();
2820 virtual void Update();
2832 static Option showBlinkenData;
2836 const std::string& type );
2840 virtual void Load();
2841 virtual void Update();
2842 virtual void DataVisualize(
Camera* cam );
2861 GLfloat* _frame_data;
2862 GLubyte* _frame_color_data;
2864 bool _valid_vertexbuf_cache;
2869 static const int _depth = 4;
2871 int _camera_quads_size;
2872 GLfloat* _camera_quads;
2873 GLubyte* _camera_colors;
2875 static Option showCameraData;
2879 double _pitch_offset;
2887 const std::string& type );
2891 virtual void Load();
2894 virtual void Update();
2900 virtual void DataVisualize(
Camera* cam );
2918 void setPitch(
double pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache =
false; }
2921 void setYaw(
double yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache =
false; }
2955 ControlMode control_mode;
2956 DriveMode drive_mode;
2957 LocalizationMode localization_mode;
2972 const std::string& type );
2976 virtual void Move();
2977 virtual void Startup();
2978 virtual void Shutdown();
2979 virtual void Update();
2980 virtual void Load();
2985 void SetVelocity(
const Velocity& val );
2992 Waypoint( meters_t x, meters_t y, meters_t z, radians_t
a,
Color color ) ;
3008 virtual void Visualize( Model* mod,
Camera* cam );
3016 virtual void Visualize( Model* mod,
Camera* cam );
3020 void SetOdom(
Pose odom );
3024 void SetSpeed(
double x,
double y,
double a );
3025 void SetXSpeed(
double x );
3026 void SetYSpeed(
double y );
3027 void SetZSpeed(
double z );
3028 void SetTurnSpeed(
double a );
3035 void GoTo(
double x,
double y,
double a );
3036 void GoTo(
Pose pose );
3041 void SetAcceleration(
double x,
double y,
double a );
3072 double min_position;
3073 double max_position;
3074 double start_position;
3077 ControlMode control_mode;
3078 ActuatorType actuator_type;
3086 const std::string& type );
3090 virtual void Startup();
3091 virtual void Shutdown();
3092 virtual void Update();
3093 virtual void Load();
3097 void SetSpeed(
double speed );
3103 void GoTo(
double pose );
3109 ActuatorType
GetType()
const {
return actuator_type; }
Bounds()
Definition: stage.hh:437
callback_type_t
Definition: stage.hh:1835
radians_t fov
field of view
Definition: stage.hh:2727
meters_t range
range to the target
Definition: stage.hh:2694
Pose pose
location and direction of the ray origin
Definition: stage.hh:744
std::vector< point_int_t > rt_candidate_cells
Definition: stage.hh:935
unsigned int scan_height
Definition: stage.hh:2516
virtual void PushColor(Color col)
Definition: stage.hh:2189
Definition: stage.hh:3011
Geom(const Pose &p, const Size &s)
Definition: stage.hh:419
Model class
Definition: stage.hh:1742
Definition: stage.hh:1785
meters_t max_range_anon
maximum detection range
Definition: stage.hh:2724
Definition: stage.hh:1628
Model * Parent() const
Definition: stage.hh:2299
void coord_shift(double x, double y, double z, double a)
Definition: gl.cc:6
RaytraceResult(const Pose &pose, meters_t range)
Definition: stage.hh:750
virtual void Start()
Definition: stage.hh:923
msec_t period
duration of a complete cycle
Definition: stage.hh:581
Worldfile * GetWorldFile()
Definition: stage.hh:1123
bool autosnatch
if true, cycle the gripper through open-close-up-down automatically
Definition: stage.hh:2614
point3_t(meters_t x, meters_t y, meters_t z)
Definition: stage.hh:486
meters_t size
rendered as a sphere with this diameter
Definition: stage.hh:579
Sensor()
Definition: stage.hh:2786
int subs
the number of subscriptions to this model
Definition: stage.hh:1959
ActuatorType GetType() const
Definition: stage.hh:3109
Event(usec_t time, Model *mod, model_callback_t cb, void *arg)
Definition: stage.hh:1045
static size_t Count()
Definition: stage.hh:793
Canvas * GetCanvas(void) const
Definition: stage.hh:1592
const std::string & GetWorldfileName()
Definition: stage.hh:562
point_int_t(int x, int y)
Definition: stage.hh:497
bool ztest
Definition: stage.hh:770
cmd_t
Definition: stage.hh:2593
void Enqueue(unsigned int queue_num, usec_t delay, Model *mod, model_callback_t cb, void *arg)
Definition: stage.hh:1075
joules_t stored
Definition: stage.hh:1667
Bounds y
volume extent along y axis, initially zero
Definition: stage.hh:453
radians_t heading
center of field of view
Definition: stage.hh:2728
std::list< Visualizer * > cv_list
Definition: stage.hh:1872
void CommandOpen()
Definition: stage.hh:2661
virtual void PushColor(Color col)
Definition: stage.hh:988
point3_t GetAxis() const
Definition: stage.hh:3110
point_int_t()
Definition: stage.hh:498
const Model * mod
Definition: stage.hh:765
static joules_t global_input
Definition: stage.hh:1687
const std::set< Model * > GetAllModels() const
Definition: stage.hh:1178
std::list< PowerPack * > pps_charging
Definition: stage.hh:1923
bool operator==(const point_int_t &other) const
Definition: stage.hh:508
Model * mod
Definition: stage.hh:1664
double a
Definition: stage.hh:200
Definition: stage.hh:2934
std::string cmdline
Definition: stage.hh:806
uint64_t updates
the number of simulated time steps executed so far
Definition: stage.hh:912
unsigned int GetFlagCount() const
Definition: stage.hh:2261
virtual ~Vis(void)
Definition: stage.hh:2768
Velocity()
Definition: stage.hh:363
const std::string & GetMenuName()
Definition: stage.hh:561
bool stack_children
whether child models should be stacked on top of this model or not
Definition: stage.hh:1956
bool graphics
true iff we have a GUI
Definition: stage.hh:900
const char LICENSE[]
Definition: stage.hh:116
virtual bool IsGUI() const
Definition: stage.hh:1582
double max
largest value in range, initially zero
Definition: stage.hh:435
Definition: stage.hh:2824
usec_t SimTimeNow(void) const
Definition: stage.hh:1119
World class
Definition: stage.hh:814
double dtor(double d)
Definition: stage.hh:151
void setYaw(double yaw)
Definition: stage.hh:1468
usec_t GetEnergyInterval() const
Definition: stage.hh:2063
std::string String() const
Definition: stage.hh:286
Definition: stage.hh:2595
The Stage library uses its own namespace.
Definition: canvas.hh:8
bool operator<(const Pose &other) const
Definition: stage.hh:317
void Print(const char *prefix) const
Definition: stage.hh:402
std::vector< Fiducial > & GetFiducials()
Definition: stage.hh:2733
static const Color red
Definition: stage.hh:217
void EnableEnergy(Model *m)
Definition: stage.hh:1080
GLfloat z
Definition: stage.hh:2855
void setPose(double x, double y)
Definition: stage.hh:1483
Color color
Definition: stage.hh:580
void CommandClose()
Definition: stage.hh:2659
std::list< std::pair< world_callback_t, void * > > cb_list
List of callback functions and arguments.
Definition: stage.hh:898
int fiducial_return
Definition: stage.hh:2051
Definition: stage.hh:2487
radians_t fov
Definition: stage.hh:2778
Definition: stage.hh:2581
static std::vector< std::string > args
Definition: stage.hh:826
bool InitDone()
Definition: stage.cc:41
static const Color magenta
Definition: stage.hh:217
ActuatorType
Definition: stage.hh:3063
radians_t fov
Definition: stage.hh:2513
Definition: stage.hh:1839
bool ignore_zloc
Are we ignoring the Z-loc of the fiducials we detect compared to the fiducial detector?
Definition: stage.hh:2730
virtual void Stop()
Definition: stage.hh:924
bool DataIsFresh() const
Definition: stage.hh:2405
meters_t x
Definition: stage.hh:228
void ChargeStop()
Definition: stage.hh:1731
void GLSet(void)
Definition: stage.hh:221
Model * GetModel()
Definition: stage.hh:1262
Size()
Definition: stage.hh:237
std::list< float * > ray_list
List of rays traced for debug visualization.
Definition: stage.hh:906
Definition: stage.hh:1967
StripPlotVis output_vis
Definition: stage.hh:1660
double lift_position
0.0 = full down, 1.0 full up
Definition: stage.hh:2610
int getHeight(void) const
height of captured image
Definition: stage.hh:2906
Definition: stage.hh:1439
Definition: stage.hh:2589
cb_t(model_callback_t cb, void *arg)
Definition: stage.hh:1791
Color GetColor()
Definition: stage.hh:1824
virtual void PushColor(double r, double g, double b, double a)
Definition: stage.hh:990
static joules_t global_capacity
Definition: stage.hh:1685
uint32_t GetCount()
Definition: stage.hh:1322
std::set< Option * > option_table
GUI options (toggles) registered by models.
Definition: stage.hh:902
const char AUTHORS[]
Definition: stage.hh:105
int key
/// only detect fiducials with a key that matches this one (defaults 0)
Definition: stage.hh:2729
PowerPack * power_pack
Definition: stage.hh:1919
double _yaw
Definition: stage.hh:1368
Definition: stage.hh:3064
CtrlArgs(std::string w, std::string c)
Definition: stage.hh:808
Definition: options_dlg.hh:20
virtual void PushColor(double r, double g, double b, double a)
Definition: stage.hh:2190
virtual void PopColor()
Definition: stage.hh:993
std::string token
Definition: stage.hh:696
void Init(int *argc, char **argv[])
Definition: stage.cc:18
static const Size size
Definition: stage.hh:2638
std::vector< Sensor > & GetSensorsMutable()
Definition: stage.hh:2808
Definition: stage.hh:2772
void addPose(double x, double y, double z)
Definition: stage.hh:1413
static const Color blue
Definition: stage.hh:217
int x
Definition: stage.hh:496
virtual ~RasterVis(void)
Definition: stage.hh:1936
Definition: stage.hh:2575
Definition: stage.hh:1926
static Color RandomColor()
Definition: color.cc:95
StripPlotVis stored_vis
Definition: stage.hh:1661
meters_t z
Definition: stage.hh:485
double scale() const
Definition: stage.hh:1488
cb_t(world_callback_t cb, void *arg)
Definition: stage.hh:1794
static joules_t global_stored
Definition: stage.hh:1684
point_t()
Definition: stage.hh:475
usec_t sim_interval
Definition: stage.hh:1087
double _z
Definition: stage.hh:1369
Geom geom
Definition: stage.hh:1883
void Zero()
Definition: stage.hh:243
bool used
TRUE iff this model has been returned by GetUnusedModelOfType()
Definition: stage.hh:2003
bool operator==(const Pose &other) const
Definition: stage.hh:322
const std::string type
Definition: stage.hh:1999
static void Clear()
Definition: stage.hh:796
watts_t watts
power consumed by this model
Definition: stage.hh:2005
Definition: stage.hh:2583
Definition: stage.hh:1392
int(* model_callback_t)(Model *mod, void *user)
Definition: stage.hh:568
void EraseAll(T thing, C &cont)
Definition: stage.hh:621
double min
smallest value in range, initially zero
Definition: stage.hh:433
const double billion
Definition: stage.hh:145
const char * Version()
Definition: stage.cc:12
double nearClip(void) const
Definition: stage.hh:1429
void setYaw(double yaw)
Definition: stage.hh:1418
void SetConfig(config_t &newcfg)
Definition: stage.hh:2651
Color GetColor() const
Definition: stage.hh:2362
Size size
extent
Definition: stage.hh:395
uint32_t id
Definition: stage.hh:1901
void draw_origin(double len)
Definition: gl.cc:134
const double thousand
Definition: stage.hh:139
double GetSpeed() const
Definition: stage.hh:3099
double y(void) const
Definition: stage.hh:1382
void Print(const char *prefix) const
Definition: stage.hh:1713
meters_t map_resolution
Definition: stage.hh:1908
Definition: stage.hh:1836
int update_cb_count
Definition: stage.hh:1092
Pose & Load(Worldfile *wf, int section, const char *keyword)
Definition: model.cc:180
double yaw(void) const
Definition: stage.hh:1378
uint32_t top
Definition: stage.hh:2491
double duty_cycle
mark/space ratio
Definition: stage.hh:582
ray_test_func_t func
Definition: stage.hh:768
uint64_t UpdateCount()
Definition: stage.hh:919
void draw_string_multiline(float x, float y, float w, float h, const char *string, Fl_Align align)
Definition: gl.cc:77
watts_t watts_take
Definition: stage.hh:2013
meters_t range
Definition: stage.hh:2515
void setPose(double x, double y, double z)
Definition: stage.hh:1412
ModelCamera class
Definition: stage.hh:2849
double ProportionRemaining() const
Definition: stage.hh:1708
unsigned int event_queue_num
Definition: stage.hh:2002
usec_t interval_energy
time between updates of powerpack in usec
Definition: stage.hh:1903
Definition: stage.hh:2590
const double million
Definition: stage.hh:142
double _pitch
Definition: stage.hh:1367
usec_t time
time that event occurs
Definition: stage.hh:1048
virtual void PopColor()
Definition: stage.hh:2191
void Load(Worldfile *wf, int wf_entity)
Definition: stage.hh:2234
model_callback_t cb
Definition: stage.hh:1050
Bounds(double min, double max)
Definition: stage.hh:438
std::vector< TrailItem > trail
Definition: stage.hh:1982
double ranger_return
Definition: stage.hh:2054
void setFov(double horiz_fov, double vert_fov)
Definition: stage.hh:1415
kg_t mass
Definition: stage.hh:1909
std::map< std::string, unsigned int > child_type_counts
Definition: stage.hh:687
lift_state_t
Definition: stage.hh:2586
usec_t last_time
Definition: stage.hh:1679
bool nose
Definition: stage.hh:1891
float s
Definition: glutgraphics.cc:58
virtual ~Pose()
Definition: stage.hh:264
void RegisterModels()
Definition: typetable.cc:19
void reset(void)
Definition: stage.hh:1486
TrailItem()
Definition: stage.hh:1974
bool thread_safe
Definition: stage.hh:1964
virtual ~PoseVis(void)
Definition: stage.hh:3015
virtual ~WaypointVis(void)
Definition: stage.hh:3007
World * GetWorld() const
Definition: stage.hh:2302
meters_t range
Definition: stage.hh:2492
Color col
Definition: stage.hh:2780
usec_t time
Definition: stage.hh:1970
meters_t z
location in 3 axes
Definition: stage.hh:251
meters_t y
Definition: stage.hh:251
Definition: file_manager.hh:9
Worldfile * wf
Definition: stage.hh:2015
bool charging
Definition: stage.hh:1673
point_t(meters_t x, meters_t y)
Definition: stage.hh:474
Definition: stage.hh:2596
ModelPosition class
Definition: stage.hh:2927
void InvalidateModelPointCache()
Definition: stage.hh:1356
void draw_octagon(float x, float y, float w, float h, float m)
Definition: gl.cc:106
void SetWorldfile(Worldfile *wf, int wf_entity)
Definition: stage.hh:2243
double z(void) const
Definition: stage.hh:1383
OrthoCamera(void)
Definition: stage.hh:1449
bool disabled
Definition: stage.hh:1869
Color color
Definition: stage.hh:2998
Pose pose
Definition: stage.hh:589
double watts_t
Definition: stage.hh:195
Definition: stage.hh:1813
int y
Definition: stage.hh:496
Model * mod
Pointer to the model (real fiducial detectors can't do this!)
Definition: stage.hh:2699
Bounds range
Definition: stage.hh:2777
joules_t last_joules
Definition: stage.hh:1680
void CancelQuit()
Definition: stage.hh:1162
Definition: stage.hh:1843
void pose_inverse_shift(const Pose &pose)
Definition: gl.cc:18
Pose origin
Definition: stage.hh:766
double normalize(double a)
Definition: stage.hh:154
Definition: stage.hh:2947
const Color & Load(Worldfile *wf, int entity)
Definition: color.cc:105
int wf_entity
Definition: stage.hh:2016
bool move
Definition: stage.hh:1890
Model * Root()
Definition: stage.hh:2305
std::vector< double > bearings
Definition: stage.hh:2784
bool operator<(const point_int_t &other) const
Definition: stage.hh:501
lift_state_t lift
Definition: stage.hh:2608
double GetMinPosition() const
Definition: stage.hh:3107
WorldGui * world_gui
Definition: stage.hh:2018
Model * mod
model to pass into callback
Definition: stage.hh:1049
static unsigned int trail_length
Definition: stage.hh:1990
static int UpdateWrapper(Model *mod, void *arg)
Definition: stage.hh:2152
virtual ~Visualizer(void)
Definition: stage.hh:558
static int argc
Definition: glutgraphics.cc:238
usec_t GetUpdateInterval() const
Definition: stage.hh:2062
Pose est_pose
position estimate in local coordinates
Definition: stage.hh:3044
usec_t sim_time
the current sim time in this world in microseconds
Definition: stage.hh:907
void draw_centered_rect(float x, float y, float dx, float dy)
Definition: gl.cc:121
void ChargeStart()
Definition: stage.hh:1730
unsigned int sample_count
Definition: stage.hh:2779
Definition: stage.hh:2941
void setClip(double near, double far)
Definition: stage.hh:1431
void addYaw(double yaw)
Definition: stage.hh:1472
Size paddle_size
paddle dimensions
Definition: stage.hh:2606
double r
Definition: stage.hh:200
virtual void TogglePause()
Definition: stage.hh:925
int rotrects_from_image_file(const std::string &filename, std::vector< rotrect_t > &rects)
Definition: stage.cc:102
Definition: stage.hh:1841
static Option showArea
Definition: stage.hh:2761
std::string PoseString()
Definition: stage.hh:2202
static Option showBeams
Definition: stage.hh:2764
Model()
Definition: stage.hh:2219
double rtod(double r)
Definition: stage.hh:148
static Pose Random(meters_t xmin, meters_t xmax, meters_t ymin, meters_t ymax)
Definition: stage.hh:268
radians_t bearing
bearing to the target
Definition: stage.hh:2695
Definition: stage.hh:1837
void addPitch(double pitch)
Definition: stage.hh:1474
meters_t range
Definition: stage.hh:767
Definition: stage.hh:2588
Ray()
Definition: stage.hh:762
double GetSize()
Definition: stage.hh:1825
double realDistance(double z_buf_val) const
Definition: stage.hh:1425
int used
Definition: stage.hh:1946
Pose pose
Definition: stage.hh:1971
Pose pose
position
Definition: stage.hh:394
const bounds3d_t & GetExtent() const
Definition: stage.hh:1181
bool obstacle_return
Definition: stage.hh:2053
Camera()
Definition: stage.hh:1372
std::map< point_int_t, SuperRegion * > superregions
Definition: stage.hh:908
meters_t range
range to beam hit in meters
Definition: stage.hh:745
#define PRINT_ERR(m)
Definition: stage.hh:625
const Size & GetSize()
Definition: stage.hh:1323
int getWidth(void) const
width of captured image
Definition: stage.hh:2903
model_callback_t callback
Definition: stage.hh:1788
Definition: stage.hh:2852
meters_t y
Definition: stage.hh:473
meters_t max_range_id
maximum range at which the ID can be read
Definition: stage.hh:2725
bool Stalled() const
Definition: stage.hh:2457
std::string GetSayString()
Definition: stage.hh:2037
Definition: stage.hh:1498
Definition: stage.hh:1845
uint64_t usec_t
Definition: stage.hh:186
Definition: stage.hh:1635
bounds3d_t extent
Describes the 3D volume of the world.
Definition: stage.hh:899
void Zero()
Definition: stage.hh:299
void draw_grid(bounds3d_t vol)
Definition: gl.cc:141
double meters_t
Definition: stage.hh:174
bool GetCharging() const
Definition: stage.hh:1728
virtual Model * RecentlySelectedModel() const
Definition: stage.hh:967
joules_t dissipated
Definition: stage.hh:1676
Definition: stage.hh:1197
void draw_vector(double x, double y, double z)
Definition: gl.cc:126
meters_t x
Definition: stage.hh:473
const char DESCRIPTION[]
Definition: stage.hh:112
usec_t interval
time between updates in usec
Definition: stage.hh:1902
radians_t pan
Definition: stage.hh:2514
static const Color cyan
Definition: stage.hh:217
std::map< std::string, void * > props
Definition: stage.hh:694
meters_t y
Definition: stage.hh:228
int32_t MetersToPixels(meters_t x) const
Definition: stage.hh:981
Pose operator+(const Pose &p) const
Definition: stage.hh:305
Definition: stage.hh:1041
Pose pose
Definition: stage.hh:578
const std::vector< Sensor > & GetSensors() const
Definition: stage.hh:2804
void pose_shift(const Pose &pose)
Definition: gl.cc:13
Definition: stage.hh:1846
std::vector< Blob > GetBlobs() const
Definition: stage.hh:2537
std::vector< point_int_t > rt_cells
Definition: stage.hh:932
ControlMode
Definition: stage.hh:2933
bool operator<(const cb_t &other) const
Definition: stage.hh:1800
Color color
the color struck by this beam
Definition: stage.hh:747
void CommandUp()
Definition: stage.hh:2663
Visualizer(const std::string &menu_name, const std::string &worldfile_name)
Definition: stage.hh:552
double Resolution() const
Definition: stage.hh:1171
void SetProperty(std::string &key, void *value)
Definition: stage.hh:729
void SetCommand(cmd_t cmd)
Definition: stage.hh:2657
void CommandDown()
Definition: stage.hh:2665
std::vector< Waypoint > waypoints
Definition: stage.hh:3001
meters_t z
Definition: stage.hh:228
Pose pose
Absolute accurate position of the target in world coordinates (it's cheating to use this in robot con...
Definition: stage.hh:2698
unsigned int GetSubscriptionCount() const
Definition: stage.hh:2461
static std::vector< LogEntry > log
Definition: stage.hh:790
double GetMaxPosition() const
Definition: stage.hh:3106
radians_t angle
width of viewing angle of sensor
Definition: stage.hh:466
void addPitch(double pitch)
Definition: stage.hh:1423
watts_t watts_give
Definition: stage.hh:2009
const char COPYRIGHT[]
Definition: stage.hh:101
double b
Definition: stage.hh:200
virtual void SetToken(const std::string &str)
Definition: stage.hh:2022
double close_limit
How far the gripper can close. If < 1.0, the gripper has its mouth full.
Definition: stage.hh:2613
void Enable()
Definition: stage.hh:2271
Definition: stage.hh:1844
Pose est_pose_error
estimated error in position estimate
Definition: stage.hh:3045
Velocity & Load(Worldfile *wf, int section, const char *keyword)
Definition: stage.hh:372
Worldfile * wf
If set, points to the worldfile used to create this world.
Definition: stage.hh:913
bool blob_return
Definition: stage.hh:2049
bool(* ray_test_func_t)(Model *candidate, Model *finder, const void *arg)
Definition: stage.hh:603
bool has_default_block
Definition: stage.hh:1898
void draw_string(float x, float y, float z, const char *string)
Definition: gl.cc:65
bool stall
Set to true iff the model collided with something else.
Definition: stage.hh:1958
static char * argv
Definition: glutgraphics.cc:239
static Model * LookupId(uint32_t id)
Definition: stage.hh:2206
virtual void Update()
Definition: model.cc:735
double g
Definition: stage.hh:200
double horizFov(void) const
Definition: stage.hh:1419
Definition: stage.hh:1842
Bounds x
volume extent along x axis, intially zero
Definition: stage.hh:451
Bounds range
min and max range of sensor
Definition: stage.hh:465
void Map()
Definition: stage.hh:2099
usec_t GetInterval()
Definition: stage.hh:2044
virtual ~Camera()
Definition: stage.hh:1373
void NeedRedraw()
Definition: stage.hh:951
unsigned int trail_index
Definition: stage.hh:1985
Pose(meters_t x, meters_t y, meters_t z, radians_t a)
Definition: stage.hh:254
void Disable()
Definition: stage.hh:2267
std::vector< Model * > children
Definition: stage.hh:689
bool alwayson
Definition: stage.hh:1771
virtual bool IsGUI() const
Definition: stage.hh:1128
bool paused
if true, the simulation is stopped
Definition: stage.hh:921
unsigned long msec_t
Definition: stage.hh:183
double x(void) const
Definition: stage.hh:1381
const GLfloat * FrameDepth() const
get a reference to camera depth buffer
Definition: stage.hh:2912
ModelFiducial class
Definition: stage.hh:2687
std::vector< std::set< cb_t > > callbacks
Definition: stage.hh:1856
Pose pose
Definition: stage.hh:2775
int blocks_dl
Definition: stage.hh:1775
std::vector< std::priority_queue< Event > > event_queues
Definition: stage.hh:1059
const PerspectiveCamera & getCamera(void) const
get reference to camera used
Definition: stage.hh:2909
virtual void Print(const char *prefix) const
Definition: stage.hh:280
bounds3d_t(const Bounds &x, const Bounds &y, const Bounds &z)
Definition: stage.hh:458
virtual void SetToken(const std::string &str)
Definition: stage.hh:718
double pitch(void) const
Definition: stage.hh:1379
meters_t Distance2D(const Pose &other) const
Definition: stage.hh:338
ModelRanger class
Definition: stage.hh:2747
Velocity GetVelocity() const
Definition: stage.hh:2984
BlockGroup blockgroup
Definition: stage.hh:1773
bool paddles_stalled
Definition: stage.hh:2612
LocalizationMode
Definition: stage.hh:2940
Bounds z
volume extent along z axis, initially zero
Definition: stage.hh:455
void Zero()
Definition: stage.hh:421
Pose pose
Definition: stage.hh:1916
bool operator==(const cb_t &other) const
Definition: stage.hh:1809
Definition: stage.hh:1303
std::string worldfile
Definition: stage.hh:805
void setScale(double scale)
Definition: stage.hh:1482
static std::map< std::string, creator_t > name_map
Definition: stage.hh:2466
const char WEBSITE[]
Definition: stage.hh:109
bool debug
Definition: stage.hh:691
uint32_t id_t
Definition: stage.hh:171
Pose pose
Definition: stage.hh:2997
static Option showTransducers
Definition: stage.hh:2765
void ShowClock(bool enable)
Control printing time to stdout.
Definition: stage.hh:1190
ControlMode
Definition: stage.hh:3057
Pose geom
size and relative angle of the target
Definition: stage.hh:2696
World * world
Definition: stage.hh:2017
bool rebuild_displaylist
iff true, regenerate block display list before redraw
Definition: stage.hh:1953
Color()
Definition: color.cc:13
bool operator!=(const Color &other) const
Definition: color.cc:17
void setPitch(double pitch)
Definition: stage.hh:1470
bool IsZero() const
Definition: stage.hh:295
Definition: stage.hh:2758
std::set< Model * > active_energy
Definition: stage.hh:1079
void scroll(double dy)
Definition: stage.hh:1428
int GetFiducialReturn() const
Definition: stage.hh:2356
bool data_fresh
Definition: stage.hh:1864
void CancelQuitAll()
Definition: stage.hh:1165
std::string say_string
if non-empty, this string is displayed in the GUI
Definition: stage.hh:1954
int enabled
Definition: stage.hh:577
std::list< PowerPack * > powerpack_list
List of all the powerpacks attached to models in the world.
Definition: stage.hh:903
void addYaw(double yaw)
Definition: stage.hh:1421
Definition: stage.hh:1364
std::vector< std::queue< Model * > > pending_update_callbacks
Definition: stage.hh:1062
void draw_speech_bubble(float x, float y, float z, const char *str)
Definition: gl.cc:83
bool operator==(const Color &other) const
Definition: color.cc:90
Definition: stage.hh:2046
const void * arg
Definition: stage.hh:769
DriveMode
Definition: stage.hh:2946
const std::string & TokenStr() const
Definition: stage.hh:716
std::list< Flag * > flag_list
Definition: stage.hh:1875
static Option showFov
Definition: stage.hh:2763
Definition: stage.hh:3003
void Quit()
Definition: stage.hh:1156
Size size
Definition: stage.hh:590
bool HasSubscribers() const
Definition: stage.hh:2464
void QuitAll()
Definition: stage.hh:1159
Definition: stage.hh:2935
std::vector< Model * > & GetChildren()
Definition: stage.hh:706
void UnMap()
Definition: stage.hh:2104
double constrain(double val, double minval, double maxval)
Definition: stage.cc:235
static const Color green
Definition: stage.hh:217
static const Color yellow
Definition: stage.hh:217
Definition: worldfile.hh:69
struct timeval time_t
Definition: stage.hh:180
joules_t capacity
Definition: stage.hh:1670
Definition: stage.hh:2691
const GLubyte * FrameColor() const
get a reference to camera color image. 4 bytes (RGBA) per pixel
Definition: stage.hh:2915
Geom()
Definition: stage.hh:416
#define FOR_EACH(I, C)
Definition: stage.hh:616
paddle_state_t paddles
Definition: stage.hh:2607
Definition: stage.hh:1838
virtual void Print(const char *prefix) const
Definition: stage.hh:378
Pose est_origin
global origin of the local coordinate system
Definition: stage.hh:3046
Pose GetPose() const
Definition: stage.hh:2382
ModelActuator class
Definition: stage.hh:3053
Definition: stage.hh:1605
const point3_t & GetOffset()
Definition: stage.hh:1324
radians_t a
rotation about the z axis.
Definition: stage.hh:252
Definition: stage.hh:2948
static Option showStrikes
Definition: stage.hh:2762
meters_t min_range
minimum detection range
Definition: stage.hh:2726
void DisableEnergy(Model *m)
Definition: stage.hh:1081
Definition: stage.hh:1840
void setAspect(double aspect)
update vertical fov based on window aspect and current horizontal fov
Definition: stage.hh:1417
std::vector< meters_t > ranges
Definition: stage.hh:2782
bool operator!=(const Pose &other) const
Definition: stage.hh:330
void * arg
Definition: stage.hh:1051
Definition: stage.hh:2555
int(* world_callback_t)(World *world, void *user)
Definition: stage.hh:570
const std::string & GetModelType() const
Definition: stage.hh:2036
void setPitch(double pitch)
Definition: stage.hh:1422
double friction
Definition: stage.hh:1879
Definition: stage.hh:3058
std::vector< double > intensities
Definition: stage.hh:2783
void AddModelName(Model *mod, const std::string &name)
Definition: world.cc:290
double vertFov(void) const
Definition: stage.hh:1420
void setPitch(double pitch)
change the pitch
Definition: stage.hh:2918
Definition: stage.hh:2597
uint64_t GetUpdateCount() const
Definition: stage.hh:1184
double kg_t
Definition: stage.hh:189
static uint64_t trail_interval
Definition: stage.hh:1993
double paddle_position
0.0 = full open, 1.0 full closed
Definition: stage.hh:2609
bool Paused() const
Definition: stage.hh:927
Color color
Definition: stage.hh:2490
Color color
Definition: stage.hh:1859
int boundary
Definition: stage.hh:1780
bool log_state
iff true, model state is logged
Definition: stage.hh:1907
bool gripper_return
Definition: stage.hh:2052
Definition: stage.hh:2604
watts_t last_watts
Definition: stage.hh:1681
bool outline
Definition: stage.hh:1892
static std::string ctrlargs
Definition: stage.hh:827
config_t GetConfig()
Definition: stage.hh:2654
static joules_t global_dissipated
Definition: stage.hh:1686
std::vector< std::vector< Model * > > update_lists
Definition: stage.hh:910
void setYaw(double yaw)
change the yaw
Definition: stage.hh:2921
Definition: stage.hh:2989
double joules_t
Definition: stage.hh:192
Definition: stage.hh:2495
Pose LocalToGlobal(const Pose &pose) const
Definition: stage.hh:2435
bool operator+=(const point_t &other)
Definition: stage.hh:477
cb_t()
Definition: stage.hh:1797
uint32_t GetId() const
Definition: stage.hh:2365
Model * mod
the model struck by this beam
Definition: stage.hh:746
RaytraceResult()
Definition: stage.hh:749
point_t * unit_square_points_create()
Definition: stage.cc:218
void * arg
Definition: stage.hh:1789
paddle_state_t
Definition: stage.hh:2579
bool TestQuit() const
Definition: stage.hh:1153
point_int_t MetersToPixels(const point_t &pt) const
Definition: stage.hh:984
void Print(const char *prefix) const
Definition: color.cc:100
ModelBlobfinder class
Definition: stage.hh:2483
point3_t()
Definition: stage.hh:489
Size(meters_t x, meters_t y, meters_t z)
Definition: stage.hh:230
virtual ~Vis(void)
Definition: stage.hh:2501
int id
the fiducial identifier of the target (i.e. its fiducial_return value), or -1 if none can be detected...
Definition: stage.hh:2700
Ray(const Model *mod, const Pose &origin, const meters_t range, const ray_test_func_t func, const void *arg, const bool ztest)
Definition: stage.hh:758
bounds3d_t()
Definition: stage.hh:457
Fiducial * GetFiducials(unsigned int *count)
Definition: stage.hh:2736
Definition: stage.hh:2582
double radians_t
Definition: stage.hh:177
void draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset)
Definition: gl.cc:49
meters_t x
Definition: stage.hh:251
void reset()
Definition: stage.hh:1433
Definition: stage.hh:1847
void * GetProperty(std::string &key)
Definition: stage.hh:732
const char * Token() const
Definition: stage.hh:715
double farClip(void) const
Definition: stage.hh:1430
Color color
Definition: stage.hh:1972
int sgn(int a)
Definition: stage.hh:162
Definition: stage.hh:1886
Size size
Definition: stage.hh:2776
Model * gripped
Definition: stage.hh:2611
int fiducial_key
Definition: stage.hh:2050
bool grid
Definition: stage.hh:1889
int subs
Definition: stage.hh:1945
Model * GetGround()
Definition: stage.hh:1193
Model * parent
Definition: stage.hh:1912
Velocity(double x, double y, double z, double a)
Definition: stage.hh:356
usec_t quit_time
Definition: stage.hh:905
double GetPosition() const
Definition: stage.hh:3105
Blob * GetBlobs(unsigned int *count)
Definition: stage.hh:2531
Geom GetGeom() const
Definition: stage.hh:2378
Pose()
Definition: stage.hh:261
unsigned int scan_width
Definition: stage.hh:2517
usec_t last_update
time of last update in us
Definition: stage.hh:1906