00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "WorldModel2.h"
00017
00018
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
00030 #include "FastSLAM/afsMain.h"
00031 #include "Maps/almMain.h"
00032 #include "Maps/agmMain.h"
00033 #include "Maps/almStructures.h"
00034
00035 #include "FastSLAM/Configuration.h"
00036
00037 #include "FastSLAM/afsParticle.h"
00038
00039 #include "Maps/Configuration.h"
00040
00041 #include "Poses.h"
00042
00043
00044 #include "Shared/get_time.h"
00045
00046
00047 #define UNLESS(item) if(!( (item) ))
00048
00049
00050
00051
00052
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
00060
00061 bool WorldModel2::locked = false;
00062
00063
00064
00065 WorldModel2::WorldModel2()
00066
00067
00068 : kludges(0),
00069
00070 moving(true),
00071
00072 enabledIR(false), enabledGPA(false),
00073
00074
00075 dx(0.0), dy(0.0), da(0.0), movingSince(0),
00076
00077
00078
00079
00080
00081
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
00097 if(locked) return;
00098
00099 locked = true;
00100
00101 #ifdef WANT_GLOBAL_MAP
00102
00103 for(int i=0; i<AFS_NUM_LANDMARKS; ++i) haveSeenMarker[i] = false;
00104 #endif
00105
00106
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
00115 resetWorldModel();
00116
00117
00118 erouter->addListener(this,EventBase::locomotionEGID);
00119
00120
00121 erouter->addTimer(this, TIMER_SID_SRL, SRLdelay, true);
00122 }
00123
00124
00125 WorldModel2::~WorldModel2()
00126 {
00127
00128 disableIR();
00129 disableGPA();
00130 #ifdef WANT_GLOBAL_MAP
00131 disableMarkers();
00132 #endif
00133 erouter->forgetListener(this);
00134
00135
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
00144 locked = false;
00145 }
00146
00147
00148
00149
00150
00151 void WorldModel2::enableIR()
00152 {
00153
00154 erouter->addListener(this, EventBase::sensorEGID, SensorSourceID::UpdatedSID);
00155 enabledIR = true;
00156 }
00157 void WorldModel2::disableIR()
00158 {
00159
00160 erouter->removeListener(this, EventBase::sensorEGID,
00161 SensorSourceID::UpdatedSID);
00162 enabledIR = false;
00163 }
00164
00165
00166
00167
00168
00169 void WorldModel2::enableGPA()
00170 {
00171
00172
00173
00174 if(movingSince && !moving)
00175 erouter->addTimer(this, TIMER_SID_GPA, GPAdelay, true);
00176
00177 enabledGPA = true;
00178 }
00179 void WorldModel2::disableGPA()
00180 {
00181
00182 erouter->removeTimer(this, TIMER_SID_GPA);
00183
00184 enabledGPA = false;
00185 }
00186
00187
00188
00189
00190 void WorldModel2::enableKludge(unsigned int kludge)
00191 {
00192 kludges |= kludge;
00193 }
00194
00195
00196 void WorldModel2::disableKludge(unsigned int kludge)
00197 {
00198 kludges &= ~kludge;
00199 }
00200
00201
00202 #ifdef WANT_GLOBAL_MAP
00203
00204 void WorldModel2::enableMarkers()
00205 {
00206
00207 vision->enableEvents(VisionEventNS::MarkersSID);
00208
00209
00210 if(movingSince && !moving)
00211 erouter->addListener(this,EventBase::visionEGID, VisionEventNS::MarkersSID);
00212 enabledMarkers = true;
00213 }
00214 void WorldModel2::disableMarkers()
00215 {
00216
00217 vision->disableEvents(VisionEventNS::MarkersSID);
00218 erouter->removeListener(this, EventBase::visionEGID,
00219 VisionEventNS::MarkersSID);
00220 enabledMarkers = false;
00221 }
00222 #endif
00223
00224
00225
00226
00227 void WorldModel2::resetWorldModel()
00228 {
00229
00230 ALM::init();
00231 #ifdef WANT_GLOBAL_MAP
00232
00233 AGM::init();
00234
00235 afsInit();
00236 #endif
00237 }
00238
00239 #ifdef WANT_GLOBAL_MAP
00240
00241
00242 void WorldModel2::setLandmark(int landmark,
00243 double x, double y, double covariance)
00244 {
00245 afsSetLandmark(landmark, x, y, covariance);
00246 }
00247
00248
00249
00250
00251
00252 void WorldModel2::fsDistribute(double lx, double ty, double rx, double by)
00253 {
00254 afsDistribute(lx, ty, rx, by);
00255 }
00256 #endif
00257
00258
00259 void WorldModel2::processEvent(const EventBase& event)
00260 {
00261
00262 switch(event.getGeneratorID()) {
00263
00264 case EventBase::sensorEGID: processSensors(); break;
00265 #ifdef WANT_GLOBAL_MAP
00266
00267 case EventBase::visionEGID: processVision(event); break;
00268 #endif
00269
00270 case EventBase::locomotionEGID: processLocomotion(event); break;
00271
00272
00273 case EventBase::timerEGID:
00274 if(event.getSourceID() == TIMER_SID_GPA) processGround();
00275 else if(event.getSourceID() == TIMER_SID_SRL) serialize();
00276 break;
00277
00278 default:
00279 cerr << "warning: WorldModel2 receved unrequested event" << endl;
00280 }
00281 }
00282
00283
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
00294
00295 void WorldModel2::pickDMData(dmPicker &p, float *dest)
00296 {
00297
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
00304
00305 void WorldModel2::pickHMData(hmPicker &p, float *dest)
00306 {
00307
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
00315
00316 void WorldModel2::pickGMData(hmPicker &p, float *dest)
00317 {
00318
00319
00320
00321
00322
00323 }
00324 hm_cell *WorldModel2::invadeGMData(void) { return NULL; }
00325 #endif
00326
00327
00328
00329 void WorldModel2::processSensors()
00330 {
00331
00332 UNLESS(enabledIR) {
00333 cerr << "warning: WorldModel2 handling IR event w/o authorization" << endl;
00334 return;
00335 }
00336
00337
00338
00339
00340
00341
00342
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
00352 void WorldModel2::processVision(const EventBase& event)
00353 {
00354
00355
00356
00357 static int curr_img_width = 0;
00358 static float cam_depth_wpix = 0;
00359 static float ciw_2 = 0;
00360
00361
00362 UNLESS(enabledMarkers) {
00363 cerr << "warning: WorldModel2 handling markers w/o authorization" << endl;
00364 return;
00365 }
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378 UNLESS((event.getTypeID() == EventBase::statusETID) &&
00379 (event.getSourceID() == VisionEventNS::MarkersSID)) return;
00380
00381
00382 float x = static_cast<const VisionEvent*>(&event)->getCenterX();
00383 int whichMarker = static_cast<const VisionEvent*>(&event)->getProperty();
00384
00385
00386 if((whichMarker < 0) || (whichMarker >= AFS_NUM_LANDMARKS)) return;
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396 if(vision->width != curr_img_width) {
00397 curr_img_width = vision->width;
00398 ciw_2 = ((float)curr_img_width) / 2.0;
00399
00400
00401
00402 cam_depth_wpix = ((double) ciw_2)*tan(AIBO_CAM_H_FOV/2);
00403 }
00404
00405
00406 float theta = atan2(ciw_2-x, cam_depth_wpix);
00407
00408
00409
00410 if(theta*theta > maxMarkerAngle*maxMarkerAngle) return;
00411
00412
00413 theta += state->outputs[HeadOffset+PanOffset];
00414
00415
00416 afsMeasurement(whichMarker, theta);
00417
00418 #ifdef DEBUG_WM2
00419
00420 cout << " OBS: " << whichMarker << " at " << theta
00421 << "\t (" << theta*180/M_PI << ")" << endl;
00422 #endif
00423
00424
00425 haveSeenMarker[whichMarker] = true;
00426 }
00427 #endif
00428
00429
00430 void WorldModel2::processLocomotion(const EventBase& event)
00431 {
00432 long currTime = get_time();
00433
00434
00435
00436 if(movingSince) {
00437 long timeDiff = currTime - movingSince;
00438
00439 if(moving) {
00440 ALM::move(dx, dy, da, timeDiff);
00441 #ifdef WANT_GLOBAL_MAP
00442
00443
00444
00445
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
00458 movingSince = currTime;
00459
00460
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
00466
00467 if(movingSince && (dx == 0.0) && (dy == 0.0) && (da == 0.0)) moving = false;
00468 else moving = true;
00469
00470
00471 if(moving) {
00472
00473
00474 if(enabledGPA) erouter->removeTimer(this);
00475 #ifdef WANT_GLOBAL_MAP
00476
00477
00478 if(enabledMarkers) erouter->removeListener(this, EventBase::visionEGID);
00479
00480 #endif
00481
00482 if(enabledIR) erouter->removeListener(this, EventBase::sensorEGID,
00483 SensorSourceID::UpdatedSID);
00484 }
00485
00486 else {
00487
00488 if(enabledGPA) erouter->addTimer(this, 0, GPAdelay, true);
00489 #ifdef WANT_GLOBAL_MAP
00490
00491
00492 if(enabledMarkers) erouter->addListener(this, EventBase::visionEGID);
00493
00494 #endif
00495 if(enabledIR) erouter->addListener(this, EventBase::sensorEGID,
00496 SensorSourceID::UpdatedSID);
00497 }
00498
00499 #ifdef WANT_GLOBAL_MAP
00500
00501
00502
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
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
00520
00521 if(moving && (kludges & WM2Kludge::LazyFastSLAM)) return;
00522
00523 while(!fs_updates.empty()) {
00524 FastSLAM_update fsu = fs_updates.front();
00525
00526
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
00550 fs_updates.pop_front();
00551 }
00552 #endif
00553 }
00554
00555
00556 void WorldModel2::processGround()
00557 {
00558
00559 UNLESS(enabledGPA) {
00560 cerr << "warning: WorldModel2 doing GPA w/o authorization" << endl;
00561 return;
00562 }
00563
00564 UNLESS(aiboIsErect() && aiboStaresDeadAhead()) return;
00565
00566
00567 erouter->removeTimer(this);
00568
00569
00570 ALM::registerGround();
00571 }
00572
00573 #ifdef WANT_GLOBAL_MAP
00574
00575 void WorldModel2::getFastSLAMMap(afsParticle &p)
00576 {
00577 afsParticle *best = afsWhatsUp(NULL);
00578
00579 p = *best;
00580 }
00581 #endif
00582
00583
00584 void WorldModel2::serialize()
00585 {
00586 char *buf;
00587
00588
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
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
00615 sockHM->write(HM_CELL_COUNT*(sizeof(float)*3 + sizeof(colortype)));
00616 }
00617
00618 #ifdef WANT_GLOBAL_MAP
00619
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
00632 sockGM->write(GM_CELL_COUNT*(sizeof(float)*3 + sizeof(colortype)));
00633 }
00634
00635
00636 buf = (char *)sockFS->getWriteBuffer(AFS_NUM_PARTICLES*sizeof(afsPose) +
00637 sizeof(afsParticle));
00638 if(buf) {
00639
00640 afsParticle *Particles = afsInvadeFSData();
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
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
00657
00658
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 {
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
00681
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
00718
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
00734
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 }