Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

WorldModel2 Class Reference

#include <WorldModel2.h>

Inheritance diagram for WorldModel2:

Inheritance graph
[legend]
List of all members.

Detailed Description

Tekkotsu's localization and mapping system (IN ACTIVE DEVELOPMENT).

WorldModel2 is a system of egocentric and allocentric maps designed to represent the world around the AIBO. Presently, WorldModel2 is composed of beta and alpha software, with most egocentric functionality in beta and everything else in alpha.

The bulk of the alpha software in WorldModel2 is involved in global mapping and localization. This software is disabled by default and therefore escapes documentation by Doxygen. For more information on these routines, read WorldModel2.h; to give them a try (hardly advisable) set the environment variable GLOBAL_MAP to -DWANT_GLOBAL_MAP before compiling.

Definition at line 120 of file WorldModel2.h.

Public Member Functions

 WorldModel2 ()
 Constructor.

virtual ~WorldModel2 ()
 Destructor.

void resetWorldModel ()
 Resets the world model to the start state.

void enableKludge (unsigned int kludge)
 Enable a kludge (see the WM2Kludge namespace for details).

void disableKludge (unsigned int kludge)
 Disable a kludge (see the WM2Kludge namespace).

virtual void processEvent (const EventBase &event)
 Process vision and motion events.

void getRequests (MRvector &requests)
 Get suggestions about what to look at next (ALPHA).

Sensor toggles
Enable or disable certain sensing mechanisms, which are off by default Disabling sensor mechanisms is useful when you have other CPU intensive things to do and don't want to spend time updating the maps. Note that there is no way to disable processing of WalkMC movement--this would lead to gross inconsistencies in the map. Note that the mechanisms all start off blind--you have to turn them on yourself before they'll work.

void enableIR ()
 Enable infrared data registration.

void disableIR ()
 Disable infrared data registration.

void enableGPA ()
 Enable ground plane assumption mapping (ALPHA).

void disableGPA ()
 Disable ground plane assumption mapping (ALPHA).

Map data access routines
Access routines for the SDM and HHM data. You have your choice of "safer" and "faster". First are the "safer" methods, which copies the requested data into arrays furnished by the user. They're not *that* safe, though--you're passing a pointer to your array, and you'd better make certain that your array has enough space for the data.

You'll probably want to include the header file WorldModel2/Maps/Configuration.h to get the defines DM_CELL_COUNT, HM_CELL_COUNT, and GM_CELL_COUNT, which describe the number of cells in the maps. The same file also contains plenty of other constants describing the maps, which may be of use to you.

Safe access routines choose the data you want using "Pickers", functors described and documented in WorldModel2/Maps/almStructures.h that you specify as the first argument. Unfortunately, all the data is encoded as float, but this shouldn't create many problems.

Fast access routines simply return pointers to the active map data itself. Please do not change anything unless you know what you are doing!

void pickDMData (dmPicker &p, float *dest)
 The safe access method for spherical depth map data.

dm_cellinvadeDMData (void)
 The fast access method for spherical depth map data. Use at own risk.

void pickHMData (hmPicker &p, float *dest)
 Safe access method for horizontal height map. Do see long description.

hm_cellinvadeHMData (void)
 Risky fast access method for horiz. height map. See pickHMData note.


Private Member Functions

void processSensors ()
 process sensors, incorporating IRs into local maps

void processLocomotion (const EventBase &event)
 process locomotion events--movements

void processGround ()
 Try to do ground plane asssumption.

void serialize ()
 Perform serialization -- send data down the line, if needed.

 WorldModel2 (const WorldModel2 &)
 dummy functions prevent warnings

WorldModel2operator= (const WorldModel2 &)
 dummy functions prevent warnings


Private Attributes

unsigned int kludges
 Which kludges are enabled? See WM2Kludge documentation for info.

bool moving
bool enabledIR
 True if IR sensing is enabled (c.f. enableIR).

bool enabledGPA
 True if GPA mapping is enabled (c.f. enableGPA).

double dx
 The current velocities of the AIBO.

double dy
 The current velocities of the AIBO.

double da
 The current velocities of the AIBO.

long movingSince
SocketsockDM
 Here are sockets for serialization of the egocentric map data.

SocketsockHM
 Here are sockets for serialization of the egocentric map data.


Static Private Attributes

const int GPAdelay = 100
const int SRLdelay = 1000
bool locked = false


Constructor & Destructor Documentation

WorldModel2::WorldModel2  ) 
 

Constructor.

Definition at line 65 of file WorldModel2.cc.

References EventRouter::addListener(), EventRouter::addTimer(), AFS_NUM_LANDMARKS, AFS_NUM_PARTICLES, config, da, DM_CELL_COUNT, Config::worldmodel2_config::dm_port, dx, erouter, Config::worldmodel2_config::fs_port, GM_CELL_COUNT, Config::worldmodel2_config::gm_port, HM_CELL_COUNT, Config::worldmodel2_config::hm_port, Wireless::listen(), locked, EventBase::locomotionEGID, resetWorldModel(), Socket::sock, SocketNS::SOCK_STREAM, sockDM, sockHM, SRLdelay, TIMER_SID_SRL, wireless, and Config::worldmodel2.

WorldModel2::~WorldModel2  )  [virtual]
 

Destructor.

Definition at line 125 of file WorldModel2.cc.

References Wireless::close(), disableGPA(), disableIR(), erouter, EventRouter::forgetListener(), locked, sockDM, sockHM, and wireless.

WorldModel2::WorldModel2 const WorldModel2  )  [private]
 

dummy functions prevent warnings


Member Function Documentation

void WorldModel2::disableGPA  ) 
 

Disable ground plane assumption mapping (ALPHA).

Definition at line 179 of file WorldModel2.cc.

References enabledGPA, erouter, EventRouter::removeTimer(), and TIMER_SID_GPA.

void WorldModel2::disableIR  ) 
 

Disable infrared data registration.

Definition at line 157 of file WorldModel2.cc.

References enabledIR, erouter, EventRouter::removeListener(), and EventBase::sensorEGID.

void WorldModel2::disableKludge unsigned int  kludge  ) 
 

Disable a kludge (see the WM2Kludge namespace).

Definition at line 196 of file WorldModel2.cc.

References kludges.

void WorldModel2::enableGPA  ) 
 

Enable ground plane assumption mapping (ALPHA).

Our AIBO environment has a green floor. We can assume that all green items in the color segmented image from the vision system are parts of the floor. Using triangulation and the assumption that the floor is a flat plane at z=0, we can fill in parts of the map instantaniously. Note that ground plane assumption mapping only occurs when the AIBO is standing still in the erect posture from ms/data/motion/stand.pos and staring straight ahead (i.e. all head positioning attributes are 0).

Definition at line 169 of file WorldModel2.cc.

References EventRouter::addTimer(), enabledGPA, erouter, GPAdelay, moving, movingSince, and TIMER_SID_GPA.

void WorldModel2::enableIR  ) 
 

Enable infrared data registration.

The AIBO continually takes depth measurements with the IR range sensor in its nose. Calling this function will cause WorldModel2 to integrate these range measurements into its maps, along with segmented color data from the vision system for the measured point. Note that IR measurements are only integrated when the AIBO is standing still in the erect posture from ms/data/motion/stand.pos.

Definition at line 151 of file WorldModel2.cc.

References EventRouter::addListener(), enabledIR, erouter, and EventBase::sensorEGID.

void WorldModel2::enableKludge unsigned int  kludge  ) 
 

Enable a kludge (see the WM2Kludge namespace for details).

Definition at line 190 of file WorldModel2.cc.

References kludges.

void WorldModel2::getRequests MRvector requests  ) 
 

Get suggestions about what to look at next (ALPHA).

All maps within WorldModel2 give each of their grid cells 'confidence' scores: measures of how certain the mapping system is about the data they contain. At the moment, these scores are mostly ad-hoc, but later they may be based on more statistically rigorous techniques. In any case, getRequests performs a simple k-means cluster analysis on the certainty measurements of the WorldModel2 maps (where K and the number of iterations are specified in WorldModel2's Configuration.h) and places the centers of these clusters in appropriate MotionRequest structures, which are inserted into the requests vector. Your code may choose to honor these suggestions, or it may not.

Definition at line 284 of file WorldModel2.cc.

References AGM::genRequests(), and ALM::genRequests().

dm_cell * WorldModel2::invadeDMData void   ) 
 

The fast access method for spherical depth map data. Use at own risk.

Definition at line 301 of file WorldModel2.cc.

References ALM::getDM().

hm_cell * WorldModel2::invadeHMData void   ) 
 

Risky fast access method for horiz. height map. See pickHMData note.

Definition at line 311 of file WorldModel2.cc.

References ALM::getHM().

WorldModel2& WorldModel2::operator= const WorldModel2  )  [private]
 

dummy functions prevent warnings

void WorldModel2::pickDMData dmPicker p,
float *  dest
 

The safe access method for spherical depth map data.

Definition at line 295 of file WorldModel2.cc.

References DM_CELL_COUNT, and ALM::getDM().

void WorldModel2::pickHMData hmPicker p,
float *  dest
 

Safe access method for horizontal height map. Do see long description.

You'll want to use the HM_CELL_COUNT define in WorldModel2/Maps/Configuration.h for sizing. Do remember that the HHM is *round* and is not guaranteed to maintain all the data in its array--only that within the circle of cells within ALM_HM_SIZE (Euclidean) of the central cell of the array.

Definition at line 305 of file WorldModel2.cc.

References ALM::getHM(), and HM_CELL_COUNT.

void WorldModel2::processEvent const EventBase event  )  [virtual]
 

Process vision and motion events.

Implements EventListener.

Definition at line 259 of file WorldModel2.cc.

References EventBase::getGeneratorID(), EventBase::getSourceID(), EventBase::locomotionEGID, processGround(), processLocomotion(), processSensors(), EventBase::sensorEGID, serialize(), TIMER_SID_GPA, TIMER_SID_SRL, EventBase::timerEGID, and EventBase::visionEGID.

void WorldModel2::processGround  )  [private]
 

Try to do ground plane asssumption.

Definition at line 556 of file WorldModel2.cc.

References aiboIsErect(), aiboStaresDeadAhead(), enabledGPA, erouter, ALM::registerGround(), EventRouter::removeTimer(), and UNLESS.

void WorldModel2::processLocomotion const EventBase event  )  [private]
 

process locomotion events--movements

Definition at line 430 of file WorldModel2.cc.

References EventRouter::addListener(), EventRouter::addTimer(), AFS_NUM_LANDMARKS, afsMotion(), afsResample(), afsWhatsUp(), _FastSLAM_update::da, da, _FastSLAM_update::dx, dx, _FastSLAM_update::dy, dy, enabledGPA, enabledIR, erouter, get_time(), GPAdelay, kludges, ALM::move(), moving, movingSince, EventRouter::removeListener(), EventRouter::removeTimer(), EventBase::sensorEGID, _FastSLAM_update::time, _FastSLAM_update::type, and EventBase::visionEGID.

void WorldModel2::processSensors  )  [private]
 

process sensors, incorporating IRs into local maps

Definition at line 329 of file WorldModel2.cc.

References enabledIR, ERS210Info::HeadOffset, ERS210Info::IRDistOffset, kludges, WorldState::outputs, ERS210Info::PanOffset, ALM::registerDepth(), WorldState::sensors, state, ERS210Info::TiltOffset, and UNLESS.

void WorldModel2::resetWorldModel  ) 
 

Resets the world model to the start state.

Clears all maps ("empty" maps will contain an assumed ground plane at elevation 0) and, if allocentric functionality is enabled, resets the localization system to its initial zero-knowledge state.

Definition at line 227 of file WorldModel2.cc.

References afsInit(), AGM::init(), and ALM::init().

void WorldModel2::serialize  )  [private]
 

Perform serialization -- send data down the line, if needed.

Definition at line 584 of file WorldModel2.cc.

References AFS_NUM_LANDMARKS, AFS_NUM_PARTICLES, afsInvadeFSData(), afsWhatsUp(), _hm_cell::color, _dm_cell::color, colortype, _hm_cell::confidence, _dm_cell::confidence, _dm_cell::depth, DM_CELL_COUNT, Serializer::encode(), ALM::getDM(), AGM::getGM(), ALM::getHM(), Socket::getWriteBuffer(), GM_CELL_COUNT, _hm_cell::height, HM_CELL_COUNT, _afsParticle::landmarks, _afsLandmarkLoc::mean, Particles, _afsParticle::pose, sockDM, sockHM, _afsLandmarkLoc::state, _afsPose::theta, _hm_cell::trav, _afsLandmarkLoc::variance, Socket::write(), _afsPose::x, and _afsPose::y.


Member Data Documentation

double WorldModel2::da [private]
 

The current velocities of the AIBO.

Definition at line 306 of file WorldModel2.h.

double WorldModel2::dx [private]
 

The current velocities of the AIBO.

Definition at line 306 of file WorldModel2.h.

double WorldModel2::dy [private]
 

The current velocities of the AIBO.

Definition at line 306 of file WorldModel2.h.

bool WorldModel2::enabledGPA [private]
 

True if GPA mapping is enabled (c.f. enableGPA).

Definition at line 303 of file WorldModel2.h.

bool WorldModel2::enabledIR [private]
 

True if IR sensing is enabled (c.f. enableIR).

Definition at line 302 of file WorldModel2.h.

const int WorldModel2::GPAdelay = 100 [static, private]
 

Constant indicates how frequently head position sampling should take place for the ground plane assumption. Units are milliseconds.

Definition at line 281 of file WorldModel2.h.

unsigned int WorldModel2::kludges [private]
 

Which kludges are enabled? See WM2Kludge documentation for info.

Definition at line 293 of file WorldModel2.h.

bool WorldModel2::locked = false [static, private]
 

The AIBO's memory management system is borked. Consequently, all memory space is allocated statically and can only be used by one WorldModel2 at a time. This variable indicates whether there's already someone using the data.

Definition at line 61 of file WorldModel2.cc.

bool WorldModel2::moving [private]
 

This boolean is set to true if the system isn't certain that AIBO is stopped. In other words, in spite of its name, it may be true even when the AIBO isn't moving, e.g. during emergency stop. If it's false, though, the AIBO should surely be at rest.

Definition at line 299 of file WorldModel2.h.

long WorldModel2::movingSince [private]
 

The time when we started going at current velocities. 0 is a special value for movingSince, indicating that no information on movement has been given to this WorldModel object yet. Routines that use this motion information should check for the 0 value and abend if they find it.

Definition at line 312 of file WorldModel2.h.

Socket* WorldModel2::sockDM [private]
 

Here are sockets for serialization of the egocentric map data.

Definition at line 315 of file WorldModel2.h.

Socket * WorldModel2::sockHM [private]
 

Here are sockets for serialization of the egocentric map data.

Definition at line 315 of file WorldModel2.h.

const int WorldModel2::SRLdelay = 1000 [static, private]
 

Constant indicates how frequently serialization of map data should be performed. Units are milliseconds.

Definition at line 284 of file WorldModel2.h.


The documentation for this class was generated from the following files:
Tekkotsu v1.4
Generated Sat Jul 19 00:09:16 2003 by Doxygen 1.3.2