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.h

Go to the documentation of this file.
00001 /*
00002  * Tekkotsu framework class for the Second Generation World Model.
00003  * This class is the glue that connects all of the representations in
00004  * the 2GWM with the rest of Tekkotsu. It implements EventListener so it
00005  * can receive motion and video events; it maintains a queue of actions
00006  * that the AIBO can elect to perform to improve the certainty of the
00007  * world representations; and it also has access functions that allow
00008  * other code to get at the contents of the world model.
00009  *
00010  * This code checks for the definition of WANT_GLOBAL_MAP. If WANT_GLOBAL_MAP
00011  * is enabled, then global mapping functionality (FastSLAM, the global height
00012  * map) is compiled in. If not, these parts are omitted.
00013  */
00014 
00015 #ifndef _WORLD_MODEL_2_H_
00016 #define _WORLD_MODEL_2_H_
00017 
00018 #include <vector>
00019 #include <deque>
00020 #include <math.h>
00021 
00022 #include "Events/EventListener.h"
00023 // for AFS_NUM_LANDMARKS
00024 #include "WorldModel2/FastSLAM/Configuration.h"
00025 // for afsParticle
00026 #include "WorldModel2/FastSLAM/afsParticle.h"
00027 // for Picker objects
00028 #include "WorldModel2/Maps/almStructures.h"
00029 // for serializer stuff
00030 #include "Shared/Serializer.h"
00031 // for wireless
00032 #include "Wireless/Wireless.h"
00033 
00034 //! Symbols for kludges that alter the behavior of WorldModel2 mapping
00035 
00036 //! Various assumptions (kludges) you can enable or disable with enableKludge
00037 //! and disableKludge to make the behavior of the maps appear better
00038 namespace WM2Kludge {
00039   //! Don't incorporate measurements of items situated under the ground plane
00040 
00041   //! If your environment has no pits in it, there will be nothing that has
00042   //! a z value smaller than 0. Measurements which have such z values are surely
00043   //! noise and can be ignored.
00044   const unsigned int IgnoreZLessThanZero = 1;
00045   //! Don't incorporate measurements of green items in the map
00046 
00047   //! Our environment has green matting that uniquely indicates that we're
00048   //! looking at the ground. Since we can already guess that the ground is
00049   //! flat, we don't need to waste time on measurements of it.
00050   const unsigned int IgnoreGreenItems  = 2;
00051   //! Delay passing of movement information to FastSLAM
00052 
00053   //! With lots of particles, FastSLAM takes forever to do motion resamples--too
00054   //! long for the robot to remain responsive if it's just doing a course correction
00055   //! while en route. With this kludge enabled, motion resampling will wait until the
00056   //! AIBO is completely stopped; then all the motions will be applied en masse.
00057   //! This will still have an impact on state machine timing, but at least it keeps
00058   //! AIBO from running into a wall. Be warned: you will not know where you are
00059   //! with LazyFastSLAM enabled until you stop moving and FastSLAM has had an
00060   //! opportunity to catch up with itself!
00061   const unsigned int LazyFastSLAM  = 4;
00062 }
00063 
00064 
00065 //! Structure for containing motion requests.
00066 //  I wish there were a more clean way to do this
00067 typedef struct {
00068     //! Is this motion request from the DM, the HM, or the GM?
00069   enum { LOOK_AT, LOOK_DOWN_AT, GO_TO } type;
00070     // Hang on to your hats, it's a union!
00071     //! Depending on which it is, we use different data.
00072   union {
00073       //! Used for LOOK_AT, the requests made by the DM
00074     struct {
00075       double azimuth;  //!< angle from straight ahead (bearing)
00076       double altitude; //!< height relative to ? TSS_TODO
00077     } azalt;
00078       //! Used for LOOK_DOWN_AT and GO_TO, the requests made by
00079       //! the HM and the GM.
00080     struct {
00081       double x; //!< x
00082       double y; //!< y
00083     } xy;
00084   };
00085 } MotionRequest;
00086 
00087 
00088 //! Structure for keeping track of FastSLAM system updates for eventual
00089 //! evaluation. See the documentation on the LazyFastSLAM kludge to see
00090 //! why we need to keep this data around at times.
00091 typedef struct _FastSLAM_update {
00092   enum { MOTION, LANDMARK } type; //!< the type of record
00093   double dx; //!< x distance
00094   double dy; //!< y distance
00095   double da; //!< angular distance
00096   long time; //!< timestamp
00097 } FastSLAM_update;
00098 
00099 
00100 //! A vector for motion requests
00101 typedef std::vector<MotionRequest> MRvector;
00102 
00103 //! A deque for FastSLAM updates
00104 typedef std::deque<FastSLAM_update> FSUdeque;
00105 
00106 // The WorldModel2 class itself
00107 //! Tekkotsu's localization and mapping system <b>(IN ACTIVE DEVELOPMENT)</b>
00108 
00109 //! WorldModel2 is a system of egocentric and allocentric maps designed to
00110 //! represent the world around the AIBO. Presently, WorldModel2 is composed
00111 //! of <b>beta</b> and <b>alpha</b> software, with most egocentric
00112 //! functionality in beta and everything else in alpha.
00113 //! <p>
00114 //! The bulk of the alpha software in WorldModel2 is involved in global
00115 //! mapping and localization. This software is disabled by default and
00116 //! therefore escapes documentation by Doxygen. For more information on
00117 //! these routines, read WorldModel2.h; to give them a try (hardly advisable)
00118 //! set the environment variable GLOBAL_MAP to -DWANT_GLOBAL_MAP before
00119 //! compiling.
00120 class WorldModel2 : public EventListener, public Serializer {
00121   public:
00122 
00123       //! Constructor
00124     WorldModel2();
00125       //! Destructor
00126     virtual ~WorldModel2();
00127 
00128       //! Resets the world model to the start state
00129 
00130       //! Clears all maps ("empty" maps will contain an assumed ground plane
00131       //! at elevation 0) and, if allocentric functionality is enabled, resets
00132       //! the localization system to its initial zero-knowledge state.
00133     void resetWorldModel();
00134  
00135       //! @name Sensor toggles
00136       //! Enable or disable certain sensing mechanisms, which are off by default
00137       //! Disabling sensor mechanisms is useful when you have other CPU
00138       //! intensive things to do and don't want to spend time updating the
00139       //! maps. Note that there is no way to disable processing of WalkMC
00140       //! movement--this would lead to gross inconsistencies in the map.
00141       //! Note that the mechanisms all start off blind--you have to turn
00142       //! them on yourself before they'll work.
00143       //@{
00144       //! Enable infrared data registration
00145 
00146       //! The AIBO continually takes depth measurements with the IR range
00147       //! sensor in its nose. Calling this function will cause WorldModel2
00148       //! to integrate these range measurements into its maps, along with
00149       //! segmented color data from the vision system for the measured point.
00150       //! Note that IR measurements are only integrated when the AIBO is
00151       //! standing still in the erect posture from ms/data/motion/stand.pos.
00152     void enableIR();
00153     void disableIR(); //!< Disable infrared data registration
00154 
00155       //! Enable ground plane assumption mapping (ALPHA)
00156 
00157       //! Our AIBO environment has a green floor. We can assume that all green
00158       //! items in the color segmented image from the vision system are parts
00159       //! of the floor. Using triangulation and the assumption that the floor
00160       //! is a flat plane at z=0, we can fill in parts of the map
00161       //! instantaniously. Note that ground plane assumption mapping only
00162       //! occurs when the AIBO is standing still in the erect posture from
00163       //! ms/data/motion/stand.pos and staring straight ahead (i.e. all head
00164       //! positioning attributes are 0).
00165     void enableGPA();
00166     void disableGPA();  //!< Disable ground plane assumption mapping (ALPHA)
00167       //@}
00168 
00169       //! Enable a kludge (see the WM2Kludge namespace for details)
00170     void enableKludge(unsigned int kludge);
00171       //! Disable a kludge (see the WM2Kludge namespace)
00172     void disableKludge(unsigned int kludge);
00173 
00174       //! Process vision and motion events
00175     virtual void processEvent(const EventBase& event);
00176 
00177       //! Get suggestions about what to look at next (ALPHA)
00178 
00179       //! All maps within WorldModel2 give each of their grid cells
00180       //! 'confidence' scores: measures of how certain the mapping system
00181       //! is about the data they contain. At the moment, these scores are
00182       //! mostly ad-hoc, but later they may be based on more statistically
00183       //! rigorous techniques. In any case, getRequests performs a simple
00184       //! k-means cluster analysis on the certainty measurements of the
00185       //! WorldModel2 maps (where K and the number of iterations are
00186       //! specified in WorldModel2's Configuration.h) and places the centers
00187       //! of these clusters in appropriate MotionRequest structures, which
00188       //! are inserted into the requests vector. Your code may choose to
00189       //! honor these suggestions, or it may not.
00190     void getRequests(MRvector &requests);
00191 
00192       //! @name Map data access routines
00193       //! Access routines for the SDM and HHM data. You have your
00194       //! choice of "safer" and "faster". First are the "safer" methods,
00195       //! which copies the requested data into arrays furnished by the user.
00196       //! They're not *that* safe, though--you're passing a pointer to
00197       //! your array, and you'd better make certain that your array has
00198       //! enough space for the data.<p>
00199       //!   You'll probably want to include the header file
00200       //! WorldModel2/Maps/Configuration.h to get the defines DM_CELL_COUNT,
00201       //! HM_CELL_COUNT, and GM_CELL_COUNT, which describe the number of
00202       //! cells in the maps. The same file also contains plenty of other
00203       //! constants describing the maps, which may be of use to you.<p>
00204       //!   Safe access routines choose the data you want using "Pickers",
00205       //! functors described and documented in
00206       //! WorldModel2/Maps/almStructures.h that you specify as the first
00207       //! argument. Unfortunately, all the data is encoded as float, but
00208       //! this shouldn't create many problems.<p>
00209       //!   Fast access routines simply return pointers to the active map
00210       //! data itself. <b>Please do not change anything unless you know what
00211       //! you are doing!</b>
00212       //@{
00213       //! The safe access method for spherical depth map data.
00214     void pickDMData(dmPicker &p, float *dest);
00215       //! The fast access method for spherical depth map data. Use at own risk.
00216     dm_cell *invadeDMData(void);
00217 
00218       //! Safe access method for horizontal height map. Do see long description.
00219 
00220       //! You'll want to use the HM_CELL_COUNT define in
00221       //! WorldModel2/Maps/Configuration.h for sizing. Do remember that
00222       //! the HHM is *round* and is not guaranteed to maintain all the data
00223       //! in its array--only that within the circle of cells within
00224       //! ALM_HM_SIZE (Euclidean) of the central cell of the array.
00225     void pickHMData(hmPicker &p, float *dest);
00226       //! Risky fast access method for horiz. height map. See pickHMData note.
00227     hm_cell *invadeHMData(void);
00228       //@}
00229 
00230       // Here are public extensions to the class that are added when global
00231       // map features are desired.
00232 #ifdef WANT_GLOBAL_MAP
00233       //! Specify the position of a landmark (for FastSLAM) (ALPHA)
00234 
00235       //! Recommended for use BEFORE any measurements have been made--either
00236       //! build a map from scratch with FastSLAM or specify a map and have
00237       //! FastSLAM navigate around in it.<p>
00238       //! If you use this function, you will probably want to employ the
00239       //! fsDistribute as well, effectively turning FastSLAM into Monte Carlo
00240       //! Localizaton
00241     void setLandmark(int landmark, double x, double y, double covariance);
00242 
00243       //! Random FastSLAM initialization within bounding box. (ALPHA)
00244 
00245       //! Distribute FastSLAM particles randomly (uniformly) throughout
00246       //! a bounding box. Recommended for use AFTER all landmarks have been
00247       //! specified with setLandmark (above) and BEFORE any measurements
00248       //! have been made. Use in this way effectively reduces FastSLAM to
00249       //! Monte Carlo Localization.<p>
00250       //! The tighter the bounding box, the better the starting performance.
00251       //! lx = left x, ty = top y, rx = right x, bottom y = by
00252     void fsDistribute(double lx, double ty, double rx, double by);
00253 
00254       // \addtogroup Sensor toggles
00255       //@{
00256       //! Enable the detection of markers (used by FastSLAM) (ALPHA)
00257     void enableMarkers();
00258       //! Disable marker detection (used by FastSLAM) (ALPHA)
00259     void disableMarkers();
00260       //@}
00261 
00262       //! Gets the current best FastSLAM map (ALPHA)
00263 
00264       //! Places the contents of the map in p. The map contains not only
00265       //! estimated landmark positions but also the current best guess of
00266       //! the robot's position and heading.
00267     void getFastSLAMMap(afsParticle &p);
00268 
00269       // \addtogroup Map data access routines
00270       //@{
00271       //! Safe access method for global map data.
00272     void pickGMData(hmPicker &p, float *dest);
00273       //! Risky fast access method for global map data.
00274     hm_cell *invadeGMData(void);
00275       //@}
00276 #endif
00277 
00278   private:
00279       //! Constant indicates how frequently head position sampling should take
00280       //! place for the ground plane assumption. Units are milliseconds.
00281     static const int GPAdelay = 100;
00282       //! Constant indicates how frequently serialization of map data should
00283       //! be performed. Units are milliseconds.
00284     static const int SRLdelay = 1000;
00285 
00286       //! The AIBO's memory management system is borked. Consequently, all
00287       //! memory space is allocated statically and can only be used by one
00288       //! WorldModel2 at a time. This variable indicates whether there's
00289       //! already someone using the data.
00290     static bool locked;
00291 
00292       //! Which kludges are enabled? See WM2Kludge documentation for info.
00293     unsigned int kludges;
00294 
00295       //! This boolean is set to true if the system isn't certain that AIBO
00296       //! is stopped. In other words, in spite of its name, it may be true
00297       //! even when the AIBO isn't moving, e.g. during emergency stop.
00298       //! If it's false, though, the AIBO should surely be at rest.
00299     bool moving;
00300 
00301       // These booleans keep track of what sensing mechanisms are enabled.
00302     bool enabledIR; //!< True if IR sensing is enabled (c.f. enableIR)
00303     bool enabledGPA;  //!< True if GPA mapping is enabled (c.f. enableGPA)
00304 
00305       //! The current velocities of the AIBO.
00306     double dx, dy, da;
00307       //! The time when we started going at current velocities. 0 is a special
00308       //! value for movingSince, indicating that no information on movement
00309       //! has been given to this WorldModel object yet. Routines that use
00310       //! this motion information should check for the 0 value and abend if
00311       //! they find it.
00312     long movingSince;
00313 
00314       //! Here are sockets for serialization of the egocentric map data
00315     Socket *sockDM, *sockHM;
00316 
00317       //! process sensors, incorporating IRs into local maps
00318     void processSensors();
00319       //! process locomotion events--movements
00320     void processLocomotion(const EventBase& event);
00321       //! Try to do ground plane asssumption
00322     void processGround();
00323 
00324       //! Perform serialization -- send data down the line, if needed.
00325     void serialize();
00326 
00327       // Here are private extensions to the class added when global map
00328       // features are desired
00329 #ifdef WANT_GLOBAL_MAP
00330       //! Here are sockets for serialization of the global map data.
00331     Socket *sockGM, *sockFS;
00332 
00333       //! Constant for the max angle from center of AIBO's FOV in which
00334       //! we'll actually pay attention to viewed landmarks. This is a way
00335       //! of avoiding problemes with distortion at the edges of the
00336       //! image. Must be in radians.
00337     static const float maxMarkerAngle = 30*M_PI/180;
00338       //! How many markers do we need to see before we do a FastSLAM resampling?
00339     static const int minMarkersForFastSLAM = 2;
00340 
00341       //@name Sensor processing checklist flags.
00342       //@{
00343       //! After each traverse, there's a list of things we want to do. Once
00344       //! we've done them, we don't want to do them again until we've moved
00345     bool haveSeenMarker[AFS_NUM_LANDMARKS];
00346 
00347       //! Keeps track of whether marker sensing is enabled (ALPHA)
00348     bool enabledMarkers; // marker sensing for FastSLAM
00349       //@}
00350 
00351       //! Pending updates for FastSLAM. See note on LazyFastSLAM in Kludges
00352      FSUdeque fs_updates;
00353 
00354       //! process vision events, specifically markers (ALPHA)
00355     void processVision(const EventBase& event);
00356 #endif
00357 
00358       //! dummy functions prevent warnings
00359     WorldModel2(const WorldModel2&);
00360       //! dummy functions prevent warnings
00361     WorldModel2& operator= (const WorldModel2&);
00362 };
00363 
00364   //! Determine whether Aibo is in an erect stature, allowing us to do
00365   //! measurements. TODO: replace with a real motion model
00366 bool aiboIsErect();
00367 
00368   //! Determine whether Aibo is looking dead ahead. Needed for ground plane
00369   //! assumption, for the moment.
00370 bool aiboStaresDeadAhead();
00371 
00372   //! Determine whether Aibo is keeping its head level (i.e. no tilt or roll).
00373   //! Needed for FastSLAM at the moment.
00374 bool aiboIsLevelHeaded();
00375 
00376 #endif

Tekkotsu v1.4
Generated Sat Jul 19 00:06:32 2003 by Doxygen 1.3.2