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

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 // include me
00016 #include "WorldModel2.h"
00017 
00018 // include STL, Tekkotsu, and other basics
00019 #include <cmath>
00020 #include <vector>
00021 #include <deque>
00022 #include "Events/EventRouter.h"
00023 #include "Events/VisionEvent.h"
00024 #include "Events/LocomotionEvent.h"
00025 #include "Shared/WorldState.h"
00026 #include "Vision/Vision.h"
00027 #include "Wireless/Wireless.h"
00028 
00029 // include WorldModel2 particulars
00030 #include "FastSLAM/afsMain.h"
00031 #include "Maps/almMain.h"
00032 #include "Maps/agmMain.h"
00033 #include "Maps/almStructures.h"
00034 // needed for AFS_NUM_LANDMARKS, AFS_NUM_PARTICLES
00035 #include "FastSLAM/Configuration.h"
00036 // needed for afsParticle
00037 #include "FastSLAM/afsParticle.h"
00038 // needed for pinhole camera model parameters
00039 #include "Maps/Configuration.h"
00040 // Standing robot parameters. TODO--replace with motion model
00041 #include "Poses.h"
00042 
00043 // Timing to time our moves
00044 #include "Shared/get_time.h"
00045 
00046 // use perl;
00047 #define UNLESS(item)  if(!( (item) ))
00048 
00049 // debugging?
00050 //#define DEBUG_WM2
00051 
00052 // Timer SIDs
00053 #define TIMER_SID_GPA 0 // sid for ground plane assumption timer
00054 #define TIMER_SID_SRL 1 // sid for serializer
00055 
00056 using namespace std;
00057 
00058 
00059   // Start everything out with WorldMap2 resource lock flag (see
00060   // WorldModel2.h) unlocked
00061 bool WorldModel2::locked = false;
00062 
00063   // Constructor. The constructor must initialize all of the world model
00064   // representations
00065 WorldModel2::WorldModel2()
00066       // We start with no kludges enabled. This is known to all my "homies"
00067       // as "keeping it real".
00068   : kludges(0),
00069       // NOTE: moving is true because we don't know our state on initialization
00070     moving(true),
00071       // Set all enabled sensing mechanisms to false
00072     enabledIR(false), enabledGPA(false),
00073       // Set current velocities and travel time start to 0.
00074       // NOTE: 0 is a special value for movingSince--see WorldModel2.h
00075     dx(0.0), dy(0.0), da(0.0), movingSince(0),
00076       // Initialize sockets for outbound communication. Note strikingly
00077       // unintelligent outbound buffer sizes--this could lead to
00078       // problems if we ever get great big maps.
00079       // We're also assuming that the space used by cells in the maps is equal
00080       // to or greater than the amount of room you need to transmit cell data
00081       // over the ether.
00082     sockDM(wireless->socket(SocketNS::SOCK_STREAM, 1024,
00083           DM_CELL_COUNT*sizeof(dm_cell))),
00084     sockHM(wireless->socket(SocketNS::SOCK_STREAM, 1024,
00085           HM_CELL_COUNT*sizeof(hm_cell)))
00086 #ifdef WANT_GLOBAL_MAP
00087     ,
00088     sockGM(wireless->socket(SocketNS::SOCK_STREAM, 1024,
00089           GM_CELL_COUNT*sizeof(hm_cell))),
00090     sockFS(wireless->socket(SocketNS::SOCK_STREAM, 1024,
00091     AFS_NUM_PARTICLES*sizeof(afsPose) + sizeof(afsParticle))),
00092     enabledMarkers(false),
00093     fs_updates()
00094 #endif
00095 {
00096     // Make sure the world model resources aren't owned by someone else.
00097   if(locked) return; // TODO: throw an exception?
00098     // OK, claim the resources for ourselves
00099   locked = true;
00100 
00101 #ifdef WANT_GLOBAL_MAP
00102     // Clear sensor processing checklist flags (see WorldModel2.h)
00103   for(int i=0; i<AFS_NUM_LANDMARKS; ++i) haveSeenMarker[i] = false;
00104 #endif
00105 
00106     // Start listening on output ports
00107   wireless->listen(sockDM->sock, config->worldmodel2.dm_port);
00108   wireless->listen(sockHM->sock, config->worldmodel2.hm_port);
00109 #ifdef WANT_GLOBAL_MAP
00110   wireless->listen(sockGM->sock, config->worldmodel2.gm_port);
00111   wireless->listen(sockFS->sock, config->worldmodel2.fs_port);
00112 #endif
00113 
00114     // We just set the world model to the start state.
00115   resetWorldModel();
00116 
00117     // Subscribe to locomotion events
00118   erouter->addListener(this,EventBase::locomotionEGID);
00119 
00120     // Turn on serialization timer
00121   erouter->addTimer(this, TIMER_SID_SRL, SRLdelay, true);
00122 }
00123 
00124   // Destructor
00125 WorldModel2::~WorldModel2()
00126 {
00127     // Cancel subscriptions to various events--also turns off timers
00128   disableIR();
00129   disableGPA();
00130 #ifdef WANT_GLOBAL_MAP
00131   disableMarkers();
00132 #endif
00133   erouter->forgetListener(this);
00134 
00135     // Close sockets
00136   wireless->close(sockDM);
00137   wireless->close(sockHM);
00138 #ifdef WANT_GLOBAL_MAP
00139   wireless->close(sockGM);
00140   wireless->close(sockFS);
00141 #endif
00142 
00143     // Free the resources up for someone else
00144   locked = false;
00145 }
00146 
00147 
00148 
00149   // Enable or disable certain sensing mechanisms.
00150   // infrared
00151 void WorldModel2::enableIR()
00152 {
00153     // Subscribe to sensor updates so that we can know what the IR sees
00154   erouter->addListener(this, EventBase::sensorEGID, SensorSourceID::UpdatedSID);
00155   enabledIR = true;
00156 }
00157 void WorldModel2::disableIR()
00158 {
00159     // Cancel subscription to sensor updates--we don't need them for IR anymore
00160   erouter->removeListener(this, EventBase::sensorEGID,
00161         SensorSourceID::UpdatedSID);
00162   enabledIR = false;
00163 }
00164 
00165   // Ground plane assumption. The GPA uses timer events to tell it to check
00166   // periodically for the head to be in the right position to do GPA.
00167   // Thus, after we've enabled GPA, we need to see if we're not moving
00168   // and if so, turn on the GPA timer
00169 void WorldModel2::enableGPA()
00170 {
00171     // turn on GPA timer if we're not moving
00172     // note: checks for indicator value of 0 of movingSince
00173     // Timer SID 0 is for
00174   if(movingSince && !moving)
00175     erouter->addTimer(this, TIMER_SID_GPA, GPAdelay, true);
00176 
00177   enabledGPA = true;
00178 }
00179 void WorldModel2::disableGPA()
00180 {
00181     // turn off the GPA timer if it's running
00182   erouter->removeTimer(this, TIMER_SID_GPA);
00183 
00184   enabledGPA = false;
00185 }
00186 
00187 
00188 // Enable a kludge. See the definition of the WM2Kludge namespace
00189 // to find out what they are
00190 void WorldModel2::enableKludge(unsigned int kludge)
00191 {
00192   kludges |= kludge;
00193 }
00194 
00195 // Disable a kludge
00196 void WorldModel2::disableKludge(unsigned int kludge)
00197 {
00198   kludges &= ~kludge;
00199 }
00200 
00201 
00202 #ifdef WANT_GLOBAL_MAP
00203   // Marker events--for FastSLAM
00204 void WorldModel2::enableMarkers()
00205 {
00206     // We enable marker vision events
00207   vision->enableEvents(VisionEventNS::MarkersSID);
00208     // We even listen to them if we're not moving
00209     // note: also checks for indicator value of 0 of movingSince
00210   if(movingSince && !moving)
00211     erouter->addListener(this,EventBase::visionEGID, VisionEventNS::MarkersSID);
00212   enabledMarkers = true;
00213 }
00214 void WorldModel2::disableMarkers()
00215 {
00216     // Marker vision events go off
00217   vision->disableEvents(VisionEventNS::MarkersSID);
00218   erouter->removeListener(this, EventBase::visionEGID,
00219         VisionEventNS::MarkersSID);
00220   enabledMarkers = false;
00221 }
00222 #endif
00223 
00224 
00225 
00226   // Resets the world model to the start state
00227 void WorldModel2::resetWorldModel()
00228 {
00229     // Reset egocentric maps
00230   ALM::init();
00231 #ifdef WANT_GLOBAL_MAP
00232     // Reset global maps
00233   AGM::init();
00234     // Reset FastSLAM global localizer
00235   afsInit();
00236 #endif
00237 }
00238 
00239 #ifdef WANT_GLOBAL_MAP
00240   // Specify the position of a landmark (for FastSLAM)
00241   // just a wrapper function, really.
00242 void WorldModel2::setLandmark(int landmark,
00243             double x, double y, double covariance)
00244 {
00245   afsSetLandmark(landmark, x, y, covariance);
00246 }
00247 
00248   // Distribute FastSLAM particles randomly (uniformly) throughout
00249   // a bounding box.
00250   // just a wrapper function, really.
00251   // lx = left x, ty = top y, rx = right x, bottom y = by
00252 void WorldModel2::fsDistribute(double lx, double ty, double rx, double by)
00253 {
00254   afsDistribute(lx, ty, rx, by);
00255 }
00256 #endif
00257 
00258   // Process vision and motion events
00259 void WorldModel2::processEvent(const EventBase& event)
00260 {
00261     // Find out what sort of event we have here and act accordingly
00262   switch(event.getGeneratorID()) {
00263     // process sensor data, specifically IR
00264   case EventBase::sensorEGID: processSensors(); break;
00265 #ifdef WANT_GLOBAL_MAP
00266     // process vision data, specifically markers
00267   case EventBase::visionEGID: processVision(event); break;
00268 #endif
00269     // process locomotion data
00270   case EventBase::locomotionEGID: processLocomotion(event); break;
00271     // either it's time to try and process ground data or it's time to
00272     // serialize our data
00273   case EventBase::timerEGID:
00274    if(event.getSourceID() == TIMER_SID_GPA) processGround();
00275     else if(event.getSourceID() == TIMER_SID_SRL) serialize();
00276     break;
00277     // dunno what this is, then
00278   default:
00279     cerr << "warning: WorldModel2 receved unrequested event" << endl;
00280   }
00281 }
00282 
00283   // Get suggestions about what to look at next
00284 void WorldModel2::getRequests(MRvector &requests)
00285 {
00286   ALM::genRequests(requests);
00287 #ifdef WANT_GLOBAL_MAP
00288   AGM::genRequests(requests);
00289 #endif
00290 }
00291 
00292 
00293   // Access methods for the spherical depth map. See WorldModel2.h for
00294   // extensive documentation.
00295 void WorldModel2::pickDMData(dmPicker &p, float *dest)
00296 {
00297   // Get DM data
00298   dm_cell *DMp = ALM::getDM();
00299   for(int i=0; i<DM_CELL_COUNT; ++i) dest[i] = p(DMp[i]);
00300 }
00301 dm_cell *WorldModel2::invadeDMData(void) { return ALM::getDM(); }
00302 
00303   // Access methods for the horizontal height map. See WorldModel2.h for
00304   // extensive documentation.
00305 void WorldModel2::pickHMData(hmPicker &p, float *dest)
00306 {
00307   // Get HM data
00308   hm_cell *HMp = ALM::getHM();
00309   for(int i=0; i<HM_CELL_COUNT; ++i) dest[i] = p(HMp[i]);
00310 }
00311 hm_cell *WorldModel2::invadeHMData(void) { return ALM::getHM(); }
00312 
00313 #ifdef WANT_GLOBAL_MAP
00314   // Access methods for the horizontal height map. See WorldModel2.h for
00315   // extensive documentation.
00316 void WorldModel2::pickGMData(hmPicker &p, float *dest)
00317 {
00318   // Get HM data
00319   /*
00320   hm_cell *GMp = ALM::getGM();
00321   for(int i=0; i<GM_CELL_COUNT; ++i) dest[i] = p(GMp[i]);
00322   */
00323 }
00324 hm_cell *WorldModel2::invadeGMData(void) { return NULL; } //return ALM::getGM(); }
00325 #endif
00326 
00327 
00328   // process sensors, specifically IRs
00329 void WorldModel2::processSensors()
00330 {
00331   // We should never be called if enabledIR is off, but here's a test anyway
00332   UNLESS(enabledIR) {
00333     cerr << "warning: WorldModel2 handling IR event w/o authorization" << endl;
00334     return;
00335   }
00336   // just abort if the AIBO isn't standing erect--we can't do math on the
00337   // IR stuff then. In the future, we'll have inverse kinematics that
00338   // take care of this for us.
00339 //UNCOMMENT THIS IN REAL LIFE
00340   //if(!aiboIsErect()) return;
00341 
00342   // OK, AIBO is in the right position. Go ahead and integrate its sensor data
00343   double depth    = state->sensors[IRDistOffset];
00344   double azimuth  = state->outputs[HeadOffset+PanOffset];
00345   double altitude = state->outputs[HeadOffset+TiltOffset];
00346 
00347   ALM::registerDepth(depth, altitude, azimuth, kludges);
00348 }
00349 
00350 #ifdef WANT_GLOBAL_MAP
00351   // process vision events
00352 void WorldModel2::processVision(const EventBase& event)
00353 {
00354   // These variables are part of the same mechanism that tracks image size
00355   // (and hence depth of the "film plane" in our pinhole camera model)
00356   // used in Maps/almMain.cc
00357   static int curr_img_width = 0;
00358   static float cam_depth_wpix = 0;
00359   static float ciw_2 = 0; // curr_img_width / 2
00360 
00361   // We should never be called if enabledMarkers is off, but here's a test
00362   UNLESS(enabledMarkers) {
00363     cerr << "warning: WorldModel2 handling markers w/o authorization" << endl;
00364     return;
00365   }
00366   // just abort if the AIBO isn't standing erect--we can't do math on the
00367   // vision stuff then. In the future, we'll have inverse kinematics that
00368   // take care of this for us.
00369 //UNCOMMENT THIS IN REAL LIFE
00370   //UNLESS(aiboIsErect()) return;
00371 
00372   // Likewise if the AIBO's head is tilted or rolled. We'll improve this
00373   // later.
00374 //UNCOMMENT THIS IN REAL LIFE
00375   //UNLESS(aiboIsLevelHeaded()) return;
00376 
00377   // Make sure this is a marker event
00378   UNLESS((event.getTypeID() == EventBase::statusETID) &&
00379    (event.getSourceID() == VisionEventNS::MarkersSID)) return;
00380 
00381   // Glean the data
00382   float x = static_cast<const VisionEvent*>(&event)->getCenterX();
00383   int whichMarker = static_cast<const VisionEvent*>(&event)->getProperty();
00384 
00385   // just in case things go crazy
00386   if((whichMarker < 0) || (whichMarker >= AFS_NUM_LANDMARKS)) return;
00387 
00388   // find out the degree offset of the landmark. As in the ground plane
00389   // assumption code, I'm assuming a pinhole camera model. The constants
00390   // used in the following code are drawn from Maps/Configuration.h
00391 
00392   //if(haveSeenMarker[whichMarker]) return; // seen this marker before? quit.
00393 
00394   // The first thing we have to do is see whether the image size has
00395   // changed and alter our own camera model measurements accordingly
00396   if(vision->width != curr_img_width) {
00397     curr_img_width = vision->width;
00398     ciw_2 = ((float)curr_img_width) / 2.0;
00399 
00400     // See almMain.cc method ALM::registerGround for details on this
00401     // computation.
00402     cam_depth_wpix = ((double) ciw_2)*tan(AIBO_CAM_H_FOV/2);
00403   }
00404 
00405   // Now we can find the degree offset of the landmark within the image
00406   float theta = atan2(ciw_2-x, cam_depth_wpix);
00407 
00408   // Let's ignore camera measurements that aren't relatively near to our
00409   // center of vision. Constant is in WorldModel2.h
00410   if(theta*theta > maxMarkerAngle*maxMarkerAngle) return;
00411 
00412   // We add that to our head pan angle to get the actual relative angle
00413   theta += state->outputs[HeadOffset+PanOffset];
00414 
00415   // Now we pass on the creamy data freshness to FastSLAM
00416   afsMeasurement(whichMarker, theta);
00417 
00418 #ifdef DEBUG_WM2
00419   // Print out observation details.
00420   cout << "  OBS: " << whichMarker << " at " << theta
00421        << "\t (" << theta*180/M_PI << ")" << endl;
00422 #endif
00423 
00424   // Now we've officially seen this marker.
00425   haveSeenMarker[whichMarker] = true;
00426 }
00427 #endif
00428 
00429   // process locomotion events--movements
00430 void WorldModel2::processLocomotion(const EventBase& event)
00431 {
00432   long currTime = get_time(); // get the current time
00433 
00434   // See if we know where we've been last. Update our representations' ideas
00435   // of our position given our past trajectory accordingly
00436   if(movingSince) {
00437     long timeDiff = currTime - movingSince;
00438     // Only translate maps, FastSLAM particles if we've been moving
00439     if(moving) {
00440       ALM::move(dx, dy, da, timeDiff);
00441 #ifdef WANT_GLOBAL_MAP
00442       // Add this motion to the list of things for FastSLAM to process. If
00443       // the LazyFastSLAM kludge is on, it won't happen until the AIBO has
00444       // come to a complete stop.
00445       //afsMotion(dx, dy, da, timeDiff);
00446       FastSLAM_update fsu;
00447       fsu.type = FastSLAM_update::MOTION;
00448       fsu.dx = dx;
00449       fsu.dy = dy;
00450       fsu.da = da;
00451       fsu.time = timeDiff;
00452       fs_updates.push_back(fsu);
00453 #endif
00454     }
00455   }
00456 
00457   // Update start time for new trajectory
00458   movingSince = currTime;
00459 
00460   // Update dx,dy,da with new trajectory info
00461   dx = static_cast<const LocomotionEvent*>(&event)->x;
00462   dy = static_cast<const LocomotionEvent*>(&event)->y;
00463   da = static_cast<const LocomotionEvent*>(&event)->a;
00464 
00465   // See if we're moving
00466   // Note use of movingSince test.
00467   if(movingSince && (dx == 0.0) && (dy == 0.0) && (da == 0.0)) moving = false;
00468   else moving = true;
00469 
00470     // Things to do if we've started moving
00471   if(moving) {
00472       // Turn off the GPA timer so that we don't try to do GPA on the move
00473       // If we did do GPA successfully, this will already have happened.
00474     if(enabledGPA) erouter->removeTimer(this);
00475 #ifdef WANT_GLOBAL_MAP
00476       // Turn off marker events for FastSLAM
00477       // We don't seem to be able to request only markers
00478     if(enabledMarkers) erouter->removeListener(this, EventBase::visionEGID);//,
00479                  //VisionEventNS::MarkersSID);
00480 #endif
00481       // Turn off IR ranging
00482     if(enabledIR) erouter->removeListener(this, EventBase::sensorEGID,
00483             SensorSourceID::UpdatedSID);
00484   }
00485     // Things to do if we've stopped
00486   else {
00487       // Turn on GPA timer to trigger GPA if the pose is right
00488     if(enabledGPA) erouter->addTimer(this, 0, GPAdelay, true);
00489 #ifdef WANT_GLOBAL_MAP
00490       // Turn on marker events for FastSLAM
00491       // We don't seem to be able to request only markers
00492     if(enabledMarkers) erouter->addListener(this, EventBase::visionEGID);//,
00493               //VisionEventNS::MarkersSID);
00494 #endif
00495     if(enabledIR) erouter->addListener(this, EventBase::sensorEGID,
00496                SensorSourceID::UpdatedSID);
00497   }
00498 
00499 #ifdef WANT_GLOBAL_MAP
00500   // Clear sensor processing checklist flags, and see if we've seen any
00501   // landmarks. If we've seen more than a small number, then let's do
00502   // a FastSLAM resample.
00503   int numMarkersSeen = 0;
00504   for(int i=0; i<AFS_NUM_LANDMARKS; ++i)
00505     if(haveSeenMarker[i]) {
00506       ++numMarkersSeen;
00507       haveSeenMarker[i] = false;
00508     }
00509   //if(numMarkersSeen >= minMarkersForFastSLAM) afsResample();
00510   if(numMarkersSeen >= minMarkersForFastSLAM) {
00511     FastSLAM_update fsu;
00512     fsu.type = FastSLAM_update::LANDMARK;
00513     fsu.dx = fsu.dy = fsu.da = 0;
00514     fsu.time = 0;
00515     fs_updates.push_back(fsu);
00516   }
00517 
00518 
00519   // If we're moving and we have the LazyFastSLAM kludge enabled, then we
00520   // stop here.
00521   if(moving && (kludges & WM2Kludge::LazyFastSLAM)) return;
00522   // Either we're stopped or LazyFastSLAM is off. Process away.
00523   while(!fs_updates.empty()) {
00524     FastSLAM_update fsu = fs_updates.front();
00525 
00526     // Is this a resampling update or a motion update? Act accordingly.
00527     if(fsu.type == FastSLAM_update::MOTION) {
00528 #ifdef DEBUG_WM2
00529       cout << "MOTION RESAMPLE <" << fsu.dx << ',' << fsu.dy << ',' << fsu.da
00530      << ' ' << fsu.time << ">... " << flush;
00531 #endif
00532       afsMotion(fsu.dx, fsu.dy, fsu.da, fsu.time);
00533 #ifdef DEBUG_WM2
00534       cout << "done." << endl;
00535       afsWhatsUp(NULL);
00536 #endif
00537     }
00538     else if(fsu.type == FastSLAM_update::LANDMARK) {
00539 #ifdef DEBUG_WM2
00540       cout << "LANDMARK RESAMPLE... " << flush;
00541 #endif
00542       afsResample();
00543 #ifdef DEBUG_WM2
00544       cout << "done." << endl;
00545       afsWhatsUp(NULL);
00546 #endif
00547     }
00548 
00549     // Next!
00550     fs_updates.pop_front();
00551   }
00552 #endif
00553 }
00554 
00555   // Try to do ground plane assumption
00556 void WorldModel2::processGround()
00557 {
00558   // We should never be called if enabledGPA is off, but here's a test anyway
00559   UNLESS(enabledGPA) {
00560     cerr << "warning: WorldModel2 doing GPA w/o authorization" << endl;
00561     return;
00562   }
00563   // Make sure AIBO is standing erect AND facing dead ahead
00564   UNLESS(aiboIsErect() && aiboStaresDeadAhead()) return;
00565 
00566   // Delete the ground plane assumption timer
00567   erouter->removeTimer(this);
00568 
00569   // Go ahead and do the ground plane assumption
00570   ALM::registerGround();
00571 }
00572 
00573 #ifdef WANT_GLOBAL_MAP
00574   // Gets the current best FastSLAM map and places its contents in p
00575 void WorldModel2::getFastSLAMMap(afsParticle &p)
00576 {
00577   afsParticle *best = afsWhatsUp(NULL);
00578 
00579   p = *best;
00580 }
00581 #endif
00582 
00583   // Perform serialization -- send data down the line, if needed.
00584 void WorldModel2::serialize()
00585 {
00586   char *buf;
00587 
00588   // Encode depth map data
00589   buf = (char *)sockDM->getWriteBuffer(DM_CELL_COUNT*sizeof(dm_cell));
00590   if(buf) {
00591     dm_cell *DMp = ALM::getDM();
00592 
00593     for(int i=0; i<DM_CELL_COUNT; ++i) {
00594       dm_cell *cel = &DMp[i];
00595       encode(&buf, cel->depth);
00596       encode(&buf, cel->confidence);
00597       encode(&buf, cel->color);
00598     }
00599     sockDM->write(DM_CELL_COUNT*(sizeof(float)*2 + sizeof(colortype)));
00600   }
00601 
00602   // Encode height map data
00603   buf = (char *)sockHM->getWriteBuffer(HM_CELL_COUNT*sizeof(hm_cell));
00604   if(buf) {
00605     hm_cell *HMp = ALM::getHM();
00606 
00607     for(int i=0; i<HM_CELL_COUNT; ++i) {
00608       hm_cell *cel = &HMp[i];
00609       encode(&buf, cel->height);
00610       encode(&buf, cel->trav);
00611       encode(&buf, cel->confidence);
00612       encode(&buf, cel->color);
00613     }
00614     // colortype is defined in Maps/almStructures.h
00615     sockHM->write(HM_CELL_COUNT*(sizeof(float)*3 + sizeof(colortype)));
00616   }
00617 
00618 #ifdef WANT_GLOBAL_MAP
00619   // Encode global map data
00620   buf = (char *)sockGM->getWriteBuffer(GM_CELL_COUNT*sizeof(hm_cell));
00621   if(buf) {
00622     hm_cell *GMp = AGM::getGM();
00623 
00624     for(int i=0; i<GM_CELL_COUNT; ++i) {
00625       hm_cell *cel = &GMp[i];
00626       encode(&buf, cel->height);
00627       encode(&buf, cel->trav);
00628       encode(&buf, cel->confidence);
00629       encode(&buf, cel->color);
00630     }
00631     // colortype is defined in Maps/almStructures.h
00632     sockGM->write(GM_CELL_COUNT*(sizeof(float)*3 + sizeof(colortype)));
00633   }
00634 
00635   // Encode FastSLAM map and location data
00636   buf = (char *)sockFS->getWriteBuffer(AFS_NUM_PARTICLES*sizeof(afsPose) +
00637                sizeof(afsParticle));
00638   if(buf) {
00639     // Insert the locations of all the particles
00640     afsParticle *Particles = afsInvadeFSData(); // Routine not for general use!
00641     for(int i=0; i<AFS_NUM_PARTICLES; ++i) {
00642       encode(&buf, (float) Particles[i].pose.x);
00643       encode(&buf, (float) Particles[i].pose.y);
00644       encode(&buf, (float) Particles[i].pose.theta);
00645     }
00646 
00647     // Send information about the best particle
00648     afsParticle *p = afsWhatsUp(NULL);
00649 
00650     encode(&buf, (float) p->pose.x);
00651     encode(&buf, (float) p->pose.y);
00652     encode(&buf, (float) p->pose.theta);
00653     for(int i=0; i<AFS_NUM_LANDMARKS; ++i) {
00654       afsLandmarkLoc *l = &p->landmarks[i];
00655 
00656       // Just use HUGE_VAL to indicate uninitialized landmarks. The code
00657       // at the other end will detect these unreasonable values and act
00658       // accordingly
00659       if(l->state == PRIMING) {
00660   encode(&buf, (float) HUGE_VAL);
00661   encode(&buf, (float) HUGE_VAL);
00662   encode(&buf, (float) HUGE_VAL);
00663   encode(&buf, (float) HUGE_VAL);
00664   encode(&buf, (float) HUGE_VAL);
00665       }
00666       else {  // state == PRIMED
00667   encode(&buf, (float) l->mean.x);
00668   encode(&buf, (float) l->mean.y);
00669   encode(&buf, (float) l->variance.x);
00670   encode(&buf, (float) l->variance.xy);
00671   encode(&buf, (float) l->variance.y);
00672       }
00673     }
00674     sockFS->write(AFS_NUM_PARTICLES*(3*sizeof(float)) +
00675       (3*sizeof(float)) + AFS_NUM_LANDMARKS*(5*sizeof(float)));
00676   }
00677 #endif
00678 }
00679 
00680   // Determine whether Aibo is in an erect stature, allowing us to do
00681   // measurements. TODO: replace with a real motion model
00682 bool aiboIsErect()
00683 {
00684   double diff;
00685 
00686   diff = SP_LFR_JOINT - state->outputs[LFrLegOffset+RotatorOffset];
00687   if(diff*diff > SP_TOLERANCE) return false;
00688   diff = SP_LFR_SHLDR - state->outputs[LFrLegOffset+ElevatorOffset];
00689   if(diff*diff > SP_TOLERANCE) return false;
00690   diff = SP_LFR_KNEE  - state->outputs[LFrLegOffset+KneeOffset];
00691   if(diff*diff > SP_TOLERANCE) return false;
00692 
00693   diff = SP_RFR_JOINT - state->outputs[RFrLegOffset+RotatorOffset];
00694   if(diff*diff > SP_TOLERANCE) return false;
00695   diff = SP_RFR_SHLDR - state->outputs[RFrLegOffset+ElevatorOffset];
00696   if(diff*diff > SP_TOLERANCE) return false;
00697   diff = SP_RFR_KNEE  - state->outputs[RFrLegOffset+KneeOffset];
00698   if(diff*diff > SP_TOLERANCE) return false;
00699 
00700   diff = SP_LFR_JOINT - state->outputs[LBkLegOffset+RotatorOffset];
00701   if(diff*diff > SP_TOLERANCE) return false;
00702   diff = SP_LFR_SHLDR - state->outputs[LBkLegOffset+ElevatorOffset];
00703   if(diff*diff > SP_TOLERANCE) return false;
00704   diff = SP_LFR_KNEE  - state->outputs[LBkLegOffset+KneeOffset];
00705   if(diff*diff > SP_TOLERANCE) return false;
00706 
00707   diff = SP_RFR_JOINT - state->outputs[RBkLegOffset+RotatorOffset];
00708   if(diff*diff > SP_TOLERANCE) return false;
00709   diff = SP_RFR_SHLDR - state->outputs[RBkLegOffset+ElevatorOffset];
00710   if(diff*diff > SP_TOLERANCE) return false;
00711   diff = SP_RFR_KNEE  - state->outputs[RBkLegOffset+KneeOffset];
00712   if(diff*diff > SP_TOLERANCE) return false;
00713 
00714   return true;
00715 }
00716 
00717   // Determine whether Aibo is looking dead ahead. Needed for ground plane
00718   // assumption, for the moment.
00719 bool aiboStaresDeadAhead()
00720 {
00721   double diff;
00722 
00723   diff = DA_TILT - state->outputs[HeadOffset+TiltOffset];
00724   if(diff*diff > DA_TOLERANCE) return false;
00725   diff = DA_PAN  - state->outputs[HeadOffset+PanOffset];
00726   if(diff*diff > DA_TOLERANCE) return false;
00727   diff = DA_ROLL - state->outputs[HeadOffset+RollOffset];
00728   if(diff*diff > DA_TOLERANCE) return false;
00729 
00730   return true;
00731 }
00732 
00733   // Determine whether Aibo is keeping its head level (i.e. no tilt or roll).
00734   // Needed for FastSLAM at the moment
00735 bool aiboIsLevelHeaded()
00736 {
00737   double diff;
00738 
00739   diff = DA_TILT - state->outputs[HeadOffset+TiltOffset];
00740   if(diff*diff > DA_TOLERANCE) return false;
00741   diff = DA_ROLL - state->outputs[HeadOffset+RollOffset];
00742   if(diff*diff > DA_TOLERANCE) return false;
00743 
00744   return true;
00745 }

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