stage.hh
Go to the documentation of this file.
1 
2 #ifndef STG_H
3 #define STG_H
4 /*
5  * Stage : a multi-robot simulator. Part of the Player Project.
6  *
7  * Copyright (C) 2001-2009 Richard Vaughan, Brian Gerkey, Andrew
8  * Howard, Toby Collett, Reed Hedges, Alex Couture-Beil, Jeremy
9  * Asher, Pooya Karimian
10  *
11  * This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program; if not, write to the Free Software
23  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
24  *
25  */
26 
36 // C libs
37 #include <assert.h>
38 #include <libgen.h>
39 #include <pthread.h>
40 #include <stdint.h> // for portable int types eg. uint32_t
41 #include <stdio.h>
42 #include <stdlib.h>
43 #include <string.h>
44 #include <sys/time.h>
45 #include <sys/types.h>
46 #include <unistd.h>
47 
48 // C++ libs
49 #include <algorithm>
50 #include <cmath>
51 #include <iostream>
52 #include <list>
53 #include <map>
54 #include <queue>
55 #include <set>
56 #include <vector>
57 
58 // FLTK Gui includes
59 #include <FL/Fl.H>
60 #include <FL/Fl_Box.H>
61 #include <FL/Fl_Gl_Window.H>
62 #include <FL/Fl_Menu_Bar.H>
63 #include <FL/Fl_Window.H>
64 #include <FL/fl_draw.H>
65 #include <FL/gl.h> // FLTK takes care of platform-specific GL stuff
66 // except GLU
67 #ifdef __APPLE__
68 #include <OpenGL/glu.h>
69 #else
70 #include <GL/glu.h>
71 #endif
72 
74 namespace Stg {
75 // forward declare
76 class Block;
77 class Canvas;
78 class Cell;
79 class Worldfile;
80 class World;
81 class WorldGui;
82 class Model;
83 class OptionsDlg;
84 class Camera;
85 class FileManager;
86 class Option;
87 
88 typedef Model *(*creator_t)(World *, Model *, const std::string &type);
89 
92 void Init(int *argc, char **argv[]);
93 
95 bool InitDone();
96 
99 const char *Version();
100 
102 unsigned int FullVersion();
103 
105 const char COPYRIGHT[] = "Copyright Richard Vaughan and contributors 2000-2017";
106 
108 const char AUTHORS[] = "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, "
109  "Toby Collett, Jeremy Asher, Alex Couture-Beil, Adrian Böckenkamp and "
110  "contributors.";
111 
113 const char WEBSITE[] = "http://playerstage.org";
114 
116 const char DESCRIPTION[] = "Robot simulation library\nPart of the Player Project";
117 
119 const char LICENSE[] =
120  "Stage robot simulation library\n"
121  "Copyright (C) 2000-2017 Richard Vaughan and contributors\n"
122  "Part of the Player Project [http://playerstage.org]\n"
123  "\n"
124  "This program is free software; you can redistribute it and/or\n"
125  "modify it under the terms of the GNU General Public License\n"
126  "as published by the Free Software Foundation; either version 2\n"
127  "of the License, or (at your option) any later version.\n"
128  "\n"
129  "This program is distributed in the hope that it will be useful,\n"
130  "but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
131  "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
132  "GNU General Public License for more details.\n"
133  "\n"
134  "You should have received a copy of the GNU General Public License\n"
135  "along with this program; if not, write to the Free Software\n"
136  "Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.\n"
137  "\n"
138  "The text of the license may also be available online at\n"
139  "http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";
140 
142 const double thousand = 1e3;
143 
145 const double million = 1e6;
146 
148 const double billion = 1e9;
149 
151 inline double rtod(double r)
152 {
153  return (r * 180.0 / M_PI);
154 }
155 
157 inline double dtor(double d)
158 {
159  return (d * M_PI / 180.0);
160 }
161 
163 inline double normalize(double a)
164 {
165  while (a < -M_PI)
166  a += 2.0 * M_PI;
167  while (a > M_PI)
168  a -= 2.0 * M_PI;
169  return a;
170 }
171 
173 inline int sgn(int a)
174 {
175  return (a < 0 ? -1 : 1);
176 }
177 
179 inline double sgn(double a)
180 {
181  return (a < 0 ? -1.0 : 1.0);
182 }
183 
185 enum { FiducialNone = 0 };
186 
188 typedef uint32_t id_t;
189 
191 typedef double meters_t;
192 
194 typedef double radians_t;
195 
197 typedef struct timeval time_t;
198 
200 typedef unsigned long msec_t;
201 
203 typedef uint64_t usec_t;
204 
206 typedef double kg_t; // Kilograms (mass)
207 
209 typedef double joules_t;
210 
212 typedef double watts_t;
213 
214 class Color {
215 public:
216  double r, g, b, a;
217 
218  explicit Color(double r, double g, double b, double a = 1.0);
219 
223  explicit Color(const std::string &name);
224 
225  Color();
226 
227  bool operator!=(const Color &other) const;
228  bool operator==(const Color &other) const;
229  static Color RandomColor();
230  void Print(const char *prefix) const;
231 
233  static const Color blue, red, green, yellow, magenta, cyan;
234 
235  const Color &Load(Worldfile *wf, int entity);
236 
237  void GLSet(void) { glColor4f(r, g, b, a); }
238 };
239 
241 class Size {
242 public:
244 
245  Size(meters_t x, meters_t y, meters_t z) : x(x), y(y), z(z) { /*empty*/}
246 
248  Size() : x(0.4), y(0.4), z(1.0) { /*empty*/}
249 
250  Size &Load(Worldfile *wf, int section, const char *keyword);
251  void Save(Worldfile *wf, int section, const char *keyword) const;
252 
253  void Zero() { x = y = z = 0.0; }
254 };
255 
257 class Pose {
258 public:
259  meters_t x, y, z;
261 
262  Pose(meters_t x, meters_t y, meters_t z, radians_t a) : x(x), y(y), z(z), a(a) { /*empty*/}
263 
264  Pose() : x(0.0), y(0.0), z(0.0), a(0.0) { /*empty*/}
265 
266  virtual ~Pose() {}
269  static Pose Random(meters_t xmin, meters_t xmax, meters_t ymin, meters_t ymax)
270  {
271  return Pose(xmin + drand48() * (xmax - xmin), ymin + drand48() * (ymax - ymin), 0,
272  normalize(drand48() * (2.0 * M_PI)));
273  }
274 
278  virtual void Print(const char *prefix) const
279  {
280  printf("%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n", prefix, x, y, z, a);
281  }
282 
283  std::string String() const
284  {
285  char buf[256];
286  snprintf(buf, 256, "[ %.3f %.3f %.3f %.3f ]", x, y, z, a);
287  return std::string(buf);
288  }
289 
291  bool IsZero() const { return (!(x || y || z || a)); }
293  void Zero() { x = y = z = a = 0.0; }
294  Pose &Load(Worldfile *wf, int section, const char *keyword);
295  void Save(Worldfile *wf, int section, const char *keyword);
296 
297  inline Pose operator+(const Pose &p) const
298  {
299  const double cosa = cos(a);
300  const double sina = sin(a);
301 
302  return Pose(x + p.x * cosa - p.y * sina, y + p.x * sina + p.y * cosa, z + p.z,
303  normalize(a + p.a));
304  }
305 
307  bool operator<(const Pose &p) const
308  {
309  // return( hypot( y, x ) < hypot( otHer.y, other.x ));
310  // just compare the squared values to avoid the sqrt()
311  return ((y * y + x * x) < (p.y * p.y + p.x * p.x));
312  }
313 
314  bool operator==(const Pose &other) const
315  {
316  return (x == other.x && y == other.y && z == other.z && a == other.a);
317  }
318 
319  bool operator!=(const Pose &other) const
320  {
321  return (x != other.x || y != other.y || z != other.z || a != other.a);
322  }
323 
324  meters_t Distance(const Pose &other) const { return hypot(x - other.x, y - other.y); }
325 };
326 
328 public:
333 
334  RaytraceResult() : pose(), mod(NULL), color(), range(0.0) {}
336  : pose(pose), mod(mod), color(color), range(range)
337  {
338  }
339 };
340 
343 class Velocity : public Pose {
344 public:
350  Velocity(double x, double y, double z, double a) : Pose(x, y, z, a) { /*empty*/}
351 
352  Velocity() { /*empty*/}
353 
354  Velocity &Load(Worldfile *wf, int section, const char *keyword)
355  {
356  Pose::Load(wf, section, keyword);
357  return *this;
358  }
359 
365  virtual void Print(const char *prefix) const
366  {
367  if (prefix)
368  printf("%s", prefix);
369 
370  printf("velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n", x, y, z, a);
371  }
372 };
373 
376 class Geom {
377 public:
380 
386  void Print(const char *prefix) const
387  {
388  if (prefix)
389  printf("%s", prefix);
390 
391  printf("geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n", pose.x, pose.y, pose.a, size.x,
392  size.y);
393  }
394 
396  Geom() : pose(), size() {}
398  Geom(const Pose &p, const Size &s) : pose(p), size(s) {}
399  void Zero()
400  {
401  pose.Zero();
402  size.Zero();
403  }
404 };
405 
407 class Bounds {
408 public:
410  double min;
412  double max;
413 
414  Bounds() : min(0), max(0) { /* empty*/}
415  Bounds(double min, double max) : min(min), max(max) { /* empty*/}
416 
417  Bounds &Load(Worldfile *wf, int section, const char *keyword);
418 
420  double Constrain(double value);
421 };
422 
424 class bounds3d_t {
425 public:
432 
433  bounds3d_t() : x(), y(), z() {}
434  bounds3d_t(const Bounds &x, const Bounds &y, const Bounds &z) : x(x), y(y), z(z) {}
435 };
436 
438 typedef struct {
441 } fov_t;
442 
444 class point_t {
445 public:
448  point_t() : x(0.0), y(0.0) {}
449  bool operator+=(const point_t &other) { return ((x += other.x) && (y += other.y)); }
451  bool operator<(const point_t &other) const
452  {
453  if (x < other.x)
454  return true;
455  if (other.x < x)
456  return false;
457  return y < other.y;
458  }
459 
460  bool operator==(const point_t &other) const { return ((x == other.x) && (y == other.y)); }
461 };
462 
464 class point3_t {
465 public:
468  point3_t() : x(0.0), y(0.0), z(0.0) {}
469 };
470 
472 class point_int_t {
473 public:
474  int x, y;
475  point_int_t(int x, int y) : x(x), y(y) {}
476  point_int_t() : x(0), y(0) {}
478  bool operator<(const point_int_t &other) const
479  {
480  if (x < other.x)
481  return true;
482  if (other.x < x)
483  return false;
484  return y < other.y;
485  }
486 
487  bool operator==(const point_int_t &other) const { return ((x == other.x) && (y == other.y)); }
488 };
489 
492 point_t *unit_square_points_create();
493 
496 namespace Gl {
497 void pose_shift(const Pose &pose);
498 void pose_inverse_shift(const Pose &pose);
499 void coord_shift(double x, double y, double z, double a);
500 void draw_grid(bounds3d_t vol);
502 void draw_string(float x, float y, float z, const char *string);
503 void draw_string_multiline(float x, float y, float w, float h, const char *string, Fl_Align align);
504 void draw_speech_bubble(float x, float y, float z, const char *str);
505 void draw_octagon(float w, float h, float m);
506 void draw_octagon(float x, float y, float w, float h, float m);
507 void draw_vector(double x, double y, double z);
508 void draw_origin(double len);
509 void draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset,
510  float min, float max);
511 void draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset);
513 void draw_centered_rect(float x, float y, float dx, float dy);
514 } // namespace Gl
515 
516 void RegisterModels();
517 
520 class Visualizer {
521 private:
522  const std::string menu_name;
523  const std::string worldfile_name;
524 
525 public:
526  Visualizer(const std::string &menu_name, const std::string &worldfile_name)
527  : menu_name(menu_name), worldfile_name(worldfile_name)
528  {
529  }
530 
531  virtual ~Visualizer(void) {}
532  virtual void Visualize(Model *mod, Camera *cam) = 0;
533 
534  const std::string &GetMenuName() { return menu_name; }
535  const std::string &GetWorldfileName() { return worldfile_name; }
536 };
537 
540 typedef int (*model_callback_t)(Model *mod, void *user);
541 
542 typedef int (*world_callback_t)(World *world, void *user);
543 
545 double constrain(double val, double minval, double maxval);
546 
547 typedef struct {
548  int enabled;
553  double duty_cycle;
555 
557 typedef struct {
560 } rotrect_t;
561 
564 int polys_from_image_file(const std::string &filename, std::vector<std::vector<point_t> > &polys);
565 
569 typedef bool (*ray_test_func_t)(Model *candidate, const Model *finder, const void *arg);
570 
573 #define VAR(V, init) __typeof(init) V = (init)
574 
575 //#define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();++I)
576 
577 // NOTE:
578 // this version assumes the container is not modified in the loop,
579 // which I think is true everywhere it is used in Stage
580 #define FOR_EACH(I, C) for (VAR(I, (C).begin()), ite = (C).end(); (I) != ite; ++(I))
581 
584 template <class T, class C> void EraseAll(T thing, C &cont)
585 {
586  cont.erase(std::remove(cont.begin(), cont.end(), thing), cont.end());
587 }
588 
589 // Error macros - output goes to stderr
590 #define PRINT_ERR(m) fprintf(stderr, "\033[41merr\033[0m: " m " (%s %s)\n", __FILE__, __FUNCTION__)
591 #define PRINT_ERR1(m, a) \
592  fprintf(stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, __FILE__, __FUNCTION__)
593 #define PRINT_ERR2(m, a, b) \
594  fprintf(stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, b, __FILE__, __FUNCTION__)
595 #define PRINT_ERR3(m, a, b, c) \
596  fprintf(stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
597 #define PRINT_ERR4(m, a, b, c, d) \
598  fprintf(stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
599 #define PRINT_ERR5(m, a, b, c, d, e) \
600  fprintf(stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)
601 
602 // Warning macros
603 #define PRINT_WARN(m) printf("\033[44mwarn\033[0m: " m " (%s %s)\n", __FILE__, __FUNCTION__)
604 #define PRINT_WARN1(m, a) printf("\033[44mwarn\033[0m: " m " (%s %s)\n", a, __FILE__, __FUNCTION__)
605 #define PRINT_WARN2(m, a, b) \
606  printf("\033[44mwarn\033[0m: " m " (%s %s)\n", a, b, __FILE__, __FUNCTION__)
607 #define PRINT_WARN3(m, a, b, c) \
608  printf("\033[44mwarn\033[0m: " m " (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
609 #define PRINT_WARN4(m, a, b, c, d) \
610  printf("\033[44mwarn\033[0m: " m " (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
611 #define PRINT_WARN5(m, a, b, c, d, e) \
612  printf("\033[44mwarn\033[0m: " m " (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)
613 
614 // Message macros
615 #ifdef DEBUG
616 #define PRINT_MSG(m) printf("Stage: " m " (%s %s)\n", __FILE__, __FUNCTION__)
617 #define PRINT_MSG1(m, a) printf("Stage: " m " (%s %s)\n", a, __FILE__, __FUNCTION__)
618 #define PRINT_MSG2(m, a, b) printf("Stage: " m " (%s %s)\n", a, b, __FILE__, __FUNCTION__)
619 #define PRINT_MSG3(m, a, b, c) printf("Stage: " m " (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
620 #define PRINT_MSG4(m, a, b, c, d) \
621  printf("Stage: " m " (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
622 #define PRINT_MSG5(m, a, b, c, d, e) \
623  printf("Stage: " m " (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)
624 #else
625 #define PRINT_MSG(m) printf("Stage: " m "\n")
626 #define PRINT_MSG1(m, a) printf("Stage: " m "\n", a)
627 #define PRINT_MSG2(m, a, b) printf("Stage: " m "\n,", a, b)
628 #define PRINT_MSG3(m, a, b, c) printf("Stage: " m "\n", a, b, c)
629 #define PRINT_MSG4(m, a, b, c, d) printf("Stage: " m "\n", a, b, c, d)
630 #define PRINT_MSG5(m, a, b, c, d, e) printf("Stage: " m "\n", a, b, c, d, e)
631 #endif
632 
633 // DEBUG macros
634 #ifdef DEBUG
635 #define PRINT_DEBUG(m) printf("debug: " m " (%s %s)\n", __FILE__, __FUNCTION__)
636 #define PRINT_DEBUG1(m, a) printf("debug: " m " (%s %s)\n", a, __FILE__, __FUNCTION__)
637 #define PRINT_DEBUG2(m, a, b) printf("debug: " m " (%s %s)\n", a, b, __FILE__, __FUNCTION__)
638 #define PRINT_DEBUG3(m, a, b, c) printf("debug: " m " (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
639 #define PRINT_DEBUG4(m, a, b, c, d) \
640  printf("debug: " m " (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
641 #define PRINT_DEBUG5(m, a, b, c, d, e) \
642  printf("debug: " m " (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)
643 #else
644 #define PRINT_DEBUG(m)
645 #define PRINT_DEBUG1(m, a)
646 #define PRINT_DEBUG2(m, a, b)
647 #define PRINT_DEBUG3(m, a, b, c)
648 #define PRINT_DEBUG4(m, a, b, c, d)
649 #define PRINT_DEBUG5(m, a, b, c, d, e)
650 #endif
651 
652 class Block;
653 class Model;
654 
655 // ANCESTOR CLASS
657 class Ancestor {
658  friend class Canvas; // allow Canvas access to our private members
659 
660 protected:
662  std::map<std::string, unsigned int> child_type_counts;
663 
664  std::vector<Model *> children;
665 
666  bool debug;
667 
669  std::map<std::string, void *> props;
670 
671  std::string token;
672 
673  Ancestor &Load(Worldfile *wf, int section);
674  void Save(Worldfile *wf, int section);
675 
676 public:
677  Ancestor();
678  virtual ~Ancestor();
679 
681  std::vector<Model *> &GetChildren() { return children; }
683  void ForEachDescendant(model_callback_t func, void *arg);
684 
685  virtual void AddChild(Model *mod);
686  virtual void RemoveChild(Model *mod);
687  virtual Pose GetGlobalPose() const;
688 
689  const char *Token() const { return token.c_str(); }
690  const std::string &TokenStr() const { return token; }
691  virtual void SetToken(const std::string &str)
692  {
693  // printf( "Ancestor::SetToken( %s )\n", str.c_str() );
694 
695  if (str.size() > 0)
696  token = str;
697  else
698  PRINT_WARN("Ancestor::SetToken() called with zero length string. Ignored.");
699  }
700 
702  void SetProperty(std::string &key, void *value) { props[key] = value; }
704  void *GetProperty(std::string &key)
705  {
706  std::map<std::string, void *>::iterator it = props.find(key);
707  return (it == props.end() ? NULL : it->second);
708  }
709 };
710 
711 class Ray {
712 public:
713  Ray(const Model *mod, const Pose &origin, const meters_t range, const ray_test_func_t func,
714  const void *arg, const bool ztest)
716  {
717  }
718 
719  Ray() : mod(NULL), origin(0, 0, 0, 0), range(0), func(NULL), arg(NULL), ztest(true) {}
720  const Model *mod;
724  const void *arg;
725  bool ztest;
726 };
727 
728 // defined in stage_internal.hh
729 class Region;
730 class SuperRegion;
731 class BlockGroup;
732 class PowerPack;
733 
734 class LogEntry {
735  usec_t timestamp;
736  Model *mod;
737  Pose pose;
738 
739 public:
740  LogEntry(usec_t timestamp, Model *mod);
741 
743  static std::vector<LogEntry> log;
744 
746  static size_t Count() { return log.size(); }
748  static void Clear() { log.clear(); }
750  static void Print();
751 };
752 
753 class CtrlArgs {
754 public:
755  std::string worldfile;
756  std::string cmdline;
757 
758  CtrlArgs(std::string w, std::string c) : worldfile(w), cmdline(c) {}
759 };
760 
761 class ModelPosition;
762 
764 class World : public Ancestor {
765 public:
766  friend class Block;
767  friend class Model; // allow access to private members
768  friend class ModelFiducial;
769  friend class Canvas;
770  friend class WorkerThread;
771 
772 public:
775  static std::vector<std::string> args;
776  static std::string ctrlargs;
777 
778 private:
779  static std::set<World *> world_set;
780  static bool quit_all;
781  static void UpdateCb(World *world);
782  static unsigned int next_id;
783 
784  bool destroy;
785  bool dirty;
786 
788  std::set<Model *> models;
789 
791  std::map<std::string, Model *> models_by_name;
792 
794  std::map<int, Model *> models_by_wfentity;
795 
798  std::vector<Model *> models_with_fiducials;
799 
800  struct ltx {
801  bool operator()(const Model *a, const Model *b) const;
802  };
803 
804  struct lty {
805  bool operator()(const Model *a, const Model *b) const;
806  };
807 
810  std::set<Model *, ltx> models_with_fiducials_byx;
811 
814  std::set<Model *, lty> models_with_fiducials_byy;
815 
817  void FiducialInsert(Model *mod)
818  {
819  FiducialErase(mod); // make sure it's not there already
820  models_with_fiducials.push_back(mod);
821  }
822 
824  void FiducialErase(Model *mod) { EraseAll(mod, models_with_fiducials); }
826  void LoadWorldPostHook();
827 
828  double ppm;
829  bool quit;
830  bool show_clock;
831  unsigned int show_clock_interval;
832 
833  //--- thread sync ----
834  pthread_mutex_t sync_mutex;
835  unsigned int threads_working;
836  pthread_cond_t threads_start_cond;
837  pthread_cond_t threads_done_cond;
838  int total_subs;
839  unsigned int worker_threads;
840 
841 protected:
842  std::list<std::pair<world_callback_t, void *> >
845  bool graphics;
846 
847  std::set<Option *> option_table;
848  std::list<PowerPack *>
852  std::list<float *> ray_list;
854  std::map<point_int_t, SuperRegion *> superregions;
855 
856  uint64_t updates;
858 
859  void CallUpdateCallbacks();
860 
861 public:
862  uint64_t UpdateCount() { return updates; }
863  bool paused;
864 
865  virtual void Start() { paused = false; }
866  virtual void Stop() { paused = true; }
867  virtual void TogglePause() { paused ? Start() : Stop(); }
868  bool Paused() const { return (paused); }
872  virtual void Redraw(void) {} // does nothing
873  std::vector<point_int_t> rt_cells;
874  std::vector<point_int_t> rt_candidate_cells;
875 
876  static const int DEFAULT_PPM = 50; //<! default resolution in pixels per meter
877 
880  void AddUpdateCallback(world_callback_t cb, void *user);
881 
884  int RemoveUpdateCallback(world_callback_t cb, void *user);
885 
887  void Log(Model *mod);
888 
890  void NeedRedraw() { dirty = true; }
893 
896  virtual std::string ClockString(void) const;
897 
898  Model *CreateModel(Model *parent, const std::string &typestr);
899 
900  void LoadModel(Worldfile *wf, int entity);
901  void LoadBlock(Worldfile *wf, int entity);
902  void LoadBlockGroup(Worldfile *wf, int entity);
903  void LoadSensor(Worldfile *wf, int entity);
904 
905  virtual Model *RecentlySelectedModel() const { return NULL; }
908  void MapPoly(const std::vector<point_int_t> &poly, Block *block, unsigned int layer);
909 
910  SuperRegion *AddSuperRegion(const point_int_t &coord);
913 
916  int32_t MetersToPixels(meters_t x) const { return (int32_t)floor(x * ppm); }
919  {
920  return point_int_t(MetersToPixels(pt.x), MetersToPixels(pt.y));
921  }
922 
924  virtual void PushColor(Color col) { /* do nothing */ (void)col; }
925  virtual void PushColor(double r, double g, double b, double a)
926  { /* do nothing */
927  (void)r;
928  (void)g;
929  (void)b;
930  (void)a;
931  }
932 
933  virtual void PopColor() { /* do nothing */}
934 
937 
939  RaytraceResult Raytrace(const Ray &ray);
940 
941  RaytraceResult Raytrace(const Pose &pose, const meters_t range, const ray_test_func_t func,
942  const Model *finder, const void *arg, const bool ztest);
943 
944  void Raytrace(const Pose &gpose, // global pose
945  const meters_t range, const radians_t fov, const ray_test_func_t func,
946  const Model *model, const void *arg, const bool ztest,
947  std::vector<RaytraceResult> &results);
948 
950  inline void Extend(point3_t pt);
951 
952  virtual void AddModel(Model *mod);
953  virtual void RemoveModel(Model *mod);
954 
955  void AddModelName(Model *mod, const std::string &name);
956 
957  void AddPowerPack(PowerPack *pp);
958  void RemovePowerPack(PowerPack *pp);
959 
960  void ClearRays();
961 
963  void RecordRay(double x1, double y1, double x2, double y2);
964 
967  bool PastQuitTime();
968 
969  static void *update_thread_entry(std::pair<World *, int> *info);
970 
971  class Event {
972  public:
974  : time(time), mod(mod), cb(cb), arg(arg)
975  {
976  }
977 
981  void *arg;
982 
985  bool operator<(const Event &other) const;
986  };
987 
989  std::vector<std::priority_queue<Event> > event_queues;
990 
992  std::vector<std::queue<Model *> > pending_update_callbacks;
993 
1005  void Enqueue(unsigned int queue_num, usec_t delay, Model *mod, model_callback_t cb, void *arg)
1006  {
1007  event_queues[queue_num].push(Event(sim_time + delay, mod, cb, arg));
1008  }
1009 
1011  std::set<Model *> active_energy;
1012  void EnableEnergy(Model *m) { active_energy.insert(m); }
1013  void DisableEnergy(Model *m) { active_energy.erase(m); }
1015  std::set<ModelPosition *> active_velocity;
1016 
1019 
1024 
1026  void ConsumeQueue(unsigned int queue_num);
1027 
1030  unsigned int GetEventQueue(Model *mod) const;
1031 
1032 public:
1034  static bool UpdateAll();
1035 
1042  static void Run();
1043 
1044  World(const std::string &name = "MyWorld", double ppm = DEFAULT_PPM);
1045 
1046  virtual ~World();
1047 
1049  usec_t SimTimeNow(void) const { return sim_time; }
1052  Worldfile *GetWorldFile() { return wf; }
1056  virtual bool IsGUI() const { return false; }
1064  virtual bool Load(const std::string &worldfile_path);
1065 
1077  virtual bool Load(std::istream &world_content, const std::string &worldfile_path = std::string());
1078 
1079  virtual void UnLoad();
1080 
1081  virtual void Reload();
1082 
1085  virtual bool Save(const char *filename);
1086 
1090  virtual bool Update(void);
1091 
1095  bool TestQuit() const { return (quit || quit_all); }
1097  void Quit() { quit = true; }
1099  void QuitAll() { quit_all = true; }
1101  void CancelQuit() { quit = false; }
1103  void CancelQuitAll() { quit_all = false; }
1104  void TryCharge(PowerPack *pp, const Pose &pose);
1105 
1108  double Resolution() const { return ppm; }
1111  Model *GetModel(const std::string &name) const;
1112 
1114  const std::set<Model *> GetAllModels() const { return models; }
1116  const bounds3d_t &GetExtent() const { return extent; }
1118  uint64_t GetUpdateCount() const { return updates; }
1120  void RegisterOption(Option *opt);
1121 
1123  void ShowClock(bool enable) { show_clock = enable; }
1125  Model *GetGround() { return ground; }
1126 };
1127 
1128 class Block {
1129  friend class BlockGroup;
1130  friend class Model;
1131  friend class SuperRegion;
1132  friend class World;
1133  friend class Canvas;
1134  friend class Cell;
1135 
1136 public:
1140  Block(BlockGroup *group, const std::vector<point_t> &pts, const Bounds &zrange);
1141 
1143  Block(BlockGroup *group, Worldfile *wf, int entity);
1144 
1145  ~Block();
1146 
1148  void Map(unsigned int layer);
1149 
1151  void UnMap(unsigned int layer);
1152 
1154  void DrawSolid(bool topview);
1155 
1157  void DrawFootPrint();
1158 
1160  void Translate(double x, double y);
1161 
1163  double CenterX();
1164 
1166  double CenterY();
1167 
1169  void SetCenterX(double y);
1170 
1172  void SetCenterY(double y);
1173 
1175  void SetCenter(double x, double y);
1176 
1178  void SetZ(double min, double max);
1179 
1180  void AppendTouchingModels(std::set<Model *> &touchers);
1181 
1183  Model *TestCollision();
1184 
1185  void Load(Worldfile *wf, int entity);
1186 
1187  void Rasterize(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth,
1188  meters_t cellheight);
1189 
1191 private:
1192  std::vector<point_t> pts;
1193  Bounds local_z;
1194  Bounds global_z;
1195 
1199  std::vector<Cell *> rendered_cells[2];
1200 
1201  void DrawTop();
1202  void DrawSides();
1203 };
1204 
1205 class BlockGroup {
1206  friend class Model;
1207  friend class Block;
1208  friend class World;
1209  friend class SuperRegion;
1210 
1211 private:
1212  std::vector<Block> blocks;
1213  int displaylist;
1214 
1215 public:
1217 
1218 private:
1219  void AppendBlock(const Block &block);
1220 
1221  void CalcSize();
1222  void Clear();
1224  void AppendTouchingModels(std::set<Model *> &touchers);
1225 
1228  Model *TestCollision();
1229 
1231  void Map(unsigned int layer);
1233  void UnMap(unsigned int layer);
1234 
1237  void LoadBitmap(const std::string &bitmapfile, Worldfile *wf);
1238 
1240  void LoadBlock(Worldfile *wf, int entity);
1241 
1243  void Rasterize(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth,
1244  meters_t cellheight);
1245 
1247  void DrawSolid(const Geom &geom);
1248 
1252  void BuildDisplayList();
1253 
1255  void CallDisplayList();
1256 
1257 public:
1258  explicit BlockGroup(Model &mod);
1259  ~BlockGroup();
1260 
1261  uint32_t GetCount() const { return blocks.size(); }
1262  const Block &GetBlock(unsigned int index) const { return blocks[index]; }
1263  Block &GetBlockMutable(unsigned int index) { return blocks[index]; }
1265  bounds3d_t BoundingBox() const;
1266 
1268  void DrawFootPrint(const Geom &geom);
1269 };
1270 
1271 class Camera {
1272 protected:
1273  double _pitch; // left-right (about y)
1274  double _yaw; // up-down (about x)
1275  double _x, _y, _z;
1276 
1277 public:
1278  Camera() : _pitch(0), _yaw(0), _x(0), _y(0), _z(0) {}
1279  virtual ~Camera() {}
1280  virtual void Draw(void) const = 0;
1281  virtual void SetProjection(void) const = 0;
1282 
1283  double yaw(void) const { return _yaw; }
1284  double pitch(void) const { return _pitch; }
1285  double x(void) const { return _x; }
1286  double y(void) const { return _y; }
1287  double z(void) const { return _z; }
1288  virtual void reset() = 0;
1289  virtual void Load(Worldfile *wf, int sec) = 0;
1290 
1291  // TODO data should be passed in somehow else. (at least min/max stuff)
1292  // virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max )
1293  // const = 0;
1294 };
1295 
1296 class PerspectiveCamera : public Camera {
1297 private:
1298  double _z_near;
1299  double _z_far;
1300  double _vert_fov;
1301  double _horiz_fov;
1302  double _aspect;
1303 
1304 public:
1305  PerspectiveCamera(void);
1306 
1307  virtual void Draw(void) const;
1308  virtual void SetProjection(void) const;
1309  // void SetProjection( double aspect ) const;
1310  void update(void);
1311 
1312  void strafe(double amount);
1313  void forward(double amount);
1314 
1315  void setPose(double x, double y, double z)
1316  {
1317  _x = x;
1318  _y = y;
1319  _z = z;
1320  }
1321  void addPose(double x, double y, double z)
1322  {
1323  _x += x;
1324  _y += y;
1325  _z += z;
1326  if (_z < 0.1)
1327  _z = 0.1;
1328  }
1329  void move(double x, double y, double z);
1330  void setFov(double horiz_fov, double vert_fov)
1331  {
1332  _horiz_fov = horiz_fov;
1333  _vert_fov = vert_fov;
1334  }
1336  void setAspect(double aspect) { _aspect = aspect; }
1337  void setYaw(double yaw) { _yaw = yaw; }
1338  double horizFov(void) const { return _horiz_fov; }
1339  double vertFov(void) const { return _vert_fov; }
1340  void addYaw(double yaw) { _yaw += yaw; }
1341  void setPitch(double pitch) { _pitch = pitch; }
1342  void addPitch(double pitch)
1343  {
1344  _pitch += pitch;
1345  if (_pitch < 0)
1346  _pitch = 0;
1347  else if (_pitch > 180)
1348  _pitch = 180;
1349  }
1350 
1351  double realDistance(double z_buf_val) const
1352  {
1353  return _z_near * _z_far / (_z_far - z_buf_val * (_z_far - _z_near));
1354  }
1355  void scroll(double dy) { _z += dy; }
1356  double nearClip(void) const { return _z_near; }
1357  double farClip(void) const { return _z_far; }
1358  void setClip(double near, double far)
1359  {
1360  _z_far = far;
1361  _z_near = near;
1362  }
1363 
1364  void reset()
1365  {
1366  setPitch(70);
1367  setYaw(0);
1368  }
1369 
1370  void Load(Worldfile *wf, int sec);
1371  void Save(Worldfile *wf, int sec);
1372 };
1373 
1374 class OrthoCamera : public Camera {
1375 private:
1376  double _scale;
1377  double _pixels_width;
1378  double _pixels_height;
1379  double _y_min;
1380  double _y_max;
1381 
1382 public:
1383  OrthoCamera(void) : _scale(15), _pixels_width(0), _pixels_height(0), _y_min(0), _y_max(0) {}
1384  virtual void Draw() const;
1385 
1386  virtual void SetProjection(double pixels_width, double pixels_height, double y_min, double y_max);
1387 
1388  virtual void SetProjection(void) const;
1389 
1390  void move(double x, double y);
1391 
1392  void setYaw(double yaw) { _yaw = yaw; }
1393  void setPitch(double pitch) { _pitch = pitch; }
1394  void addYaw(double yaw) { _yaw += yaw; }
1395  void addPitch(double pitch)
1396  {
1397  _pitch += pitch;
1398  if (_pitch > 90)
1399  _pitch = 90;
1400  else if (_pitch < 0)
1401  _pitch = 0;
1402  }
1403 
1404  void setScale(double scale) { _scale = scale; }
1405  void setPose(double x, double y)
1406  {
1407  _x = x;
1408  _y = y;
1409  }
1410 
1411  void scale(double scale, double shift_x = 0, double h = 0, double shift_y = 0, double w = 0);
1412  void reset(void) { _pitch = _yaw = 0; }
1413  double scale() const { return _scale; }
1414  void Load(Worldfile *wf, int sec);
1415  void Save(Worldfile *wf, int sec);
1416 };
1417 
1421 class WorldGui : public World, public Fl_Window {
1422  friend class Canvas;
1423  friend class ModelCamera;
1424  friend class Model;
1425  friend class Option;
1426 
1427 private:
1428  Canvas *canvas;
1429  std::vector<Option *> drawOptions;
1430  FileManager *fileMan;
1431  std::vector<usec_t> interval_log;
1432 
1435  double speedup;
1436 
1437  bool confirm_on_quit;
1438 
1439  Fl_Menu_Bar *mbar;
1440  OptionsDlg *oDlg;
1441  bool pause_time;
1442  std::string caption_prefix;
1443 
1446  usec_t real_time_interval;
1447 
1449  usec_t real_time_now;
1450 
1453  usec_t real_time_recorded;
1454 
1456  uint64_t timing_interval;
1457 
1458  // static callback functions
1459  static void windowCb(Fl_Widget *w, WorldGui *wg);
1460  static void fileLoadCb(Fl_Widget *w, WorldGui *wg);
1461  static void fileSaveCb(Fl_Widget *w, WorldGui *wg);
1462  static void fileSaveAsCb(Fl_Widget *w, WorldGui *wg);
1463  static void fileExitCb(Fl_Widget *w, WorldGui *wg);
1464  static void viewOptionsCb(OptionsDlg *oDlg, WorldGui *wg);
1465  static void optionsDlgCb(OptionsDlg *oDlg, WorldGui *wg);
1466  static void helpAboutCb(Fl_Widget *w, WorldGui *wg);
1467  static void pauseCb(Fl_Widget *w, WorldGui *wg);
1468  static void onceCb(Fl_Widget *w, WorldGui *wg);
1469  static void fasterCb(Fl_Widget *w, WorldGui *wg);
1470  static void slowerCb(Fl_Widget *w, WorldGui *wg);
1471  static void realtimeCb(Fl_Widget *w, WorldGui *wg);
1472  static void fasttimeCb(Fl_Widget *w, WorldGui *wg);
1473  static void resetViewCb(Fl_Widget *w, WorldGui *wg);
1474  static void moreHelptCb(Fl_Widget *w, WorldGui *wg);
1475 
1476  // GUI functions
1477  bool saveAsDialog();
1478  bool closeWindowQuery();
1479 
1480  virtual void AddModel(Model *mod);
1481 
1482  void SetTimeouts();
1483 
1485  void LoadWorldGuiPostHook(usec_t load_start_time);
1486 
1487 protected:
1488  virtual void PushColor(Color col);
1489  virtual void PushColor(double r, double g, double b, double a);
1490  virtual void PopColor();
1491 
1492  void DrawOccupancy() const;
1493  void DrawVoxels() const;
1494 
1495 public:
1496  WorldGui(int width, int height, const char *caption = NULL);
1497  ~WorldGui();
1498 
1500  virtual void Redraw(void);
1501 
1502  virtual std::string ClockString() const;
1503  virtual bool Update();
1504  virtual bool Load(const std::string &worldfile_path);
1505  virtual bool Load(std::istream &world_content, const std::string &worldfile_path = std::string());
1506 
1507  virtual void UnLoad();
1508  virtual bool Save(const char *filename);
1509  virtual bool IsGUI() const { return true; }
1510  virtual Model *RecentlySelectedModel() const;
1511 
1512  virtual void Start();
1513  virtual void Stop();
1514 
1515  usec_t RealTimeNow(void) const;
1516 
1517  void DrawBoundingBoxTree();
1518 
1519  Canvas *GetCanvas(void) const { return canvas; }
1521  void Show();
1522 
1524  std::string EnergyString(void) const;
1525  virtual void RemoveChild(Model *mod);
1526 
1527  bool IsTopView();
1528 };
1529 
1530 class StripPlotVis : public Visualizer {
1531 private:
1532  // Model* mod;
1533  float *data;
1534  size_t len;
1535  size_t count;
1536  // unsigned int index;
1537  float x, y, w, h, min, max;
1538  Color fgcolor, bgcolor;
1539 
1540 public:
1541  StripPlotVis(float x, float y, float w, float h, size_t len, Color fgcolor, Color bgcolor,
1542  const char *name, const char *wfname);
1543  virtual ~StripPlotVis();
1544  virtual void Visualize(Model *mod, Camera *cam);
1545  void AppendValue(float value);
1546 };
1547 
1548 class PowerPack {
1549  friend class WorldGui;
1550  friend class Canvas;
1551 
1552 protected:
1553  class DissipationVis : public Visualizer {
1554  private:
1555  unsigned int columns, rows;
1556  meters_t width, height;
1557 
1558  std::vector<joules_t> cells;
1559 
1560  joules_t peak_value;
1561  double cellsize;
1562 
1563  static joules_t global_peak_value;
1564 
1565  public:
1566  DissipationVis(meters_t width, meters_t height, meters_t cellsize);
1567 
1568  virtual ~DissipationVis();
1569  virtual void Visualize(Model *mod, Camera *cam);
1570 
1571  void Accumulate(meters_t x, meters_t y, joules_t amount);
1573 
1576 
1579 
1582 
1585 
1587  bool charging;
1588 
1591 
1592  // these are used to visualize the power draw
1596 
1597 public:
1602 
1603 public:
1604  explicit PowerPack(Model *mod);
1605  ~PowerPack();
1606 
1608  void Visualize(Camera *cam);
1609 
1611  joules_t RemainingCapacity() const;
1612 
1614  void Add(joules_t j);
1615 
1617  void Subtract(joules_t j);
1618 
1620  void TransferTo(PowerPack *dest, joules_t amount);
1621 
1622  double ProportionRemaining() const { return (stored / capacity); }
1625  void Print(const char *prefix) const
1626  {
1627  if (prefix)
1628  printf("%s", prefix);
1629 
1630  printf("PowerPack %.2f/%.2f J\n", stored, capacity);
1631  }
1632 
1633  joules_t GetStored() const;
1634  joules_t GetCapacity() const;
1635  joules_t GetDissipated() const;
1636  void SetCapacity(joules_t j);
1637  void SetStored(joules_t j);
1638 
1640  bool GetCharging() const { return charging; }
1641  void ChargeStart() { charging = true; }
1642  void ChargeStop() { charging = false; }
1644  void Dissipate(joules_t j);
1645 
1647  void Dissipate(joules_t j, const Pose &p);
1648 };
1649 
1651 class Model : public Ancestor {
1652  friend class Ancestor;
1653  friend class World;
1654  friend class World::Event;
1655  friend class WorldGui;
1656  friend class Canvas;
1657  friend class Block;
1658  friend class Region;
1659  friend class BlockGroup;
1660  friend class PowerPack;
1661  friend class Ray;
1662  friend class ModelFiducial;
1663 
1664 private:
1666  static uint32_t count;
1667  static std::map<id_t, Model *> modelsbyid;
1668 
1670  bool mapped;
1671 
1672  std::vector<Option *> drawOptions;
1673  const std::vector<Option *> &getOptions() const { return drawOptions; }
1674 protected:
1677  bool alwayson;
1678 
1680 
1684 
1687 public:
1688  class cb_t {
1689  public:
1691  void *arg;
1692 
1693  cb_t(model_callback_t cb, void *arg) : callback(cb), arg(arg) {}
1694  cb_t(world_callback_t cb, void *arg) : callback(NULL), arg(arg) { (void)cb; }
1695  cb_t() : callback(NULL), arg(NULL) {}
1697  bool operator<(const cb_t &other) const
1698  {
1699  if (callback == other.callback)
1700  return (arg < other.arg);
1701  // else
1702  return callback < other.callback;
1703  }
1704 
1706  bool operator==(const cb_t &other) const { return (callback == other.callback); }
1707  };
1708 
1709  class Flag {
1710  private:
1711  Color color;
1712  double size;
1713  int displaylist;
1714 
1715  public:
1716  void SetColor(const Color &col);
1717  void SetSize(double sz);
1718 
1719  Color GetColor() { return color; }
1720  double GetSize() { return size; }
1721  Flag(const Color &color, double size);
1722  Flag *Nibble(double portion);
1723 
1726  void Draw(GLUquadric *quadric);
1727  };
1728 
1729  typedef enum {
1742  // CB_POSTUPDATE,
1745 
1746 protected:
1750  std::vector<std::set<cb_t> > callbacks;
1751 
1754 
1759 
1763  bool disabled;
1764 
1766  std::list<Visualizer *> cv_list;
1767 
1769  std::list<Flag *> flag_list;
1770 
1773  double friction;
1774 
1778 
1780  class GuiState {
1781  public:
1782  bool grid;
1783  bool move;
1784  bool nose;
1785  bool outline;
1786 
1787  GuiState();
1789  } gui;
1790 
1792 
1794  uint32_t id;
1798  bool log_state;
1801 
1804 
1808 
1811 
1814  std::list<PowerPack *> pps_charging;
1815 
1817  class RasterVis : public Visualizer {
1818  private:
1819  uint8_t *data;
1820  unsigned int width, height;
1821  meters_t cellwidth, cellheight;
1822  std::vector<point_t> pts;
1823 
1824  public:
1825  RasterVis();
1826  virtual ~RasterVis(void) {}
1827  virtual void Visualize(Model *mod, Camera *cam);
1828 
1829  void SetData(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth,
1830  meters_t cellheight);
1831 
1832  int subs; //< the number of subscriptions to this model
1833  int used; //< the number of connections to this model
1834 
1835  void AddPoint(meters_t x, meters_t y);
1836  void ClearPts();
1837 
1839 
1841  std::string say_string;
1842 
1844 
1845  bool stall;
1846  int subs;
1852 
1854  class TrailItem {
1855  public:
1859 
1860  TrailItem() : time(0), pose(), color() {}
1861  // TrailItem( usec_t time, Pose pose, Color color )
1862  //: time(time), pose(pose), color(color){}
1863  };
1864 
1866  std::vector<TrailItem> trail;
1867 
1869  unsigned int trail_index;
1870 
1871 // /** The maxiumum length of the trail drawn. Default is 20, but can
1872 // be set in the world file using the trail_length model
1873 // property. */
1874 // unsigned int trail_length;
1875 
1877  uint64_t trail_interval;
1878 
1880  void UpdateTrail();
1881 
1882  // model_type_t type;
1883  const std::string type;
1886  unsigned int event_queue_num;
1887  bool used;
1888 
1890 
1894 
1898 
1903 
1904 public:
1905  virtual void SetToken(const std::string &str)
1906  {
1907  // printf( "Model::SetToken( %s )\n", str.c_str() );
1908 
1909  if (str.size() > 0) {
1910  world->AddModelName(this, str);
1911  Ancestor::SetToken(str);
1912  } else
1913  PRINT_ERR("Model::SetToken() called with zero length string. Ignored.");
1914  }
1915 
1916  const std::string &GetModelType() const { return type; }
1917  std::string GetSayString() { return std::string(say_string); }
1920  Model *GetChild(const std::string &name) const;
1921 
1924  class Visibility {
1925  public:
1931  double ranger_return;
1932 
1933  Visibility();
1934 
1936  void Save(Worldfile *wf, int wf_entity);
1937  } vis;
1938 
1939  usec_t GetUpdateInterval() const { return interval; }
1941  // usec_t GetPoseInterval() const { return interval_pose; }
1942 
1945  void Rasterize(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth,
1946  meters_t cellheight);
1947 
1951  bool HasCollision() { return TestCollision() != NULL; }
1952 private:
1955  explicit Model(const Model &original);
1956 
1959  Model &operator=(const Model &original);
1960 
1961 protected:
1963  void RegisterOption(Option *opt);
1964 
1965  void AppendTouchingModels(std::set<Model *> &touchers);
1966 
1971  Model *TestCollision();
1972 
1973  void Map(unsigned int layer);
1974 
1976  inline void Map()
1977  {
1978  Map(0);
1979  Map(1);
1980  }
1981 
1982  void UnMap(unsigned int layer);
1983 
1985  inline void UnMap()
1986  {
1987  UnMap(0);
1988  UnMap(1);
1989  }
1990 
1991  void MapWithChildren(unsigned int layer);
1992  void UnMapWithChildren(unsigned int layer);
1993 
1995  void MapFromRoot(unsigned int layer);
1996  void UnMapFromRoot(unsigned int layer);
1997 
2000  RaytraceResult Raytrace(const Pose &pose, const meters_t range, const ray_test_func_t func,
2001  const void *arg, const bool ztest)
2002  {
2003  return world->Raytrace(LocalToGlobal(pose), range, func, this, arg, ztest);
2004  }
2005 
2008  void Raytrace(const Pose &pose, const meters_t range, const radians_t fov,
2009  const ray_test_func_t func, const void *arg, const bool ztest,
2010  std::vector<RaytraceResult> &results)
2011  {
2012  return world->Raytrace(LocalToGlobal(pose), range, fov, func, this, arg, ztest, results);
2013  }
2014 
2015  virtual void UpdateCharge();
2016 
2017  static int UpdateWrapper(Model *mod, void *)
2018  {
2019  mod->Update();
2020  return 0;
2021  }
2022 
2024  void CallUpdateCallbacks(void);
2025 
2026  meters_t ModelHeight() const;
2027 
2028  void DrawBlocksTree();
2029  virtual void DrawBlocks();
2030  void DrawBoundingBox();
2031  void DrawBoundingBoxTree();
2032  virtual void DrawStatus(Camera *cam);
2033  void DrawStatusTree(Camera *cam);
2034 
2035  void DrawOriginTree();
2036  void DrawOrigin();
2037 
2038  void PushLocalCoords();
2039  void PopCoords();
2040 
2042  void DrawImage(uint32_t texture_id, Camera *cam, float alpha, double width = 1.0,
2043  double height = 1.0);
2044 
2045  virtual void DrawPicker();
2046  virtual void DataVisualize(Camera *cam);
2047  virtual void DrawSelected(void);
2048 
2049  void DrawTrailFootprint();
2050  void DrawTrailBlocks();
2051  void DrawTrailArrows();
2052  void DrawGrid();
2053  // void DrawBlinkenlights();
2054  void DataVisualizeTree(Camera *cam);
2055  void DrawFlagList();
2056  void DrawPose(Pose pose);
2057 
2058 public:
2059  virtual void PushColor(Color col) { world->PushColor(col); }
2060  virtual void PushColor(double r, double g, double b, double a) { world->PushColor(r, g, b, a); }
2061  virtual void PopColor() { world->PopColor(); }
2062  PowerPack *FindPowerPack() const;
2063 
2064  // void RecordRenderPoint( GSList** head, GSList* link,
2065  // unsigned int* c1, unsigned int* c2 );
2066 
2078  bool PlaceInFreeSpace(meters_t xmin, meters_t xmax, meters_t ymin, meters_t ymax,
2079  size_t max_iter = 0);
2080 
2085  bool RandomPoseInFreeSpace(meters_t xmin, meters_t xmax, meters_t ymin, meters_t ymax,
2086  size_t max_iter = 0);
2087 
2089  std::string PoseString() { return pose.String(); }
2091  static Model *LookupId(uint32_t id) { return modelsbyid[id]; }
2093  Model(World *world, Model *parent = NULL, const std::string &type = "model",
2094  const std::string &name = "");
2095 
2097  virtual ~Model();
2098 
2101  : mapped(false), alwayson(false), blockgroup(*this), boundary(false), data_fresh(false),
2102  disabled(true), friction(0), has_default_block(false), id(0), interval(0),
2103  interval_energy(0), last_update(0), log_state(false), map_resolution(0), mass(0),
2104  parent(NULL), power_pack(NULL), rebuild_displaylist(false), stack_children(true),
2105  stall(false), subs(0), thread_safe(false), trail_index(0), event_queue_num(0), used(false),
2106  watts(0), watts_give(0), watts_take(0), wf(NULL), wf_entity(0), world(NULL), world_gui(NULL)
2107  {
2108  }
2109 
2110  void Say(const std::string &str);
2111 
2113  void AddVisualizer(Visualizer *custom_visual, bool on_by_default);
2114 
2117  void RemoveVisualizer(Visualizer *custom_visual);
2118 
2119  void BecomeParentOf(Model *child);
2120 
2122  {
2126  Load(); // call virtual load
2127  }
2128 
2131  {
2132  this->wf = wf;
2133  this->wf_entity = wf_entity;
2134  }
2135 
2137  virtual void Load();
2138 
2140  virtual void Save();
2141 
2143  void InitControllers();
2144 
2145  void AddFlag(Flag *flag);
2146  void RemoveFlag(Flag *flag);
2147 
2148  void PushFlag(Flag *flag);
2149  Flag *PopFlag();
2150 
2151  unsigned int GetFlagCount() const { return flag_list.size(); }
2156  void Disable() { disabled = true; }
2159  void Enable() { disabled = false; }
2161  bool IsEnabled() const { return !disabled; }
2164  void LoadControllerModule(const char *lib);
2165 
2168  void NeedRedraw();
2169 
2171  void Redraw();
2172 
2175  void LoadBlock(Worldfile *wf, int entity);
2176 
2179  void AddBlockRect(meters_t x, meters_t y, meters_t dx, meters_t dy, meters_t dz);
2180 
2182  void ClearBlocks();
2183 
2186  Model *Parent() const { return this->parent; }
2188  World *GetWorld() const { return this->world; }
2190  Model *Root() { return (parent ? parent->Root() : this); }
2191  bool IsAntecedent(const Model *testmod) const;
2192 
2194  bool IsDescendent(const Model *testmod) const;
2195 
2197  bool IsRelated(const Model *testmod) const;
2198 
2200  Pose GetGlobalPose() const;
2201 
2203  void Subscribe();
2204 
2206  void Unsubscribe();
2207 
2209  void SetGlobalPose(const Pose &gpose);
2210 
2212  void SetPose(const Pose &pose);
2213 
2215  void AddToPose(const Pose &pose);
2216 
2218  void AddToPose(double dx, double dy, double dz, double da);
2219 
2221  void SetGeom(const Geom &src);
2222 
2225  void SetFiducialReturn(int fid);
2226 
2228  int GetFiducialReturn() const { return vis.fiducial_return; }
2231  void SetFiducialKey(int key);
2232 
2233  Color GetColor() const { return color; }
2235  uint32_t GetId() const { return id; }
2237  kg_t GetTotalMass() const;
2238 
2240  kg_t GetMassOfChildren() const;
2241 
2243  int SetParent(Model *newparent);
2244 
2247  Geom GetGeom() const { return geom; }
2250  Pose GetPose() const { return pose; }
2251  // guess what these do?
2252  void SetColor(Color col);
2253  void SetMass(kg_t mass);
2254  void SetStall(bool stall);
2255  void SetGravityReturn(bool val);
2256  void SetGripperReturn(bool val);
2257  void SetStickyReturn(bool val);
2258  void SetRangerReturn(double val);
2259  void SetObstacleReturn(bool val);
2260  void SetBlobReturn(bool val);
2261  void SetRangerReturn(bool val);
2262  void SetBoundary(bool val);
2263  void SetGuiNose(bool val);
2264  void SetGuiMove(bool val);
2265  void SetGuiGrid(bool val);
2266  void SetGuiOutline(bool val);
2267  void SetWatts(watts_t watts);
2268  void SetMapResolution(meters_t res);
2269  void SetFriction(double friction);
2270 
2271  bool DataIsFresh() const { return this->data_fresh; }
2281  void AddCallback(callback_type_t type, model_callback_t cb, void *user);
2282 
2284 
2286 
2287  virtual void Print(char *prefix) const;
2288  virtual const char *PrintWithPose() const;
2289 
2292  Pose GlobalToLocal(const Pose &pose) const;
2293 
2296  Pose LocalToGlobal(const Pose &pose) const { return ((GetGlobalPose() + geom.pose) + pose); }
2298  std::vector<point_int_t> LocalToPixels(const std::vector<point_t> &local) const;
2299 
2302  point_t LocalToGlobal(const point_t &pt) const;
2303 
2306  Model *GetUnsubscribedModelOfType(const std::string &type) const;
2307 
2310  Model *GetUnusedModelOfType(const std::string &type);
2311 
2314  bool Stalled() const { return this->stall; }
2317  unsigned int GetSubscriptionCount() const { return subs; }
2319  bool HasSubscribers() const { return (subs > 0); }
2320  static std::map<std::string, creator_t> name_map;
2321 
2322 protected:
2323  virtual void Startup();
2324  virtual void Shutdown();
2325  virtual void Update();
2326 };
2327 
2328 // BLOBFINDER MODEL --------------------------------------------------------
2330 class ModelBlobfinder : public Model {
2331 public:
2333  class Blob {
2334  public:
2336  uint32_t left, top, right, bottom;
2338  };
2339 
2341  class Vis : public Visualizer {
2342  public:
2343  explicit Vis(World *world);
2344  virtual ~Vis(void) {}
2345  virtual void Visualize(Model *mod, Camera *cam);
2346  } vis;
2347 
2348 private:
2351  std::vector<Blob> blobs;
2352 
2356  std::vector<Color> colors;
2357 
2359  static bool BlockMatcher(Block *testblock, Model *finder);
2360 
2361 public:
2366  unsigned int scan_height;
2367  unsigned int scan_width;
2368 
2370  explicit ModelBlobfinder(World *world, Model *parent, const std::string &type);
2372  ~ModelBlobfinder();
2373 
2374  virtual void Startup();
2375  virtual void Shutdown();
2376  virtual void Update();
2377  virtual void Load();
2378 
2383  const std::vector<Blob> &GetBlobs() const { return blobs; }
2386  std::vector<Blob> &GetBlobsMutable() { return blobs; }
2388  void AddColor(Color col);
2389 
2391  void RemoveColor(Color col);
2392 
2395  void RemoveAllColors();
2396 };
2397 
2398 // Light indicator model
2399 class ModelLightIndicator : public Model {
2400 public:
2401  explicit ModelLightIndicator(World *world, Model *parent, const std::string &type);
2403 
2404  void SetState(bool isOn);
2405 
2406 protected:
2407  virtual void DrawBlocks();
2408 
2409 private:
2410  bool m_IsOn;
2411 };
2412 
2413 // \todo GRIPPER MODEL --------------------------------------------------------
2414 
2415 class ModelGripper : public Model {
2416 public:
2422  };
2423 
2428  LIFT_DOWNING
2429  };
2430 
2431  enum cmd_t {
2432  CMD_NOOP = 0,
2436  CMD_DOWN
2437  };
2438 
2441  struct config_t {
2446  double lift_position;
2449  double
2451  bool autosnatch;
2452  double break_beam_inset[2];
2453  Model *beam[2];
2455  };
2456 
2457 private:
2458  virtual void Update();
2459  virtual void DataVisualize(Camera *cam);
2460 
2461  void FixBlocks();
2462  void PositionPaddles();
2463  void UpdateBreakBeams();
2464  void UpdateContacts();
2465 
2466  config_t cfg;
2467  cmd_t cmd;
2468 
2469  Block *paddle_left;
2470  Block *paddle_right;
2471 
2472  static Option showData;
2473 
2474 public:
2475  static const Size size;
2476 
2478  ModelGripper(World *world, Model *parent, const std::string &type);
2480  virtual ~ModelGripper();
2481 
2482  virtual void Load();
2483  virtual void Save();
2484 
2486  void SetConfig(config_t &newcfg)
2487  {
2488  this->cfg = newcfg;
2489  FixBlocks();
2490  }
2491 
2493  config_t GetConfig() { return cfg; }
2495  void SetCommand(cmd_t cmd) { this->cmd = cmd; }
2504 };
2505 
2506 // BUMPER MODEL --------------------------------------------------------
2508 class ModelBumper : public Model {
2509 public:
2511  public:
2514  };
2515 
2517  public:
2520  };
2521 
2522 public:
2523  ModelBumper(World *world, Model *parent, const std::string &type);
2524  virtual ~ModelBumper();
2525 
2526  virtual void Load();
2527 
2528  uint32_t bumper_count;
2531 
2532 protected:
2533  virtual void Startup();
2534  virtual void Shutdown();
2535  virtual void Update();
2536  virtual void Print(char *prefix) const;
2537 
2538  class BumperVis : public Visualizer {
2539  public:
2540  BumperVis();
2541  virtual ~BumperVis();
2542  virtual void Visualize(Model *mod, Camera *cam);
2544 
2545 private:
2546  static Option showBumperData;
2547 };
2548 
2549 // FIDUCIAL MODEL --------------------------------------------------------
2550 
2552 class ModelFiducial : public Model {
2553 public:
2555  class Fiducial {
2556  public:
2560  // Pose pose_rel; /// relative pose of the target in local coordinates
2564  int id;
2566  };
2567 
2568 private:
2570  void AddModelIfVisible(Model *him);
2571 
2572  virtual void Update();
2573  virtual void DataVisualize(Camera *cam);
2574 
2575  static Option showData;
2576  static Option showFov;
2577 
2578  std::vector<Fiducial> fiducials;
2579 
2580 public:
2581  ModelFiducial(World *world, Model *parent, const std::string &type);
2582  virtual ~ModelFiducial();
2583 
2584  virtual void Load();
2585  void Shutdown(void);
2586 
2592  int key;
2595 
2597  std::vector<Fiducial> &GetFiducials() { return fiducials; }
2599  Fiducial *GetFiducials(unsigned int *count)
2600  {
2601  if (count)
2602  *count = fiducials.size();
2603  return &fiducials[0];
2604  }
2605 };
2606 
2607 // RANGER MODEL --------------------------------------------------------
2608 
2610 class ModelRanger : public Model {
2611 public:
2612 public:
2613  ModelRanger(World *world, Model *parent, const std::string &type);
2614  virtual ~ModelRanger();
2615 
2616  virtual void Print(char *prefix) const;
2617 
2618  class Vis : public Visualizer {
2619  public:
2622  static Option showFov;
2625 
2626  explicit Vis(World *world);
2627  virtual ~Vis(void) {}
2628  virtual void Visualize(Model *mod, Camera *cam);
2629  } vis;
2630 
2631  class Sensor {
2632  public:
2637  double angle_noise; //< variance for ranger angle
2638  double range_noise; //< variance for range readings
2639  double range_noise_const; //< variance for constant noise (not depending on range)
2640  unsigned int sample_count;
2642 
2643  std::vector<meters_t> ranges;
2644  std::vector<double> intensities;
2645  std::vector<double> bearings;
2646 
2648  : pose(0, 0, 0, 0), size(0.02, 0.02, 0.02), // teeny transducer
2649  range(0.0, 5.0), fov(0.1), angle_noise(0.0), range_noise(0.0), range_noise_const(0.0),
2650  sample_count(1), color(Color(0, 0, 1, 0.15)), ranges(), intensities(), bearings()
2651  {
2652  }
2653 
2654  void Update(ModelRanger *rgr);
2655  void Visualize(Vis *vis, ModelRanger *rgr) const;
2656  std::string String() const;
2657  void Load(Worldfile *wf, int entity);
2658  };
2659 
2661  const std::vector<Sensor> &GetSensors() const { return sensors; }
2663  std::vector<Sensor> &GetSensorsMutable() { return sensors; }
2664  void LoadSensor(Worldfile *wf, int entity);
2665 
2666 private:
2667  std::vector<Sensor> sensors;
2668 
2669 protected:
2670  virtual void Startup();
2671  virtual void Shutdown();
2672  virtual void Update();
2673 };
2674 
2675 // BLINKENLIGHT MODEL ----------------------------------------------------
2676 class ModelBlinkenlight : public Model {
2677 private:
2678  double dutycycle;
2679  bool enabled;
2680  msec_t period;
2681  bool on;
2682 
2683  static Option showBlinkenData;
2684 
2685 public:
2686  ModelBlinkenlight(World *world, Model *parent, const std::string &type);
2687 
2689 
2690  virtual void Load();
2691  virtual void Update();
2692  virtual void DataVisualize(Camera *cam);
2693 };
2694 
2695 // CAMERA MODEL ----------------------------------------------------
2696 
2698 class ModelCamera : public Model {
2699 public:
2700  typedef struct {
2701  // GL_V3F
2702  GLfloat x, y, z;
2703  } ColoredVertex;
2704 
2705 private:
2706  Canvas *_canvas;
2707 
2708  GLfloat *_frame_data; // opengl read buffer
2709  GLubyte *_frame_color_data; // opengl read buffer
2710 
2711  bool _valid_vertexbuf_cache;
2712  ColoredVertex *_vertexbuf_cache; // cached unit vectors with appropriate rotations (these must be
2713  // scalled by z-buffer length)
2714 
2715  int _width; // width of buffer
2716  int _height; // height of buffer
2717  static const int _depth = 4;
2718 
2719  int _camera_quads_size;
2720  GLfloat *_camera_quads;
2721  GLubyte *_camera_colors;
2722 
2723  static Option showCameraData;
2724 
2725  PerspectiveCamera _camera;
2726  double _yaw_offset; // position camera is mounted at
2727  double _pitch_offset;
2728 
2731  bool GetFrame();
2732 
2733 public:
2734  ModelCamera(World *world, Model *parent, const std::string &type);
2735 
2736  ~ModelCamera();
2737 
2738  virtual void Load();
2739 
2741  virtual void Update();
2742 
2744  // virtual void Draw( uint32_t flags, Canvas* canvas );
2745 
2747  virtual void DataVisualize(Camera *cam);
2748 
2750  int getWidth(void) const { return _width; }
2752  int getHeight(void) const { return _height; }
2754  const PerspectiveCamera &getCamera(void) const { return _camera; }
2756  const GLfloat *FrameDepth() const { return _frame_data; }
2758  const GLubyte *FrameColor() const { return _frame_color_data; }
2760  void setPitch(double pitch)
2761  {
2762  _pitch_offset = pitch;
2763  _valid_vertexbuf_cache = false;
2764  }
2765 
2767  void setYaw(double yaw)
2768  {
2769  _yaw_offset = yaw;
2770  _valid_vertexbuf_cache = false;
2771  }
2772 };
2773 
2774 // POSITION MODEL --------------------------------------------------------
2775 
2777 class ModelPosition : public Model {
2778  friend class Canvas;
2779  friend class World;
2780 
2781 public:
2784 
2787 
2790 
2791 private:
2792  Velocity velocity;
2793  Pose goal;
2794  ControlMode control_mode;
2795  DriveMode drive_mode;
2796  LocalizationMode localization_mode;
2797  Velocity integration_error;
2798  double wheelbase;
2799 
2800 public:
2803 
2806 
2808  ModelPosition(World *world, Model *parent, const std::string &type);
2810  ~ModelPosition();
2811 
2814  Velocity GetVelocity() const { return velocity; }
2815  void SetVelocity(const Velocity &val);
2817  Velocity GetGlobalVelocity() const;
2819  void SetGlobalVelocity(const Velocity &gvel);
2820 
2822  Velocity GetOdomError() const { return integration_error; }
2825  class Waypoint {
2826  public:
2828  Waypoint(const Pose &pose, Color color);
2829  Waypoint();
2830  void Draw() const;
2831 
2834  };
2835 
2836  std::vector<Waypoint> waypoints;
2837 
2838  class WaypointVis : public Visualizer {
2839  public:
2840  WaypointVis();
2841  virtual ~WaypointVis(void) {}
2842  virtual void Visualize(Model *mod, Camera *cam);
2844 
2845  class PoseVis : public Visualizer {
2846  public:
2847  PoseVis();
2848  virtual ~PoseVis(void) {}
2849  virtual void Visualize(Model *mod, Camera *cam);
2851 
2853  void SetOdom(Pose odom);
2854 
2857  void SetSpeed(double x, double y, double a);
2858  void SetXSpeed(double x);
2859  void SetYSpeed(double y);
2860  void SetZSpeed(double z);
2861  void SetTurnSpeed(double a);
2862  void SetSpeed(Velocity vel);
2864  void Stop();
2865 
2868  void GoTo(double x, double y, double a);
2869  void GoTo(Pose pose);
2870 
2874  void SetAcceleration(double x, double y, double a);
2875 
2876  // localization state
2877  Pose est_pose; //<! position estimate in local coordinates
2878  Pose est_pose_error; //<! estimated error in position estimate
2879  Pose est_origin; //<! global origin of the local coordinate system
2880 
2881 protected:
2882  virtual void Move();
2883  virtual void Startup();
2884  virtual void Shutdown();
2885  virtual void Update();
2886  virtual void Load();
2887 };
2888 
2889 // ACTUATOR MODEL --------------------------------------------------------
2890 
2892 class ModelActuator : public Model {
2893 public:
2896 
2899 
2900 private:
2901  double goal; //< the current velocity or pose to reach depending on the value of control_mode
2902  double pos;
2903  double max_speed;
2904  double min_position;
2905  double max_position;
2906  double start_position;
2907  double cosa;
2908  double sina;
2909  ControlMode control_mode;
2910  ActuatorType actuator_type;
2911  point3_t axis;
2912 
2913  Pose InitialPose;
2914 
2915 public:
2917  ModelActuator(World *world, Model *parent, const std::string &type);
2919  ~ModelActuator();
2920 
2921  virtual void Startup();
2922  virtual void Shutdown();
2923  virtual void Update();
2924  virtual void Load();
2925 
2928  void SetSpeed(double speed);
2929 
2930  double GetSpeed() const { return goal; }
2933  void GoTo(double pose);
2934 
2935  double GetPosition() const { return pos; }
2936  double GetMaxPosition() const { return max_position; }
2937  double GetMinPosition() const { return min_position; }
2938  ActuatorType GetType() const { return actuator_type; }
2939  point3_t GetAxis() const { return axis; }
2940 };
2941 
2942 } // end namespace stg
2943 
2944 #endif
Definition: stage.hh:657
const std::string & TokenStr() const
Definition: stage.hh:690
void * GetProperty(std::string &key)
Definition: stage.hh:704
Ancestor & Load(Worldfile *wf, int section)
Definition: ancestor.cc:48
Ancestor()
Definition: ancestor.cc:4
std::string token
Definition: stage.hh:671
virtual void AddChild(Model *mod)
Definition: ancestor.cc:15
std::vector< Model * > children
Definition: stage.hh:664
bool debug
Definition: stage.hh:666
std::map< std::string, unsigned int > child_type_counts
Definition: stage.hh:662
std::map< std::string, void * > props
Definition: stage.hh:669
const char * Token() const
Definition: stage.hh:689
virtual Pose GetGlobalPose() const
Definition: ancestor.cc:34
virtual void RemoveChild(Model *mod)
Definition: ancestor.cc:28
void Save(Worldfile *wf, int section)
Definition: ancestor.cc:53
void SetProperty(std::string &key, void *value)
Definition: stage.hh:702
virtual ~Ancestor()
Definition: ancestor.cc:9
virtual void SetToken(const std::string &str)
Definition: stage.hh:691
void ForEachDescendant(model_callback_t func, void *arg)
Definition: ancestor.cc:39
std::vector< Model * > & GetChildren()
Definition: stage.hh:681
Definition: stage.hh:1205
const Block & GetBlock(unsigned int index) const
Definition: stage.hh:1262
Model & mod
Definition: stage.hh:1216
~BlockGroup()
Definition: blockgroup.cc:17
uint32_t GetCount() const
Definition: stage.hh:1261
bounds3d_t BoundingBox() const
Definition: blockgroup.cc:53
void DrawFootPrint(const Geom &geom)
Definition: blockgroup.cc:146
Block & GetBlockMutable(unsigned int index)
Definition: stage.hh:1263
BlockGroup(Model &mod)
Definition: blockgroup.cc:13
Definition: stage.hh:1128
~Block()
Definition: block.cc:30
void Translate(double x, double y)
Definition: block.cc:36
double CenterY()
Definition: block.cc:49
void DrawSolid(bool topview)
Definition: block.cc:303
void DrawFootPrint()
Definition: block.cc:295
BlockGroup * group
The BlockGroup to which this Block belongs.
Definition: stage.hh:1190
void UnMap(unsigned int layer)
Definition: block.cc:183
void SetCenterY(double y)
Definition: block.cc:95
Model * TestCollision()
Definition: block.cc:129
void Load(Worldfile *wf, int entity)
Definition: block.cc:309
void SetCenter(double x, double y)
Definition: block.cc:88
void SetZ(double min, double max)
Definition: block.cc:107
void AppendTouchingModels(std::set< Model * > &touchers)
Definition: block.cc:116
void Map(unsigned int layer)
Definition: block.cc:170
double CenterX()
Definition: block.cc:69
Block(BlockGroup *group, const std::vector< point_t > &pts, const Bounds &zrange)
Definition: block.cc:12
void Rasterize(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth, meters_t cellheight)
Definition: block.cc:198
void SetCenterX(double y)
Definition: block.cc:102
Definition: stage.hh:407
double Constrain(double value)
returns value, but no smaller than min and no larger than max.
Definition: model.cc:159
Bounds()
Definition: stage.hh:414
double max
largest value in range, initially zero
Definition: stage.hh:412
double min
smallest value in range, initially zero
Definition: stage.hh:410
Bounds(double min, double max)
Definition: stage.hh:415
Bounds & Load(Worldfile *wf, int section, const char *keyword)
Definition: model.cc:153
Definition: stage.hh:1271
double _z
Definition: stage.hh:1275
Camera()
Definition: stage.hh:1278
double z(void) const
Definition: stage.hh:1287
double _yaw
Definition: stage.hh:1274
double _y
Definition: stage.hh:1275
double pitch(void) const
Definition: stage.hh:1284
virtual ~Camera()
Definition: stage.hh:1279
double yaw(void) const
Definition: stage.hh:1283
virtual void Draw(void) const =0
virtual void SetProjection(void) const =0
double _x
Definition: stage.hh:1275
double x(void) const
Definition: stage.hh:1285
virtual void reset()=0
double _pitch
Definition: stage.hh:1273
virtual void Load(Worldfile *wf, int sec)=0
double y(void) const
Definition: stage.hh:1286
Definition: canvas.hh:9
Definition: region.hh:42
Definition: stage.hh:214
bool operator!=(const Color &other) const
Definition: color.cc:17
static const Color cyan
Definition: stage.hh:233
static const Color yellow
Definition: stage.hh:233
double g
Definition: stage.hh:216
double b
Definition: stage.hh:216
double a
Definition: stage.hh:216
static Color RandomColor()
Definition: color.cc:89
void Print(const char *prefix) const
Definition: color.cc:94
static const Color green
Definition: stage.hh:233
Color()
Definition: color.cc:13
static const Color blue
Definition: stage.hh:233
void GLSet(void)
Definition: stage.hh:237
const Color & Load(Worldfile *wf, int entity)
Definition: color.cc:99
static const Color red
Definition: stage.hh:233
bool operator==(const Color &other) const
Definition: color.cc:84
double r
Definition: stage.hh:216
static const Color magenta
Definition: stage.hh:233
Definition: stage.hh:753
std::string cmdline
Definition: stage.hh:756
CtrlArgs(std::string w, std::string c)
Definition: stage.hh:758
std::string worldfile
Definition: stage.hh:755
Definition: file_manager.hh:9
Definition: stage.hh:376
void Zero()
Definition: stage.hh:399
Pose pose
position
Definition: stage.hh:378
Geom(const Pose &p, const Size &s)
Definition: stage.hh:398
void Print(const char *prefix) const
Definition: stage.hh:386
Geom()
Definition: stage.hh:396
Size size
extent
Definition: stage.hh:379
Definition: stage.hh:734
static size_t Count()
Definition: stage.hh:746
LogEntry(usec_t timestamp, Model *mod)
Definition: logentry.cc:7
static std::vector< LogEntry > log
Definition: stage.hh:743
static void Print()
Definition: logentry.cc:14
static void Clear()
Definition: stage.hh:748
ModelActuator class
Definition: stage.hh:2892
~ModelActuator()
Destructor.
Definition: model_actuator.cc:80
virtual void Load()
Definition: model_actuator.cc:85
ControlMode
Definition: stage.hh:2895
@ CONTROL_VELOCITY
Definition: stage.hh:2895
@ CONTROL_POSITION
Definition: stage.hh:2895
point3_t GetAxis() const
Definition: stage.hh:2939
double GetMaxPosition() const
Definition: stage.hh:2936
ActuatorType
Definition: stage.hh:2898
@ TYPE_LINEAR
Definition: stage.hh:2898
@ TYPE_ROTATIONAL
Definition: stage.hh:2898
void SetSpeed(double speed)
Definition: model_actuator.cc:274
double GetMinPosition() const
Definition: stage.hh:2937
ActuatorType GetType() const
Definition: stage.hh:2938
virtual void Startup()
Definition: model_actuator.cc:255
double GetPosition() const
Definition: stage.hh:2935
virtual void Shutdown()
Definition: model_actuator.cc:262
virtual void Update()
Definition: model_actuator.cc:163
ModelActuator(World *world, Model *parent, const std::string &type)
Constructor.
Definition: model_actuator.cc:65
void GoTo(double pose)
Definition: model_actuator.cc:280
double GetSpeed() const
Definition: stage.hh:2930
Definition: stage.hh:2676
virtual void Update()
Definition: model_blinkenlight.cc:95
ModelBlinkenlight(World *world, Model *parent, const std::string &type)
Definition: model_blinkenlight.cc:61
virtual void DataVisualize(Camera *cam)
Definition: model_blinkenlight.cc:102
virtual void Load()
Definition: model_blinkenlight.cc:86
~ModelBlinkenlight()
Definition: model_blinkenlight.cc:82
Definition: stage.hh:2333
uint32_t bottom
Definition: stage.hh:2336
uint32_t left
Definition: stage.hh:2336
uint32_t right
Definition: stage.hh:2336
Color color
Definition: stage.hh:2335
uint32_t top
Definition: stage.hh:2336
meters_t range
Definition: stage.hh:2337
Definition: stage.hh:2341
virtual void Visualize(Model *mod, Camera *cam)
Definition: model_blobfinder.cc:290
Vis(World *world)
Definition: model_blobfinder.cc:282
virtual ~Vis(void)
Definition: stage.hh:2344
ModelBlobfinder class
Definition: stage.hh:2330
ModelBlobfinder(World *world, Model *parent, const std::string &type)
Constructor.
Definition: model_blobfinder.cc:86
radians_t fov
Horizontal field of view in radians, in the range 0 to pi.
Definition: stage.hh:2362
void AddColor(Color col)
virtual void Update()
Definition: model_blobfinder.cc:165
void RemoveAllColors()
Definition: model_blobfinder.cc:131
const std::vector< Blob > & GetBlobs() const
Definition: stage.hh:2383
std::vector< Blob > & GetBlobsMutable()
Definition: stage.hh:2386
meters_t range
Definition: stage.hh:2364
Stg::ModelBlobfinder::Vis vis
unsigned int scan_width
Width of the input image in pixels.
Definition: stage.hh:2367
virtual void Load()
Definition: model_blobfinder.cc:136
~ModelBlobfinder()
Destructor.
Definition: model_blobfinder.cc:98
radians_t pan
Horizontal pan angle in radians, in the range -pi to +pi.
Definition: stage.hh:2363
void RemoveColor(Color col)
Definition: model_blobfinder.cc:121
virtual void Shutdown()
Definition: model_blobfinder.cc:262
unsigned int scan_height
setting this small saves computation time.
Definition: stage.hh:2366
virtual void Startup()
Definition: model_blobfinder.cc:252
Definition: stage.hh:2510
meters_t length
Definition: stage.hh:2513
Pose pose
Definition: stage.hh:2512
Definition: stage.hh:2516
point_t hit_point
Definition: stage.hh:2519
Model * hit
Definition: stage.hh:2518
Definition: stage.hh:2538
virtual void Visualize(Model *mod, Camera *cam)
Definition: model_bumper.cc:216
virtual ~BumperVis()
Definition: model_bumper.cc:211
BumperVis()
Definition: model_bumper.cc:206
ModelBumper class
Definition: stage.hh:2508
uint32_t bumper_count
Definition: stage.hh:2528
virtual void Startup()
Definition: model_bumper.cc:95
virtual void Print(char *prefix) const
Definition: model_bumper.cc:195
BumperConfig * bumpers
Definition: stage.hh:2529
virtual ~ModelBumper()
Definition: model_bumper.cc:87
virtual void Update()
Definition: model_bumper.cc:164
BumperSample * samples
Definition: stage.hh:2530
virtual void Load()
Definition: model_bumper.cc:118
virtual void Shutdown()
Definition: model_bumper.cc:104
Stg::ModelBumper::BumperVis bumpervis
ModelBumper(World *world, Model *parent, const std::string &type)
Definition: model_bumper.cc:70
ModelCamera class
Definition: stage.hh:2698
int getWidth(void) const
width of captured image
Definition: stage.hh:2750
const PerspectiveCamera & getCamera(void) const
get reference to camera used
Definition: stage.hh:2754
virtual void Update()
Capture a new frame ( calls GetFrame )
Definition: model_camera.cc:147
const GLfloat * FrameDepth() const
get a reference to camera depth buffer
Definition: stage.hh:2756
int getHeight(void) const
height of captured image
Definition: stage.hh:2752
~ModelCamera()
Definition: model_camera.cc:115
const GLubyte * FrameColor() const
get a reference to camera color image. 4 bytes (RGBA) per pixel
Definition: stage.hh:2758
void setYaw(double yaw)
change the yaw
Definition: stage.hh:2767
ModelCamera(World *world, Model *parent, const std::string &type)
Definition: model_camera.cc:89
virtual void DataVisualize(Camera *cam)
Draw Camera Model - TODO.
Definition: model_camera.cc:221
virtual void Load()
Definition: model_camera.cc:127
void setPitch(double pitch)
change the pitch
Definition: stage.hh:2760
Definition: stage.hh:2555
Model * mod
use this in robot controllers!)
Definition: stage.hh:2563
Pose pose
Definition: stage.hh:2561
int id
Definition: stage.hh:2564
radians_t bearing
bearing to the target
Definition: stage.hh:2558
Pose geom
size and relative angle of the target
Definition: stage.hh:2559
meters_t range
range to the target
Definition: stage.hh:2557
ModelFiducial class
Definition: stage.hh:2552
Fiducial * GetFiducials(unsigned int *count)
Definition: stage.hh:2599
virtual void Load()
Definition: model_fiducial.cc:295
std::vector< Fiducial > & GetFiducials()
fiducial detector?
Definition: stage.hh:2597
radians_t fov
field of view
Definition: stage.hh:2590
bool ignore_zloc
Definition: stage.hh:2593
int key
/// only detect fiducials with a key that matches this one (defaults 0)
Definition: stage.hh:2592
meters_t max_range_anon
maximum detection range
Definition: stage.hh:2587
radians_t heading
center of field of view
Definition: stage.hh:2591
virtual ~ModelFiducial()
Definition: model_fiducial.cc:99
meters_t max_range_id
maximum range at which the ID can be read
Definition: stage.hh:2588
meters_t min_range
minimum detection range
Definition: stage.hh:2589
void Shutdown(void)
Definition: model_fiducial.cc:372
Definition: stage.hh:2415
void CommandClose()
Definition: stage.hh:2497
static const Size size
Definition: stage.hh:2475
virtual void Save()
Definition: model_gripper.cc:158
void CommandDown()
Definition: stage.hh:2503
ModelGripper(World *world, Model *parent, const std::string &type)
constructor
Definition: model_gripper.cc:63
virtual void Load()
Definition: model_gripper.cc:109
config_t GetConfig()
Definition: stage.hh:2493
paddle_state_t
Definition: stage.hh:2417
@ PADDLE_CLOSING
Definition: stage.hh:2421
@ PADDLE_OPEN
Default state.
Definition: stage.hh:2418
@ PADDLE_OPENING
Definition: stage.hh:2420
@ PADDLE_CLOSED
Definition: stage.hh:2419
void SetCommand(cmd_t cmd)
Definition: stage.hh:2495
void CommandOpen()
Definition: stage.hh:2499
lift_state_t
Definition: stage.hh:2424
@ LIFT_DOWN
Default state.
Definition: stage.hh:2425
@ LIFT_UP
Definition: stage.hh:2426
@ LIFT_UPPING
Verbed these to match the paddle state.
Definition: stage.hh:2427
@ LIFT_DOWNING
Definition: stage.hh:2428
virtual ~ModelGripper()
destructor
Definition: model_gripper.cc:105
void CommandUp()
Definition: stage.hh:2501
cmd_t
Definition: stage.hh:2431
@ CMD_DOWN
Definition: stage.hh:2436
@ CMD_NOOP
Default state.
Definition: stage.hh:2432
@ CMD_UP
Definition: stage.hh:2435
@ CMD_OPEN
Definition: stage.hh:2433
@ CMD_CLOSE
Definition: stage.hh:2434
void SetConfig(config_t &newcfg)
Definition: stage.hh:2486
Definition: stage.hh:2399
void SetState(bool isOn)
Definition: model_lightindicator.cc:14
ModelLightIndicator(World *world, Model *parent, const std::string &type)
Definition: model_lightindicator.cc:5
virtual void DrawBlocks()
Definition: model_lightindicator.cc:19
~ModelLightIndicator()
Definition: model_lightindicator.cc:10
Definition: stage.hh:2845
virtual ~PoseVis(void)
Definition: stage.hh:2848
PoseVis()
Definition: model_position.cc:680
virtual void Visualize(Model *mod, Camera *cam)
Definition: model_position.cc:684
Definition: stage.hh:2838
WaypointVis()
Definition: model_position.cc:742
virtual ~WaypointVis(void)
Definition: stage.hh:2841
virtual void Visualize(Model *mod, Camera *cam)
Definition: model_position.cc:747
Definition: stage.hh:2825
void Draw() const
Definition: model_position.cc:808
Color color
Definition: stage.hh:2833
Waypoint()
Definition: model_position.cc:804
Pose pose
Definition: stage.hh:2832
ModelPosition class
Definition: stage.hh:2777
Velocity GetVelocity() const
Definition: stage.hh:2814
~ModelPosition()
Destructor.
Definition: model_position.cc:152
DriveMode
Definition: stage.hh:2789
@ DRIVE_OMNI
Definition: stage.hh:2789
@ DRIVE_DIFFERENTIAL
Definition: stage.hh:2789
@ DRIVE_CAR
Definition: stage.hh:2789
Velocity GetOdomError() const
Definition: stage.hh:2822
void Stop()
Definition: model_position.cc:590
Bounds velocity_bounds[4]
Definition: stage.hh:2805
void SetZSpeed(double z)
Definition: model_position.cc:616
virtual void Shutdown()
Definition: model_position.cc:577
void SetYSpeed(double y)
Definition: model_position.cc:610
Stg::ModelPosition::WaypointVis wpvis
std::vector< Waypoint > waypoints
Definition: stage.hh:2836
void SetGlobalVelocity(const Velocity &gvel)
Definition: model_position.cc:180
LocalizationMode
Definition: stage.hh:2786
@ LOCALIZATION_GPS
Definition: stage.hh:2786
@ LOCALIZATION_ODOM
Definition: stage.hh:2786
virtual void Update()
Definition: model_position.cc:283
virtual void Move()
Definition: model_position.cc:526
Bounds acceleration_bounds[4]
Definition: stage.hh:2802
virtual void Startup()
Definition: model_position.cc:568
void GoTo(double x, double y, double a)
Definition: model_position.cc:637
Pose est_origin
Definition: stage.hh:2879
Velocity GetGlobalVelocity() const
Definition: model_position.cc:164
Stg::ModelPosition::PoseVis posevis
virtual void Load()
Definition: model_position.cc:195
ControlMode
Definition: stage.hh:2783
@ CONTROL_VELOCITY
Definition: stage.hh:2783
@ CONTROL_POSITION
Definition: stage.hh:2783
@ CONTROL_ACCELERATION
Definition: stage.hh:2783
void SetSpeed(double x, double y, double a)
Definition: model_position.cc:595
void SetOdom(Pose odom)
Definition: model_position.cc:664
void SetAcceleration(double x, double y, double a)
Definition: model_position.cc:652
Pose est_pose
Definition: stage.hh:2877
void SetVelocity(const Velocity &val)
Definition: model_position.cc:157
void SetTurnSpeed(double a)
Definition: model_position.cc:622
ModelPosition(World *world, Model *parent, const std::string &type)
Constructor.
Definition: model_position.cc:113
void SetXSpeed(double x)
Definition: model_position.cc:604
Pose est_pose_error
Definition: stage.hh:2878
Definition: stage.hh:2631
double angle_noise
Definition: stage.hh:2637
void Update(ModelRanger *rgr)
Definition: model_ranger.cc:211
std::string String() const
Definition: model_ranger.cc:258
double range_noise_const
Definition: stage.hh:2639
std::vector< double > bearings
Definition: stage.hh:2645
radians_t fov
Definition: stage.hh:2636
double range_noise
Definition: stage.hh:2638
std::vector< double > intensities
Definition: stage.hh:2644
void Load(Worldfile *wf, int entity)
Definition: model_ranger.cc:146
Sensor()
Definition: stage.hh:2647
unsigned int sample_count
Definition: stage.hh:2640
Bounds range
Definition: stage.hh:2635
void Visualize(Vis *vis, ModelRanger *rgr) const
Definition: model_ranger.cc:268
Size size
Definition: stage.hh:2634
std::vector< meters_t > ranges
Definition: stage.hh:2643
Pose pose
Definition: stage.hh:2633
Color color
Definition: stage.hh:2641
Definition: stage.hh:2618
static Option showStrikes
Definition: stage.hh:2621
static Option showBeams
Definition: stage.hh:2623
static Option showFov
Definition: stage.hh:2622
Vis(World *world)
Definition: model_ranger.cc:410
static Option showArea
Definition: stage.hh:2620
virtual void Visualize(Model *mod, Camera *cam)
Definition: model_ranger.cc:419
static Option showTransducers
Definition: stage.hh:2624
virtual ~Vis(void)
Definition: stage.hh:2627
ModelRanger class
Definition: stage.hh:2610
ModelRanger(World *world, Model *parent, const std::string &type)
Definition: model_ranger.cc:102
virtual void Update()
Definition: model_ranger.cc:202
virtual void Shutdown()
Definition: model_ranger.cc:130
virtual void Print(char *prefix) const
Definition: model_ranger.cc:384
std::vector< Sensor > & GetSensorsMutable()
Definition: stage.hh:2663
Stg::ModelRanger::Vis vis
const std::vector< Sensor > & GetSensors() const
Definition: stage.hh:2661
virtual void Startup()
Definition: model_ranger.cc:124
virtual ~ModelRanger()
Definition: model_ranger.cc:120
void LoadSensor(Worldfile *wf, int entity)
Definition: model_ranger.cc:139
Definition: stage.hh:1709
void SetSize(double sz)
Definition: model.cc:1022
Flag(const Color &color, double size)
Definition: model.cc:994
Color GetColor()
Definition: stage.hh:1719
double GetSize()
Definition: stage.hh:1720
void SetColor(const Color &col)
Definition: model.cc:1011
Flag * Nibble(double portion)
Definition: model.cc:998
void Draw(GLUquadric *quadric)
Definition: model.cc:1033
Definition: stage.hh:1780
GuiState & Load(Worldfile *wf, int wf_entity)
Definition: model.cc:219
GuiState()
Definition: model.cc:215
bool outline
Definition: stage.hh:1785
bool grid
Definition: stage.hh:1782
bool move
Definition: stage.hh:1783
bool nose
Definition: stage.hh:1784
Definition: stage.hh:1817
void AddPoint(meters_t x, meters_t y)
Definition: model.cc:984
int used
Definition: stage.hh:1833
void SetData(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth, meters_t cellheight)
Definition: model.cc:968
virtual void Visualize(Model *mod, Camera *cam)
Definition: model.cc:885
void ClearPts()
Definition: model.cc:989
virtual ~RasterVis(void)
Definition: stage.hh:1826
RasterVis()
Definition: model.cc:879
int subs
Definition: stage.hh:1832
Definition: stage.hh:1854
Pose pose
Definition: stage.hh:1857
TrailItem()
Definition: stage.hh:1860
Color color
Definition: stage.hh:1858
usec_t time
Definition: stage.hh:1856
Definition: stage.hh:1924
int fiducial_key
Definition: stage.hh:1927
int fiducial_return
Definition: stage.hh:1928
bool blob_return
Definition: stage.hh:1926
void Save(Worldfile *wf, int wf_entity)
Definition: model.cc:205
Visibility & Load(Worldfile *wf, int wf_entity)
Definition: model.cc:193
bool obstacle_return
Definition: stage.hh:1930
Visibility()
Definition: model.cc:187
double ranger_return
0 - 1
Definition: stage.hh:1931
bool gripper_return
Definition: stage.hh:1929
Definition: stage.hh:1688
cb_t(world_callback_t cb, void *arg)
Definition: stage.hh:1694
model_callback_t callback
Definition: stage.hh:1690
bool operator<(const cb_t &other) const
Definition: stage.hh:1697
bool operator==(const cb_t &other) const
Definition: stage.hh:1706
cb_t(model_callback_t cb, void *arg)
Definition: stage.hh:1693
cb_t()
Definition: stage.hh:1695
void * arg
Definition: stage.hh:1691
Model class
Definition: stage.hh:1651
bool disabled
Definition: stage.hh:1763
Model * parent
Definition: stage.hh:1803
void SetMapResolution(meters_t res)
Definition: model.cc:1164
void DrawTrailFootprint()
Definition: model_draw.cc:49
bool thread_safe
Definition: stage.hh:1851
Model()
Definition: stage.hh:2100
void InitControllers()
Definition: model.cc:318
bool has_default_block
Definition: stage.hh:1791
std::string PoseString()
Definition: stage.hh:2089
int boundary
Definition: stage.hh:1683
PowerPack * power_pack
Definition: stage.hh:1810
void ClearBlocks()
Definition: model.cc:360
unsigned int GetSubscriptionCount() const
Definition: stage.hh:2317
void SetRangerReturn(bool val)
virtual void Print(char *prefix) const
Definition: model.cc:542
std::vector< point_int_t > LocalToPixels(const std::vector< point_t > &local) const
Definition: model.cc:469
void RegisterOption(Option *opt)
Definition: model.cc:844
void CallUpdateCallbacks(void)
Definition: model.cc:612
void DrawFlagList()
Definition: model_draw.cc:425
BlockGroup blockgroup
Definition: stage.hh:1679
void UnMap()
Definition: stage.hh:1985
std::list< Visualizer * > cv_list
Definition: stage.hh:1766
void SetStickyReturn(bool val)
void Raytrace(const Pose &pose, const meters_t range, const radians_t fov, const ray_test_func_t func, const void *arg, const bool ztest, std::vector< RaytraceResult > &results)
Definition: stage.hh:2008
virtual void PushColor(Color col)
Definition: stage.hh:2059
bool HasCollision()
Definition: stage.hh:1951
void Map()
Definition: stage.hh:1976
virtual void SetToken(const std::string &str)
Definition: stage.hh:1905
static int UpdateWrapper(Model *mod, void *)
Definition: stage.hh:2017
virtual void DataVisualize(Camera *cam)
Definition: model_draw.cc:524
void DrawBlocksTree()
Definition: model_draw.cc:148
void RemoveVisualizer(Visualizer *custom_visual)
Definition: model_draw.cc:264
uint32_t GetId() const
Definition: stage.hh:2235
World * GetWorld() const
Definition: stage.hh:2188
bool RandomPoseInFreeSpace(meters_t xmin, meters_t xmax, meters_t ymin, meters_t ymax, size_t max_iter=0)
Definition: model.cc:644
void SetGuiOutline(bool val)
Definition: model.cc:1154
Model * GetChild(const std::string &name) const
Definition: model.cc:862
usec_t GetUpdateInterval() const
Definition: stage.hh:1939
void SetGravityReturn(bool val)
bool IsDescendent(const Model *testmod) const
Definition: model.cc:428
Flag * PopFlag()
Definition: model.cc:347
void Rasterize(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth, meters_t cellheight)
Definition: model.cc:849
callback_type_t
Definition: stage.hh:1729
@ CB_INIT
Definition: stage.hh:1733
@ CB_STARTUP
Definition: stage.hh:1739
@ CB_POSE
Definition: stage.hh:1736
@ CB_GEOM
Definition: stage.hh:1732
@ CB_FLAGDECR
Definition: stage.hh:1730
@ CB_PARENT
Definition: stage.hh:1735
@ __CB_TYPE_COUNT
must be the last entry: counts the number of types
Definition: stage.hh:1743
@ CB_UPDATE
Definition: stage.hh:1740
@ CB_VELOCITY
Definition: stage.hh:1741
@ CB_SHUTDOWN
Definition: stage.hh:1738
@ CB_LOAD
Definition: stage.hh:1734
@ CB_FLAGINCR
Definition: stage.hh:1731
@ CB_SAVE
Definition: stage.hh:1737
bool Stalled() const
Definition: stage.hh:2314
bool HasSubscribers() const
Definition: stage.hh:2319
void MapWithChildren(unsigned int layer)
Definition: model.cc:488
std::string GetSayString()
Definition: stage.hh:1917
Pose pose
Definition: stage.hh:1807
Model * Root()
Definition: stage.hh:2190
virtual void DrawSelected(void)
Definition: model_draw.cc:12
void RemoveFlag(Flag *flag)
Definition: model.cc:331
virtual void DrawStatus(Camera *cam)
Definition: model_draw.cc:282
void BecomeParentOf(Model *child)
Definition: model.cc:819
Geom geom
Definition: stage.hh:1777
void SetWorldfile(Worldfile *wf, int wf_entity)
Definition: stage.hh:2130
watts_t watts_take
Definition: stage.hh:1897
static Model * LookupId(uint32_t id)
Definition: stage.hh:2091
Stg::Model::RasterVis rastervis
Pose LocalToGlobal(const Pose &pose) const
Definition: stage.hh:2296
bool data_fresh
Definition: stage.hh:1758
void Unsubscribe()
Definition: model.cc:529
usec_t GetInterval()
Definition: stage.hh:1923
void SetGuiNose(bool val)
Definition: model.cc:1139
void SetStall(bool stall)
Definition: model.cc:1092
void DrawBoundingBox()
Definition: model_draw.cc:187
bool used
TRUE iff this model has been returned by GetUnusedModelOfType()
Definition: stage.hh:1887
std::vector< TrailItem > trail
Definition: stage.hh:1866
virtual void Load()
Definition: model.cc:1242
bool DataIsFresh() const
Definition: stage.hh:2271
void MapFromRoot(unsigned int layer)
Find the root model, and map/unmap the whole tree.
Definition: model.cc:497
void AddToPose(const Pose &pose)
Definition: model.cc:638
Color color
Definition: stage.hh:1753
double friction
Definition: stage.hh:1773
void SetColor(Color col)
Definition: model.cc:1081
Pose GetPose() const
Definition: stage.hh:2250
void DrawOrigin()
void UnMapFromRoot(unsigned int layer)
Definition: model.cc:511
void SetRangerReturn(double val)
Definition: model.cc:1129
void Subscribe()
Definition: model.cc:516
World * world
Pointer to the world in which this model exists.
Definition: stage.hh:1901
void SetFiducialReturn(int fid)
Definition: model.cc:1102
virtual void Startup()
Definition: model.cc:566
void SetGripperReturn(bool val)
Definition: model.cc:1097
std::list< Flag * > flag_list
Definition: stage.hh:1769
void SetPose(const Pose &pose)
Definition: model.cc:1216
void PopCoords()
Definition: model_draw.cc:237
watts_t watts
power consumed by this model
Definition: stage.hh:1889
class Stg::Model::GuiState gui
meters_t map_resolution
Definition: stage.hh:1799
int wf_entity
Definition: stage.hh:1900
void SetBlobReturn(bool val)
Definition: model.cc:1124
Model * GetUnusedModelOfType(const std::string &type)
Definition: model.cc:770
usec_t interval_energy
time between updates of powerpack in usec
Definition: stage.hh:1796
meters_t ModelHeight() const
Definition: model.cc:617
virtual const char * PrintWithPose() const
Definition: model.cc:555
kg_t GetMassOfChildren() const
Definition: model.cc:803
void Redraw()
Definition: model.cc:765
void LoadBlock(Worldfile *wf, int entity)
Definition: model.cc:368
bool IsRelated(const Model *testmod) const
Definition: model.cc:441
void PushFlag(Flag *flag)
Definition: model.cc:339
RaytraceResult Raytrace(const Pose &pose, const meters_t range, const ray_test_func_t func, const void *arg, const bool ztest)
Definition: stage.hh:2000
WorldGui * world_gui
Pointer to the GUI world - NULL if running in non-gui mode.
Definition: stage.hh:1902
std::list< PowerPack * > pps_charging
Definition: stage.hh:1814
Model * TestCollision()
Definition: model.cc:661
Model * GetUnsubscribedModelOfType(const std::string &type) const
Definition: model.cc:739
void PushLocalCoords()
Definition: model_draw.cc:227
usec_t last_update
time of last update in us
Definition: stage.hh:1797
unsigned int GetFlagCount() const
Definition: stage.hh:2151
bool IsAntecedent(const Model *testmod) const
Definition: model.cc:416
void Say(const std::string &str)
Definition: model.cc:410
bool stall
Set to true iff the model collided with something else.
Definition: stage.hh:1845
bool log_state
iff true, model state is logged
Definition: stage.hh:1798
usec_t GetEnergyInterval() const
Definition: stage.hh:1940
virtual void Update()
Definition: model.cc:593
void DrawTrailArrows()
Definition: model_draw.cc:106
void UpdateTrail()
Definition: model.cc:725
int GetFiducialReturn() const
Definition: stage.hh:2228
void DrawImage(uint32_t texture_id, Camera *cam, float alpha, double width=1.0, double height=1.0)
Definition: model_draw.cc:381
usec_t interval
time between updates in usec
Definition: stage.hh:1795
int subs
the number of subscriptions to this model
Definition: stage.hh:1846
void SetObstacleReturn(bool val)
Definition: model.cc:1119
unsigned int trail_index
Definition: stage.hh:1869
Model * Parent() const
Definition: stage.hh:2186
PowerPack * FindPowerPack() const
Definition: model.cc:833
void NeedRedraw()
Definition: model.cc:755
void SetGuiMove(bool val)
Definition: model.cc:1144
Geom GetGeom() const
Definition: stage.hh:2247
virtual void PopColor()
Definition: stage.hh:2061
void SetGeom(const Geom &src)
Definition: model.cc:1061
void SetFiducialKey(int key)
Definition: model.cc:1114
bool alwayson
Definition: stage.hh:1677
virtual void PushColor(double r, double g, double b, double a)
Definition: stage.hh:2060
void AddBlockRect(meters_t x, meters_t y, meters_t dx, meters_t dy, meters_t dz)
Definition: model.cc:378
virtual void DrawBlocks()
Definition: model_draw.cc:171
void Enable()
Definition: stage.hh:2159
int CallCallbacks(callback_type_t type)
Definition: model_callbacks.cc:32
Worldfile * wf
Definition: stage.hh:1899
Pose GlobalToLocal(const Pose &pose) const
Definition: model.cc:398
void SetFriction(double friction)
Definition: model.cc:857
unsigned int event_queue_num
Definition: stage.hh:1886
void SetMass(kg_t mass)
Definition: model.cc:1087
const std::string & GetModelType() const
Definition: stage.hh:1916
void AddCallback(callback_type_t type, model_callback_t cb, void *user)
Definition: model_callbacks.cc:5
void DrawPose(Pose pose)
Definition: model_draw.cc:159
void DrawTrailBlocks()
Definition: model_draw.cc:86
void DrawBoundingBoxTree()
Definition: model_draw.cc:176
void AddVisualizer(Visualizer *custom_visual, bool on_by_default)
Definition: model_draw.cc:242
virtual void Shutdown()
Definition: model.cc:582
void SetWatts(watts_t watts)
Definition: model.cc:1159
static std::map< std::string, creator_t > name_map
Definition: stage.hh:2320
void SetBoundary(bool val)
Definition: model.cc:1134
void DataVisualizeTree(Camera *cam)
Definition: model_draw.cc:529
virtual ~Model()
Definition: model.cc:300
void SetGlobalPose(const Pose &gpose)
Definition: model.cc:1170
Pose GetGlobalPose() const
Definition: model.cc:1200
kg_t GetTotalMass() const
Definition: model.cc:793
bool PlaceInFreeSpace(meters_t xmin, meters_t xmax, meters_t ymin, meters_t ymax, size_t max_iter=0)
watts_t watts_give
Definition: stage.hh:1893
void Disable()
Definition: stage.hh:2156
bool stack_children
whether child models should be stacked on top of this model or not
Definition: stage.hh:1843
virtual void Save()
Definition: model.cc:1435
void DrawOriginTree()
Definition: model_draw.cc:140
const std::string type
Definition: stage.hh:1883
Color GetColor() const
Definition: stage.hh:2233
int SetParent(Model *newparent)
Definition: model.cc:1175
void LoadControllerModule(const char *lib)
Definition: model.cc:1469
std::vector< std::set< cb_t > > callbacks
Definition: stage.hh:1750
virtual void DrawPicker()
Definition: model_draw.cc:509
void AddFlag(Flag *flag)
Definition: model.cc:323
int RemoveCallback(callback_type_t type, model_callback_t callback)
Definition: model_callbacks.cc:17
void UnMapWithChildren(unsigned int layer)
Definition: model.cc:502
bool rebuild_displaylist
iff true, regenerate block display list before redraw
Definition: stage.hh:1840
void AppendTouchingModels(std::set< Model * > &touchers)
Definition: model.cc:656
void SetGuiGrid(bool val)
Definition: model.cc:1149
friend class ModelFiducial
Definition: stage.hh:1662
void DrawGrid()
Definition: model_draw.cc:550
bool IsEnabled() const
Definition: stage.hh:2161
std::string say_string
if non-empty, this string is displayed in the GUI
Definition: stage.hh:1841
uint32_t id
Definition: stage.hh:1794
void DrawStatusTree(Camera *cam)
Definition: model_draw.cc:273
class Stg::Model::Visibility vis
kg_t mass
Definition: stage.hh:1800
void Load(Worldfile *wf, int wf_entity)
Definition: stage.hh:2121
uint64_t trail_interval
Definition: stage.hh:1877
virtual void UpdateCharge()
Definition: model.cc:676
Definition: option.hh:19
Definition: options_dlg.hh:20
Definition: stage.hh:1374
virtual void SetProjection(void) const
Definition: camera.cc:113
void addYaw(double yaw)
Definition: stage.hh:1394
void setScale(double scale)
Definition: stage.hh:1404
void reset(void)
Definition: stage.hh:1412
void move(double x, double y)
Definition: camera.cc:135
void Load(Worldfile *wf, int sec)
Definition: camera.cc:198
void addPitch(double pitch)
Definition: stage.hh:1395
virtual void Draw() const
Definition: camera.cc:101
void setYaw(double yaw)
Definition: stage.hh:1392
double scale() const
Definition: stage.hh:1413
void setPose(double x, double y)
Definition: stage.hh:1405
void setPitch(double pitch)
Definition: stage.hh:1393
void Save(Worldfile *wf, int sec)
Definition: camera.cc:205
OrthoCamera(void)
Definition: stage.hh:1383
Definition: stage.hh:1296
virtual void Draw(void) const
Definition: camera.cc:38
virtual void SetProjection(void) const
Definition: camera.cc:50
double farClip(void) const
Definition: stage.hh:1357
double realDistance(double z_buf_val) const
Definition: stage.hh:1351
void setPitch(double pitch)
Definition: stage.hh:1341
void setPose(double x, double y, double z)
Definition: stage.hh:1315
double horizFov(void) const
Definition: stage.hh:1338
void setAspect(double aspect)
update vertical fov based on window aspect and current horizontal fov
Definition: stage.hh:1336
void setYaw(double yaw)
Definition: stage.hh:1337
void setFov(double horiz_fov, double vert_fov)
Definition: stage.hh:1330
double nearClip(void) const
Definition: stage.hh:1356
void addYaw(double yaw)
Definition: stage.hh:1340
PerspectiveCamera(void)
Definition: camera.cc:16
void addPitch(double pitch)
Definition: stage.hh:1342
void Load(Worldfile *wf, int sec)
Definition: camera.cc:86
void strafe(double amount)
Definition: camera.cc:74
void move(double x, double y, double z)
Definition: camera.cc:22
double vertFov(void) const
Definition: stage.hh:1339
void reset()
Definition: stage.hh:1364
void Save(Worldfile *wf, int sec)
Definition: camera.cc:92
void scroll(double dy)
Definition: stage.hh:1355
void forward(double amount)
Definition: camera.cc:80
void update(void)
Definition: camera.cc:70
void addPose(double x, double y, double z)
Definition: stage.hh:1321
void setClip(double near, double far)
Definition: stage.hh:1358
Definition: stage.hh:257
meters_t x
Definition: stage.hh:259
bool operator==(const Pose &other) const
Definition: stage.hh:314
virtual void Print(const char *prefix) const
Definition: stage.hh:278
Pose(meters_t x, meters_t y, meters_t z, radians_t a)
Definition: stage.hh:262
Pose & Load(Worldfile *wf, int section, const char *keyword)
Definition: model.cc:175
meters_t y
Definition: stage.hh:259
meters_t z
location in 3 axes
Definition: stage.hh:259
virtual ~Pose()
Definition: stage.hh:266
meters_t Distance(const Pose &other) const
Definition: stage.hh:324
Pose operator+(const Pose &p) const
Definition: stage.hh:297
bool operator!=(const Pose &other) const
Definition: stage.hh:319
std::string String() const
Definition: stage.hh:283
Pose()
Definition: stage.hh:264
bool IsZero() const
Definition: stage.hh:291
bool operator<(const Pose &p) const
a < b iff a is closer to the origin than b
Definition: stage.hh:307
void Zero()
Definition: stage.hh:293
radians_t a
rotation about the z axis.
Definition: stage.hh:260
void Save(Worldfile *wf, int section, const char *keyword)
Definition: model.cc:182
static Pose Random(meters_t xmin, meters_t xmax, meters_t ymin, meters_t ymax)
Definition: stage.hh:269
Definition: stage.hh:1553
void Accumulate(meters_t x, meters_t y, joules_t amount)
Definition: powerpack.cc:296
virtual ~DissipationVis()
Definition: powerpack.cc:264
virtual void Visualize(Model *mod, Camera *cam)
Definition: powerpack.cc:268
DissipationVis(meters_t width, meters_t height, meters_t cellsize)
Definition: powerpack.cc:257
Definition: stage.hh:1548
void Add(joules_t j)
Definition: powerpack.cc:153
joules_t GetDissipated() const
Definition: powerpack.cc:222
Model * mod
Definition: stage.hh:1578
joules_t GetStored() const
Definition: powerpack.cc:217
joules_t stored
Definition: stage.hh:1581
static joules_t global_input
Definition: stage.hh:1601
joules_t RemainingCapacity() const
Definition: powerpack.cc:148
static joules_t global_capacity
Definition: stage.hh:1599
StripPlotVis output_vis
Definition: stage.hh:1574
bool GetCharging() const
Definition: stage.hh:1640
void SetStored(joules_t j)
Definition: powerpack.cc:227
joules_t last_joules
Definition: stage.hh:1594
PowerPack(Model *mod)
Definition: powerpack.cc:17
void ChargeStart()
Definition: stage.hh:1641
usec_t last_time
Definition: stage.hh:1593
void ChargeStop()
Definition: stage.hh:1642
void Subtract(joules_t j)
Definition: powerpack.cc:163
void TransferTo(PowerPack *dest, joules_t amount)
Definition: powerpack.cc:177
void Print(const char *prefix) const
Definition: stage.hh:1625
StripPlotVis stored_vis
Definition: stage.hh:1575
watts_t last_watts
Definition: stage.hh:1595
double ProportionRemaining() const
Definition: stage.hh:1622
bool charging
Definition: stage.hh:1587
~PowerPack()
Definition: powerpack.cc:38
void Dissipate(joules_t j)
Definition: powerpack.cc:234
joules_t dissipated
Definition: stage.hh:1590
static joules_t global_dissipated
Definition: stage.hh:1600
joules_t capacity
Definition: stage.hh:1584
joules_t GetCapacity() const
Definition: powerpack.cc:212
void SetCapacity(joules_t j)
Definition: powerpack.cc:199
void Visualize(Camera *cam)
Definition: powerpack.cc:47
Stg::PowerPack::DissipationVis event_vis
static joules_t global_stored
Definition: stage.hh:1598
Definition: stage.hh:711
ray_test_func_t func
Definition: stage.hh:723
const Model * mod
Definition: stage.hh:720
meters_t range
Definition: stage.hh:722
bool ztest
Definition: stage.hh:725
Pose origin
Definition: stage.hh:721
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:713
const void * arg
Definition: stage.hh:724
Ray()
Definition: stage.hh:719
Definition: stage.hh:327
RaytraceResult()
Definition: stage.hh:334
Color color
Definition: stage.hh:331
Model * mod
Definition: stage.hh:330
meters_t range
Definition: stage.hh:332
Pose pose
Definition: stage.hh:329
RaytraceResult(const Pose &pose, Model *mod, const Color &color, const meters_t range)
Definition: stage.hh:335
Definition: region.hh:65
Definition: stage.hh:241
meters_t x
Definition: stage.hh:243
void Save(Worldfile *wf, int section, const char *keyword) const
Definition: model.cc:170
meters_t z
Definition: stage.hh:243
Size()
Definition: stage.hh:248
Size & Load(Worldfile *wf, int section, const char *keyword)
Definition: model.cc:164
void Zero()
Definition: stage.hh:253
meters_t y
Definition: stage.hh:243
Size(meters_t x, meters_t y, meters_t z)
Definition: stage.hh:245
Definition: stage.hh:1530
virtual ~StripPlotVis()
Definition: vis_strip.cc:18
virtual void Visualize(Model *mod, Camera *cam)
Definition: vis_strip.cc:24
void AppendValue(float value)
Definition: vis_strip.cc:44
StripPlotVis(float x, float y, float w, float h, size_t len, Color fgcolor, Color bgcolor, const char *name, const char *wfname)
Definition: vis_strip.cc:9
Definition: region.hh:98
Definition: stage.hh:343
Velocity & Load(Worldfile *wf, int section, const char *keyword)
Definition: stage.hh:354
virtual void Print(const char *prefix) const
Definition: stage.hh:365
Velocity(double x, double y, double z, double a)
Definition: stage.hh:350
Velocity()
Definition: stage.hh:352
Definition: stage.hh:520
virtual ~Visualizer(void)
Definition: stage.hh:531
Visualizer(const std::string &menu_name, const std::string &worldfile_name)
Definition: stage.hh:526
virtual void Visualize(Model *mod, Camera *cam)=0
const std::string & GetMenuName()
Definition: stage.hh:534
const std::string & GetWorldfileName()
Definition: stage.hh:535
Definition: stage.hh:1421
virtual void PopColor()
Definition: worldgui.cc:875
usec_t RealTimeNow(void) const
Definition: worldgui.cc:885
virtual bool IsGUI() const
Definition: stage.hh:1509
virtual void PushColor(Color col)
dummy implementations to be overloaded by GUI subclasses
Definition: worldgui.cc:865
virtual void Stop()
Definition: worldgui.cc:637
bool IsTopView()
Definition: worldgui.cc:892
void DrawOccupancy() const
Definition: worldgui.cc:458
virtual bool Load(const std::string &worldfile_path)
Definition: worldgui.cc:278
virtual void RemoveChild(Model *mod)
Definition: worldgui.cc:439
std::string EnergyString(void) const
Definition: worldgui.cc:445
virtual void Start()
Definition: worldgui.cc:612
Canvas * GetCanvas(void) const
Definition: stage.hh:1519
void Show()
Definition: worldgui.cc:254
void DrawVoxels() const
Definition: worldgui.cc:480
virtual Model * RecentlySelectedModel() const
Definition: worldgui.cc:880
~WorldGui()
Definition: worldgui.cc:244
virtual bool Save(const char *filename)
Definition: worldgui.cc:351
virtual std::string ClockString() const
Definition: worldgui.cc:415
WorldGui(int width, int height, const char *caption=NULL)
Definition: worldgui.cc:191
virtual void Redraw(void)
Definition: worldgui.cc:606
virtual void UnLoad()
Definition: worldgui.cc:346
void DrawBoundingBoxTree()
Definition: worldgui.cc:859
virtual bool Update()
Definition: worldgui.cc:384
Definition: stage.hh:971
Event(usec_t time, Model *mod, model_callback_t cb, void *arg)
Definition: stage.hh:973
void * arg
Definition: stage.hh:981
usec_t time
time that event occurs
Definition: stage.hh:978
model_callback_t cb
Definition: stage.hh:980
bool operator<(const Event &other) const
Definition: world.cc:1128
Model * mod
model to pass into callback
Definition: stage.hh:979
World class
Definition: stage.hh:764
void CancelQuitAll()
Definition: stage.hh:1103
void Enqueue(unsigned int queue_num, usec_t delay, Model *mod, model_callback_t cb, void *arg)
Definition: stage.hh:1005
void RegisterOption(Option *opt)
Register an Option for pickup by the GUI.
Definition: world.cc:1115
point_int_t MetersToPixels(const point_t &pt) const
Definition: stage.hh:918
void EnableEnergy(Model *m)
Definition: stage.hh:1012
virtual bool Save(const char *filename)
Definition: world.cc:971
void LoadSensor(Worldfile *wf, int entity)
Definition: world.cc:294
void AddUpdateCallback(world_callback_t cb, void *user)
Definition: world.cc:524
virtual void AddModel(Model *mod)
Definition: world.cc:264
void DisableEnergy(Model *m)
Definition: stage.hh:1013
void DestroySuperRegion(SuperRegion *sr)
Definition: world.cc:184
virtual void Stop()
Definition: stage.hh:866
int update_cb_count
Definition: stage.hh:1023
std::vector< std::priority_queue< Event > > event_queues
Definition: stage.hh:989
static void * update_thread_entry(std::pair< World *, int > *info)
Definition: world.cc:228
bool graphics
true iff we have a GUI
Definition: stage.hh:845
usec_t SimTimeNow(void) const
Definition: stage.hh:1049
friend class Model
Definition: stage.hh:767
uint64_t GetUpdateCount() const
Definition: stage.hh:1118
bool Paused() const
Definition: stage.hh:868
void RecordRay(double x1, double y1, double x2, double y2)
Definition: world.cc:701
void AddModelName(Model *mod, const std::string &name)
Definition: world.cc:270
uint64_t updates
the number of simulated time steps executed so far
Definition: stage.hh:856
void LoadBlock(Worldfile *wf, int entity)
Definition: world.cc:283
void NeedRedraw()
Definition: stage.hh:890
int RemoveUpdateCallback(world_callback_t cb, void *user)
Definition: world.cc:530
bool TestQuit() const
Definition: stage.hh:1095
RaytraceResult Raytrace(const Ray &ray)
Definition: world.cc:756
std::vector< point_int_t > rt_cells
Definition: stage.hh:873
virtual void UnLoad()
Definition: world.cc:474
double Resolution() const
Definition: stage.hh:1108
World(const std::string &name="MyWorld", double ppm=DEFAULT_PPM)
Definition: world.cc:128
void TryCharge(PowerPack *pp, const Pose &pose)
void CancelQuit()
Definition: stage.hh:1101
friend class WorkerThread
Definition: stage.hh:770
bounds3d_t extent
Describes the 3D volume of the world.
Definition: stage.hh:844
virtual void TogglePause()
Definition: stage.hh:867
virtual void PushColor(Color col)
dummy implementations to be overloaded by GUI subclasses
Definition: stage.hh:924
void Log(Model *mod)
Definition: world.cc:1121
std::set< Option * > option_table
GUI options (toggles) registered by models.
Definition: stage.hh:847
virtual void Redraw(void)
Definition: stage.hh:872
uint64_t UpdateCount()
Definition: stage.hh:862
virtual Model * RecentlySelectedModel() const
Definition: stage.hh:905
SuperRegion * GetSuperRegionCreate(const point_int_t &org)
Definition: world.cc:1082
virtual void Reload()
Definition: world.cc:984
usec_t quit_time
Definition: stage.hh:851
virtual bool Load(const std::string &worldfile_path)
Definition: world.cc:380
virtual std::string ClockString(void) const
Definition: world.cc:498
void ConsumeQueue(unsigned int queue_num)
Definition: world.cc:579
virtual ~World()
Definition: world.cc:166
SuperRegion * GetSuperRegion(const point_int_t &org)
Definition: world.cc:1069
void LoadModel(Worldfile *wf, int entity)
Definition: world.cc:339
std::map< point_int_t, SuperRegion * > superregions
Definition: stage.hh:854
virtual void RemoveModel(Model *mod)
Definition: world.cc:275
static const int DEFAULT_PPM
Definition: stage.hh:876
virtual void PopColor()
Definition: stage.hh:933
std::vector< point_int_t > rt_candidate_cells
Definition: stage.hh:874
std::set< ModelPosition * > active_velocity
Definition: stage.hh:1015
void AddPowerPack(PowerPack *pp)
Definition: world.cc:1104
void QuitAll()
Definition: stage.hh:1099
const bounds3d_t & GetExtent() const
Definition: stage.hh:1116
usec_t sim_time
the current sim time in this world in microseconds
Definition: stage.hh:853
static std::vector< std::string > args
Definition: stage.hh:775
std::vector< std::queue< Model * > > pending_update_callbacks
Definition: stage.hh:992
std::set< Model * > active_energy
Definition: stage.hh:1011
usec_t sim_interval
Definition: stage.hh:1018
virtual bool IsGUI() const
Definition: stage.hh:1056
virtual bool Update(void)
Definition: world.cc:605
static std::string ctrlargs
Definition: stage.hh:776
static void Run()
Definition: world.cc:190
Worldfile * GetWorldFile()
Definition: stage.hh:1052
virtual void PushColor(double r, double g, double b, double a)
Definition: stage.hh:925
int32_t MetersToPixels(meters_t x) const
Definition: stage.hh:916
Model * ground
Definition: stage.hh:892
SuperRegion * CreateSuperRegion(point_int_t origin)
Definition: world.cc:176
bool paused
if true, the simulation is stopped
Definition: stage.hh:863
static bool UpdateAll()
Definition: world.cc:216
unsigned int GetEventQueue(Model *mod) const
Definition: world.cc:678
bool PastQuitTime()
Definition: world.cc:493
Model * GetModel(const std::string &name) const
Definition: world.cc:688
void RemovePowerPack(PowerPack *pp)
Definition: world.cc:1109
virtual void Start()
Definition: stage.hh:865
Model * CreateModel(Model *parent, const std::string &typestr)
Definition: world.cc:307
std::list< std::pair< world_callback_t, void * > > cb_list
List of callback functions and arguments.
Definition: stage.hh:843
const std::set< Model * > GetAllModels() const
Definition: stage.hh:1114
SuperRegion * AddSuperRegion(const point_int_t &coord)
Definition: world.cc:1057
Model * GetGround()
Definition: stage.hh:1125
void Quit()
Definition: stage.hh:1097
void LoadBlockGroup(Worldfile *wf, int entity)
std::list< float * > ray_list
List of rays traced for debug visualization.
Definition: stage.hh:852
std::list< PowerPack * > powerpack_list
List of all the powerpacks attached to models in the world.
Definition: stage.hh:849
Worldfile * wf
If set, points to the worldfile used to create this world.
Definition: stage.hh:857
void ShowClock(bool enable)
Control printing time to stdout.
Definition: stage.hh:1123
void Extend(point3_t pt)
Definition: world.cc:1094
void MapPoly(const std::vector< point_int_t > &poly, Block *block, unsigned int layer)
Definition: world.cc:990
void CallUpdateCallbacks()
Call all calbacks in cb_list, removing any that return true;.
Definition: world.cc:546
void ClearRays()
Definition: world.cc:711
Definition: worldfile.hh:67
Definition: stage.hh:424
Bounds x
volume extent along x axis, intially zero
Definition: stage.hh:427
Bounds z
volume extent along z axis, initially zero
Definition: stage.hh:431
bounds3d_t()
Definition: stage.hh:433
bounds3d_t(const Bounds &x, const Bounds &y, const Bounds &z)
Definition: stage.hh:434
Bounds y
volume extent along y axis, initially zero
Definition: stage.hh:429
Definition: stage.hh:464
meters_t y
Definition: stage.hh:466
point3_t()
Definition: stage.hh:468
meters_t x
Definition: stage.hh:466
point3_t(meters_t x, meters_t y, meters_t z)
Definition: stage.hh:467
meters_t z
Definition: stage.hh:466
Definition: stage.hh:472
point_int_t()
Definition: stage.hh:476
bool operator<(const point_int_t &other) const
Definition: stage.hh:478
bool operator==(const point_int_t &other) const
Definition: stage.hh:487
int y
Definition: stage.hh:474
int x
Definition: stage.hh:474
point_int_t(int x, int y)
Definition: stage.hh:475
Definition: stage.hh:444
bool operator+=(const point_t &other)
Definition: stage.hh:449
bool operator<(const point_t &other) const
Definition: stage.hh:451
point_t()
Definition: stage.hh:448
meters_t y
Definition: stage.hh:446
point_t(meters_t x, meters_t y)
Definition: stage.hh:447
meters_t x
Definition: stage.hh:446
bool operator==(const point_t &other) const
Definition: stage.hh:460
static char * argv
Definition: glutgraphics.cc:216
float s
Definition: glutgraphics.cc:51
static int argc
Definition: glutgraphics.cc:215
void pose_shift(const Pose &pose)
Definition: gl.cc:13
void coord_shift(double x, double y, double z, double a)
Definition: gl.cc:6
void draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset, float min, float max)
Definition: gl.cc:24
void draw_centered_rect(float x, float y, float dx, float dy)
Definition: gl.cc:120
void draw_grid(bounds3d_t vol)
Definition: gl.cc:140
void draw_string(float x, float y, float z, const char *string)
Definition: gl.cc:61
void draw_octagon(float w, float h, float m)
Definition: gl.cc:90
void draw_speech_bubble(float x, float y, float z, const char *str)
Definition: gl.cc:83
void draw_string_multiline(float x, float y, float w, float h, const char *string, Fl_Align align)
Definition: gl.cc:75
void pose_inverse_shift(const Pose &pose)
Definition: gl.cc:18
void draw_origin(double len)
Definition: gl.cc:133
void draw_vector(double x, double y, double z)
Definition: gl.cc:125
The Stage library uses its own namespace.
Definition: canvas.hh:8
int polys_from_image_file(const std::string &filename, std::vector< std::vector< point_t > > &polys)
rotated rectangle
Definition: stage.cc:86
double radians_t
Definition: stage.hh:194
bool InitDone()
Definition: stage.cc:40
double meters_t
Definition: stage.hh:191
const double million
Definition: stage.hh:145
double normalize(double a)
Definition: stage.hh:163
uint64_t usec_t
Definition: stage.hh:203
int(* model_callback_t)(Model *mod, void *user)
Definition: stage.hh:540
unsigned long msec_t
Definition: stage.hh:200
void Init(int *argc, char **argv[])
Definition: stage.cc:17
const double thousand
Definition: stage.hh:142
const char * Version()
Definition: stage.cc:12
struct timeval time_t
Definition: stage.hh:197
void EraseAll(T thing, C &cont)
Definition: stage.hh:584
double joules_t
Definition: stage.hh:209
uint32_t id_t
Definition: stage.hh:188
const double billion
Definition: stage.hh:148
bool(* ray_test_func_t)(Model *candidate, const Model *finder, const void *arg)
Definition: stage.hh:569
void RegisterModels()
Definition: typetable.cc:16
const char WEBSITE[]
Definition: stage.hh:113
@ FiducialNone
Definition: stage.hh:185
double watts_t
Definition: stage.hh:212
const char DESCRIPTION[]
Definition: stage.hh:116
const char COPYRIGHT[]
Definition: stage.hh:105
int sgn(int a)
Definition: stage.hh:173
int(* world_callback_t)(World *world, void *user)
Definition: stage.hh:542
double kg_t
Definition: stage.hh:206
double constrain(double val, double minval, double maxval)
return val, or minval if val < minval, or maxval if val > maxval
Definition: stage.cc:236
const char AUTHORS[]
Definition: stage.hh:108
double dtor(double d)
Definition: stage.hh:157
const char LICENSE[]
Definition: stage.hh:119
double rtod(double r)
Definition: stage.hh:151
point_t * unit_square_points_create()
Definition: stage.cc:219
unsigned int FullVersion()
#define PRINT_WARN(m)
Definition: stage.hh:603
#define PRINT_ERR(m)
Definition: stage.hh:590
Definition: stage.hh:2700
GLfloat x
Definition: stage.hh:2702
Definition: stage.hh:2441
lift_state_t lift
Definition: stage.hh:2444
Model * beam[2]
points to a model detected by the beams
Definition: stage.hh:2453
paddle_state_t paddles
Definition: stage.hh:2443
double close_limit
How far the gripper can close. If < 1.0, the gripper has its mouth full.
Definition: stage.hh:2450
Size paddle_size
paddle dimensions
Definition: stage.hh:2442
Model * contact[2]
pointers to a model detected by the contacts
Definition: stage.hh:2454
Model * gripped
Definition: stage.hh:2447
double paddle_position
0.0 = full open, 1.0 full closed
Definition: stage.hh:2445
bool paddles_stalled
true iff some solid object stopped the paddles closing or opening
Definition: stage.hh:2448
double lift_position
0.0 = full down, 1.0 full up
Definition: stage.hh:2446
double break_beam_inset[2]
distance from the end of the paddle
Definition: stage.hh:2452
bool autosnatch
if true, cycle the gripper through open-close-up-down automatically
Definition: stage.hh:2451
Definition: stage.hh:547
Color color
Definition: stage.hh:551
double duty_cycle
mark/space ratio
Definition: stage.hh:553
msec_t period
duration of a complete cycle
Definition: stage.hh:552
int enabled
Definition: stage.hh:548
Pose pose
Definition: stage.hh:549
meters_t size
rendered as a sphere with this diameter
Definition: stage.hh:550
Definition: stage.hh:438
Bounds range
min and max range of sensor
Definition: stage.hh:439
radians_t angle
width of viewing angle of sensor
Definition: stage.hh:440
Definition: stage.hh:557
Size size
Definition: stage.hh:559
Pose pose
Definition: stage.hh:558