Classes | Public Member Functions | Static Public Member Functions | Public Attributes | Static Public Attributes | Protected Member Functions | Protected Attributes | Private Member Functions | Static Private Member Functions | Private Attributes | Static Private Attributes | Friends
Stg::World Class Reference

World class More...

#include <stage.hh>

Inheritance diagram for Stg::World:
Inheritance graph
[legend]

List of all members.

Classes

class  Event
struct  ltx
struct  lty

Public Member Functions

virtual void AddModel (Model *mod)
void AddModelName (Model *mod, const std::string &name)
void AddPowerPack (PowerPack *pp)
SuperRegionAddSuperRegion (const point_int_t &coord)
void AddUpdateCallback (world_callback_t cb, void *user)
void CancelQuit ()
void CancelQuitAll ()
void ClearRays ()
virtual std::string ClockString (void) const
void ConsumeQueue (unsigned int queue_num)
ModelCreateModel (Model *parent, const std::string &typestr)
SuperRegionCreateSuperRegion (point_int_t origin)
void DestroySuperRegion (SuperRegion *sr)
void DisableEnergy (Model *m)
void EnableEnergy (Model *m)
void Enqueue (unsigned int queue_num, usec_t delay, Model *mod, model_callback_t cb, void *arg)
void Extend (point3_t pt)
const std::set< Model * > GetAllModels () const
unsigned int GetEventQueue (Model *mod) const
const bounds3d_tGetExtent () const
ModelGetGround ()
ModelGetModel (const std::string &name) const
SuperRegionGetSuperRegion (const point_int_t &org)
SuperRegionGetSuperRegionCreate (const point_int_t &org)
uint64_t GetUpdateCount () const
WorldfileGetWorldFile ()
virtual bool IsGUI () const
virtual void Load (const std::string &worldfile_path)
void LoadBlock (Worldfile *wf, int entity)
void LoadBlockGroup (Worldfile *wf, int entity)
void LoadModel (Worldfile *wf, int entity)
void LoadSensor (Worldfile *wf, int entity)
void Log (Model *mod)
void MapPoly (const std::vector< point_int_t > &poly, Block *block, unsigned int layer)
int32_t MetersToPixels (meters_t x) const
point_int_t MetersToPixels (const point_t &pt) const
void NeedRedraw ()
bool PastQuitTime ()
bool Paused () const
virtual void PopColor ()
virtual void PushColor (Color col)
virtual void PushColor (double r, double g, double b, double a)
void Quit ()
void QuitAll ()
RaytraceResult Raytrace (const Ray &ray)
RaytraceResult Raytrace (const Pose &pose, const meters_t range, const ray_test_func_t func, const Model *finder, const void *arg, const bool ztest)
void Raytrace (const Pose &pose, const meters_t range, const radians_t fov, const ray_test_func_t func, const Model *finder, const void *arg, RaytraceResult *samples, const uint32_t sample_count, const bool ztest)
virtual ModelRecentlySelectedModel () const
void RecordRay (double x1, double y1, double x2, double y2)
virtual void Redraw (void)
void RegisterOption (Option *opt)
 Register an Option for pickup by the GUI.
virtual void Reload ()
virtual void RemoveModel (Model *mod)
void RemovePowerPack (PowerPack *pp)
int RemoveUpdateCallback (world_callback_t cb, void *user)
double Resolution () const
virtual bool Save (const char *filename)
void ShowClock (bool enable)
 Control printing time to stdout.
usec_t SimTimeNow (void) const
virtual void Start ()
virtual void Stop ()
bool TestQuit () const
virtual void TogglePause ()
void TryCharge (PowerPack *pp, const Pose &pose)
virtual void UnLoad ()
virtual bool Update (void)
uint64_t UpdateCount ()
 World (const std::string &name="MyWorld", double ppm=DEFAULT_PPM)
virtual ~World ()

Static Public Member Functions

static void Run ()
static void * update_thread_entry (std::pair< World *, int > *info)
static bool UpdateAll ()

Public Attributes

std::set< Model * > active_energy
std::set< ModelPosition * > active_velocity
std::vector
< std::priority_queue< Event > > 
event_queues
Modelground
bool paused
 if true, the simulation is stopped
std::vector< std::queue< Model * > > pending_update_callbacks
std::vector< point_int_trt_candidate_cells
std::vector< point_int_trt_cells
usec_t sim_interval
int update_cb_count

Static Public Attributes

static std::vector< std::string > args
static std::string ctrlargs
static const int DEFAULT_PPM = 50

Protected Member Functions

void CallUpdateCallbacks ()
 Call all calbacks in cb_list, removing any that return true;.

Protected Attributes

std::list< std::pair
< world_callback_t, void * > > 
cb_list
 List of callback functions and arguments.
bounds3d_t extent
 Describes the 3D volume of the world.
bool graphics
 true iff we have a GUI
std::set< Option * > option_table
 GUI options (toggles) registered by models.
std::list< PowerPack * > powerpack_list
 List of all the powerpacks attached to models in the world.
usec_t quit_time
std::list< float * > ray_list
 List of rays traced for debug visualization.
usec_t sim_time
 the current sim time in this world in microseconds
std::map< point_int_t,
SuperRegion * > 
superregions
std::vector< std::vector
< Model * > > 
update_lists
uint64_t updates
 the number of simulated time steps executed so far
Worldfilewf
 If set, points to the worldfile used to create this world.

Private Member Functions

void FiducialErase (Model *mod)
void FiducialInsert (Model *mod)

Static Private Member Functions

static void UpdateCb (World *world)

Private Attributes

bool destroy
bool dirty
 iff true, a gui redraw would be required
std::set< Model * > models
std::map< std::string, Model * > models_by_name
std::map< int, Model * > models_by_wfentity
std::vector< Model * > models_with_fiducials
std::set< Model *, ltxmodels_with_fiducials_byx
std::set< Model *, ltymodels_with_fiducials_byy
double ppm
 the resolution of the world model in pixels per meter
bool quit
 quit this world ASAP
bool show_clock
 iff true, print the sim time on stdout
unsigned int show_clock_interval
 updates between clock outputs
pthread_mutex_t sync_mutex
 protect the worker thread management stuff
pthread_cond_t threads_done_cond
 signalled by last worker thread to unblock main thread
pthread_cond_t threads_start_cond
 signalled to unblock worker threads
unsigned int threads_working
 the number of worker threads not yet finished
int total_subs
 the total number of subscriptions to all models
unsigned int worker_threads
 the number of worker threads to use

Static Private Attributes

static unsigned int next_id
 initially zero, used to allocate unique sequential world ids
static bool quit_all
 quit all worlds ASAP
static std::set< World * > world_set
 all the worlds that exist

Friends

class Block
class Canvas
class Model
class ModelFiducial
class WorkerThread

Detailed Description

World class

Definition at line 814 of file stage.hh.


Constructor & Destructor Documentation

World::World ( const std::string &  name = "MyWorld",
double  ppm = DEFAULT_PPM 
)

Definition at line 123 of file world.cc.

World::~World ( void  ) [virtual]

Definition at line 185 of file world.cc.


Member Function Documentation

void World::AddModel ( Model mod) [virtual]

Reimplemented in Stg::WorldGui.

Definition at line 284 of file world.cc.

void World::AddModelName ( Model mod,
const std::string &  name 
)

Definition at line 290 of file world.cc.

Definition at line 1149 of file world.cc.

Definition at line 1095 of file world.cc.

void World::AddUpdateCallback ( world_callback_t  cb,
void *  user 
)

Attach a callback function, to be called with the argument at the end of a complete update step

Definition at line 538 of file world.cc.

void World::CallUpdateCallbacks ( ) [protected]

Call all calbacks in cb_list, removing any that return true;.

Definition at line 565 of file world.cc.

void Stg::World::CancelQuit ( ) [inline]

Cancel a local quit request.

Definition at line 1162 of file stage.hh.

void Stg::World::CancelQuitAll ( ) [inline]

Cancel a global quit request.

Definition at line 1165 of file stage.hh.

void World::ClearRays ( )

Definition at line 732 of file world.cc.

std::string World::ClockString ( void  ) const [virtual]

Get human readable string that describes the current simulation time.

Reimplemented in Stg::WorldGui.

Definition at line 511 of file world.cc.

void World::ConsumeQueue ( unsigned int  queue_num)

consume events from the queue up to and including the current sim_time

Definition at line 599 of file world.cc.

Model * World::CreateModel ( Model parent,
const std::string &  typestr 
)

Definition at line 328 of file world.cc.

Definition at line 193 of file world.cc.

Definition at line 201 of file world.cc.

void Stg::World::DisableEnergy ( Model m) [inline]

Definition at line 1081 of file stage.hh.

void Stg::World::EnableEnergy ( Model m) [inline]

Definition at line 1080 of file stage.hh.

void Stg::World::Enqueue ( unsigned int  queue_num,
usec_t  delay,
Model mod,
model_callback_t  cb,
void *  arg 
) [inline]

Create a new simulation event to be handled in the future.

Parameters:
queue_numSpecify which queue the event should be on. The main thread is 0.
delayThe time from now until the event occurs, in microseconds.
modThe model that should have its Update() method called at the specified time.

Definition at line 1075 of file stage.hh.

void World::Extend ( point3_t  pt) [inline]

Enlarge the bounding volume to include this point

Definition at line 1138 of file world.cc.

void Stg::World::FiducialErase ( Model mod) [inline, private]

Remove a model from the set of models with non-zero fiducials, if it exists.

Definition at line 878 of file stage.hh.

void Stg::World::FiducialInsert ( Model mod) [inline, private]

Add a model to the set of models with non-zero fiducials, if not already there.

Definition at line 871 of file stage.hh.

const std::set<Model*> Stg::World::GetAllModels ( ) const [inline]

Returns a const reference to the set of models in the world.

Definition at line 1178 of file stage.hh.

unsigned int World::GetEventQueue ( Model mod) const

returns an event queue index number for a model to use for updates

Definition at line 697 of file world.cc.

const bounds3d_t& Stg::World::GetExtent ( ) const [inline]

Return the 3D bounding box of the world, in meters

Definition at line 1181 of file stage.hh.

Return the floor model

Definition at line 1193 of file stage.hh.

Model * World::GetModel ( const std::string &  name) const

Returns a pointer to the model identified by name, or NULL if nonexistent

Definition at line 707 of file world.cc.

SuperRegion * World::GetSuperRegion ( const point_int_t org) [inline]

Definition at line 1112 of file world.cc.

Definition at line 1125 of file world.cc.

uint64_t Stg::World::GetUpdateCount ( ) const [inline]

Return the number of times the world has been updated.

Definition at line 1184 of file stage.hh.

Returns a pointer to the currently-open worlddfile object, or NULL if there is none.

Definition at line 1123 of file stage.hh.

virtual bool Stg::World::IsGUI ( ) const [inline, virtual]

Returns true iff this World implements a GUI. The base World class returns false, but subclasses can override this behaviour.

Reimplemented in Stg::WorldGui.

Definition at line 1128 of file stage.hh.

void World::Load ( const std::string &  worldfile_path) [virtual]

Open the file at the specified location, create a Worldfile object, read the file and configure the world from the contents, creating models as necessary. The created object persists, and can be retrieved later with World::GetWorldFile().

Reimplemented in Stg::WorldGui.

Definition at line 388 of file world.cc.

void World::LoadBlock ( Worldfile wf,
int  entity 
)

Definition at line 303 of file world.cc.

void Stg::World::LoadBlockGroup ( Worldfile wf,
int  entity 
)
void World::LoadModel ( Worldfile wf,
int  entity 
)

Definition at line 367 of file world.cc.

void World::LoadSensor ( Worldfile wf,
int  entity 
)

Definition at line 314 of file world.cc.

void World::Log ( Model mod)

Log the state of a Model

Definition at line 1166 of file world.cc.

void World::MapPoly ( const std::vector< point_int_t > &  poly,
Block block,
unsigned int  layer 
)

call Cell::AddBlock(block) for each cell on the polygon

Definition at line 1021 of file world.cc.

int32_t Stg::World::MetersToPixels ( meters_t  x) const [inline]

convert a distance in meters to a distance in world occupancy grid pixels

Definition at line 981 of file stage.hh.

point_int_t Stg::World::MetersToPixels ( const point_t pt) const [inline]

Definition at line 984 of file stage.hh.

void Stg::World::NeedRedraw ( ) [inline]

hint that the world needs to be redrawn if a GUI is attached

Definition at line 951 of file stage.hh.

Returns true iff the current time is greater than the time we should quit

Definition at line 506 of file world.cc.

bool Stg::World::Paused ( ) const [inline]

Definition at line 927 of file stage.hh.

virtual void Stg::World::PopColor ( ) [inline, virtual]

Reimplemented in Stg::WorldGui.

Definition at line 993 of file stage.hh.

virtual void Stg::World::PushColor ( Color  col) [inline, virtual]

Reimplemented in Stg::WorldGui.

Definition at line 988 of file stage.hh.

virtual void Stg::World::PushColor ( double  r,
double  g,
double  b,
double  a 
) [inline, virtual]

Reimplemented in Stg::WorldGui.

Definition at line 990 of file stage.hh.

void Stg::World::Quit ( ) [inline]

Request the world quits simulation before the next timestep.

Definition at line 1156 of file stage.hh.

void Stg::World::QuitAll ( ) [inline]

Requests all worlds quit simulation before the next timestep.

Definition at line 1159 of file stage.hh.

trace a ray.

Definition at line 775 of file world.cc.

RaytraceResult World::Raytrace ( const Pose pose,
const meters_t  range,
const ray_test_func_t  func,
const Model finder,
const void *  arg,
const bool  ztest 
)

Definition at line 764 of file world.cc.

void World::Raytrace ( const Pose pose,
const meters_t  range,
const radians_t  fov,
const ray_test_func_t  func,
const Model finder,
const void *  arg,
RaytraceResult samples,
const uint32_t  sample_count,
const bool  ztest 
)

Definition at line 742 of file world.cc.

virtual Model* Stg::World::RecentlySelectedModel ( ) const [inline, virtual]

Reimplemented in Stg::WorldGui.

Definition at line 967 of file stage.hh.

void World::RecordRay ( double  x1,
double  y1,
double  x2,
double  y2 
)

store rays traced for debugging purposes

Definition at line 722 of file world.cc.

virtual void Stg::World::Redraw ( void  ) [inline, virtual]

Force the GUI to redraw the world, even if paused. This imlementation does nothing, but can be overridden by subclasses.

Reimplemented in Stg::WorldGui.

Definition at line 932 of file stage.hh.

void World::RegisterOption ( Option opt)

Register an Option for pickup by the GUI.

Definition at line 1160 of file world.cc.

void World::Reload ( void  ) [virtual]

Definition at line 1015 of file world.cc.

void World::RemoveModel ( Model mod) [virtual]

Definition at line 295 of file world.cc.

Definition at line 1154 of file world.cc.

int World::RemoveUpdateCallback ( world_callback_t  cb,
void *  user 
)

Remove a callback function. Any argument data passed to AddUpdateCallback is not automatically freed.

Definition at line 546 of file world.cc.

double Stg::World::Resolution ( ) const [inline]

Get the resolution in pixels-per-metre of the underlying discrete raytracing model

Definition at line 1171 of file stage.hh.

void World::Run ( ) [static]

run all worlds. If only non-gui worlds were created, UpdateAll() is repeatedly called. To simulate a gui world only a single gui world may have been created. This world is then simulated.

Definition at line 207 of file world.cc.

bool World::Save ( const char *  filename) [virtual]

Save the current world state into a worldfile with the given filename.

Parameters:
Filenameto save as.

Reimplemented in Stg::WorldGui.

Definition at line 1002 of file world.cc.

void Stg::World::ShowClock ( bool  enable) [inline]

Control printing time to stdout.

Definition at line 1190 of file stage.hh.

usec_t Stg::World::SimTimeNow ( void  ) const [inline]

Returns the current simulated time in this world, in microseconds.

Definition at line 1119 of file stage.hh.

virtual void Stg::World::Start ( ) [inline, virtual]

Reimplemented in Stg::WorldGui.

Definition at line 923 of file stage.hh.

virtual void Stg::World::Stop ( ) [inline, virtual]

Reimplemented in Stg::WorldGui.

Definition at line 924 of file stage.hh.

bool Stg::World::TestQuit ( ) const [inline]

Returns true iff either the local or global quit flag was set, which usually happens because someone called Quit() or QuitAll().

Definition at line 1153 of file stage.hh.

virtual void Stg::World::TogglePause ( ) [inline, virtual]

Definition at line 925 of file stage.hh.

void Stg::World::TryCharge ( PowerPack pp,
const Pose pose 
)
void World::UnLoad ( ) [virtual]

Reimplemented in Stg::WorldGui.

Definition at line 488 of file world.cc.

bool World::Update ( void  ) [virtual]

Run one simulation timestep. Advances the simulation clock, executes all simulation updates due at the current time, then queues up future events.

Reimplemented in Stg::WorldGui.

Definition at line 624 of file world.cc.

void * World::update_thread_entry ( std::pair< World *, int > *  info) [static]

Definition at line 246 of file world.cc.

bool World::UpdateAll ( ) [static]

returns true when time to quit, false otherwise

Definition at line 231 of file world.cc.

static void Stg::World::UpdateCb ( World world) [static, private]
uint64_t Stg::World::UpdateCount ( ) [inline]

Definition at line 919 of file stage.hh.


Friends And Related Function Documentation

friend class Block [friend]

Definition at line 817 of file stage.hh.

friend class Canvas [friend]

Reimplemented from Stg::Ancestor.

Reimplemented in Stg::WorldGui.

Definition at line 820 of file stage.hh.

friend class Model [friend]

Reimplemented in Stg::WorldGui.

Definition at line 818 of file stage.hh.

friend class ModelFiducial [friend]

Definition at line 819 of file stage.hh.

friend class WorkerThread [friend]

Definition at line 821 of file stage.hh.


Member Data Documentation

Set of models that require energy calculations at each World::Update().

Definition at line 1079 of file stage.hh.

Set of models that require their positions to be recalculated at each World::Update().

Definition at line 1081 of file stage.hh.

std::vector< std::string > World::args [static]

contains the command line arguments passed to Stg::Init(), so that controllers can read them.

Definition at line 826 of file stage.hh.

std::list<std::pair<world_callback_t,void*> > Stg::World::cb_list [protected]

List of callback functions and arguments.

Definition at line 898 of file stage.hh.

std::string World::ctrlargs [static]

Definition at line 827 of file stage.hh.

const int Stg::World::DEFAULT_PPM = 50 [static]

Definition at line 937 of file stage.hh.

bool Stg::World::destroy [private]

Definition at line 836 of file stage.hh.

bool Stg::World::dirty [private]

iff true, a gui redraw would be required

Definition at line 837 of file stage.hh.

std::vector<std::priority_queue<Event> > Stg::World::event_queues

Queue of pending simulation events for the main thread to handle.

Definition at line 1059 of file stage.hh.

Describes the 3D volume of the world.

Definition at line 899 of file stage.hh.

bool Stg::World::graphics [protected]

true iff we have a GUI

Definition at line 900 of file stage.hh.

Special model for the floor of the world

Definition at line 951 of file stage.hh.

std::set<Model*> Stg::World::models [private]

Pointers to all the models in this world.

Definition at line 840 of file stage.hh.

std::map<std::string, Model*> Stg::World::models_by_name [private]

pointers to the models that make up the world, indexed by name.

Definition at line 843 of file stage.hh.

std::map<int,Model*> Stg::World::models_by_wfentity [private]

pointers to the models that make up the world, indexed by worldfile entry index

Definition at line 846 of file stage.hh.

std::vector<Model*> Stg::World::models_with_fiducials [private]

Keep a list of all models with detectable fiducials. This avoids searching the whole world for fiducials.

Definition at line 850 of file stage.hh.

maintain a set of models with fiducials sorted by pose.x, for quickly finding nearby fidcucials

Definition at line 864 of file stage.hh.

maintain a set of models with fiducials sorted by pose.y, for quickly finding nearby fidcucials

Definition at line 868 of file stage.hh.

unsigned int World::next_id [static, private]

initially zero, used to allocate unique sequential world ids

Definition at line 834 of file stage.hh.

std::set<Option*> Stg::World::option_table [protected]

GUI options (toggles) registered by models.

Definition at line 902 of file stage.hh.

if true, the simulation is stopped

Definition at line 921 of file stage.hh.

std::vector<std::queue<Model*> > Stg::World::pending_update_callbacks

Queue of pending simulation events for the main thread to handle.

Definition at line 1062 of file stage.hh.

std::list<PowerPack*> Stg::World::powerpack_list [protected]

List of all the powerpacks attached to models in the world.

Definition at line 903 of file stage.hh.

double Stg::World::ppm [private]

the resolution of the world model in pixels per meter

Definition at line 883 of file stage.hh.

bool Stg::World::quit [private]

quit this world ASAP

Definition at line 884 of file stage.hh.

bool World::quit_all [static, private]

quit all worlds ASAP

Definition at line 832 of file stage.hh.

World::quit is set true when this simulation time is reached

Definition at line 905 of file stage.hh.

std::list<float*> Stg::World::ray_list [protected]

List of rays traced for debug visualization.

Definition at line 906 of file stage.hh.

Definition at line 935 of file stage.hh.

Definition at line 932 of file stage.hh.

bool Stg::World::show_clock [private]

iff true, print the sim time on stdout

Definition at line 886 of file stage.hh.

unsigned int Stg::World::show_clock_interval [private]

updates between clock outputs

Definition at line 887 of file stage.hh.

The amount of simulated time to run for each call to Update()

Definition at line 1087 of file stage.hh.

the current sim time in this world in microseconds

Definition at line 907 of file stage.hh.

Definition at line 908 of file stage.hh.

pthread_mutex_t Stg::World::sync_mutex [private]

protect the worker thread management stuff

Definition at line 889 of file stage.hh.

pthread_cond_t Stg::World::threads_done_cond [private]

signalled by last worker thread to unblock main thread

Definition at line 892 of file stage.hh.

pthread_cond_t Stg::World::threads_start_cond [private]

signalled to unblock worker threads

Definition at line 891 of file stage.hh.

unsigned int Stg::World::threads_working [private]

the number of worker threads not yet finished

Definition at line 890 of file stage.hh.

int Stg::World::total_subs [private]

the total number of subscriptions to all models

Definition at line 893 of file stage.hh.

Definition at line 1092 of file stage.hh.

std::vector< std::vector<Model*> > Stg::World::update_lists [protected]

Definition at line 910 of file stage.hh.

uint64_t Stg::World::updates [protected]

the number of simulated time steps executed so far

Definition at line 912 of file stage.hh.

Worldfile* Stg::World::wf [protected]

If set, points to the worldfile used to create this world.

Definition at line 913 of file stage.hh.

unsigned int Stg::World::worker_threads [private]

the number of worker threads to use

Definition at line 894 of file stage.hh.

std::set< World * > World::world_set [static, private]

all the worlds that exist

Definition at line 831 of file stage.hh.


The documentation for this class was generated from the following files:


stage
Author(s): Richard Vaughan , Brian Gerkey , Reed Hedges , Andrew Howard , Toby Collett , Pooya Karimian , Jeremy Asher , Alex Couture-Beil , Geoff Biggs , Rich Mattes , Abbas Sadat
autogenerated on Thu Aug 27 2015 15:20:57