Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
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 |