Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Lookout.cc

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 #include "Events/EventRouter.h"
00004 #include "Events/LocomotionEvent.h"
00005 #include "Events/LookoutEvents.h"
00006 #include "Events/VisionObjectEvent.h"
00007 #include "Shared/ProjectInterface.h"
00008 #include "Shared/mathutils.h"
00009 #include "Shared/WorldState.h"
00010 #include "Vision/RegionGenerator.h"
00011 
00012 #include "VRmixin.h"
00013 #include "LookoutRequests.h"
00014 #include "Lookout.h"
00015 #include "MapBuilder.h"
00016 #include "ShapeBlob.h"
00017 #include "ShapeLine.h"
00018 #include "ShapePolygon.h"
00019 
00020 using namespace mathutils;
00021 using namespace std;
00022 
00023 namespace DualCoding {
00024 
00025 Lookout::Lookout()
00026   : BehaviorBase("Lookout"),
00027     pixelHistograms(VRmixin::camSkS.getNumPixels()), distanceSamples(),
00028     pointer_id(MotionManager::invalid_MC_ID),
00029     posture_id(MotionManager::invalid_MC_ID),
00030     sequence_id(MotionManager::invalid_MC_ID),
00031     requests(), curReq(NULL), curPAR(NULL), successSave(false),
00032     trackerState(inactive), idCounter(0)
00033 {}
00034 
00035 void Lookout::DoStart() {
00036   BehaviorBase::DoStart();
00037   SharedObject<HeadPointerMC> head_mc;
00038   SharedObject<PostureMC> posture_mc;
00039   SharedObject<MediumMotionSequenceMC> mseq_mc;
00040   pointer_id = motman->addPersistentMotion(head_mc,MotionManager::kIgnoredPriority);
00041   posture_id = motman->addPersistentMotion(posture_mc,MotionManager::kIgnoredPriority);
00042   sequence_id = motman->addPersistentMotion(mseq_mc,MotionManager::kIgnoredPriority);
00043   cout << "Lookout starting up: pointer_id=" << pointer_id
00044        << " posture_id=" << posture_id
00045        << " sequence_id=" << sequence_id << endl;
00046   erouter->addListener(this,EventBase::motmanEGID,pointer_id,EventBase::statusETID);
00047   erouter->addListener(this,EventBase::motmanEGID,posture_id,EventBase::statusETID);
00048   erouter->addListener(this,EventBase::motmanEGID,sequence_id,EventBase::statusETID);
00049 }
00050 
00051 void Lookout::DoStop() {
00052   motman->removeMotion(pointer_id);
00053   pointer_id = MotionManager::invalid_MC_ID;
00054   motman->removeMotion(posture_id);
00055   posture_id = MotionManager::invalid_MC_ID;
00056   motman->removeMotion(sequence_id);
00057   sequence_id = MotionManager::invalid_MC_ID;
00058   curReq = NULL;
00059   curPAR = NULL;
00060   while (!requests.empty()) {
00061     delete requests.front();
00062     requests.pop();
00063   }
00064   BehaviorBase::DoStop();
00065 }
00066 
00067 vector<DualCoding::Point> Lookout::groundSearchPoints() {
00068   vector<Point> gazePts;
00069   gazePts.push_back(Point( 200, -250, -100, egocentric));
00070   gazePts.push_back(Point( 800,-1000, -100, egocentric));
00071   gazePts.push_back(Point(1200,    0, -100, egocentric));
00072   gazePts.push_back(Point( 800, 1000, -100, egocentric));
00073   gazePts.push_back(Point( 200,  250, -100, egocentric));
00074   gazePts.push_back(Point( 200,    0, -100, egocentric));
00075   gazePts.push_back(Point( 400,    0, -100, egocentric));
00076   gazePts.push_back(Point( 800,    0, -100, egocentric));
00077   return gazePts;
00078 }
00079 
00080 unsigned int Lookout::executeRequest(const LookoutRequest &req) {
00081   switch (req.getHeadMotionType()) {
00082   case LookoutRequest::noMotion:
00083   case LookoutRequest::pointAt:
00084     pushRequest<LookoutPointRequest>(req);
00085     break;
00086   case LookoutRequest::scan:
00087     pushRequest<LookoutScanRequest>(req);
00088     break;
00089   case LookoutRequest::track:
00090     pushRequest<LookoutTrackRequest>(req); 
00091     break;
00092   case LookoutRequest::search:
00093     pushRequest<LookoutSearchRequest>(req); 
00094     break;
00095   default:
00096     cout << "Lookout::executeRequest: unknown request type " << req.getHeadMotionType() << endl;
00097   };
00098   const unsigned int reqid = requests.back()->requestID = ++idCounter;  
00099   executeRequest();
00100   return reqid;
00101 }
00102 
00103 void Lookout::executeRequest() {
00104   if ( curReq != NULL || requests.empty() )
00105     return;
00106   curReq = requests.front();
00107   cout << "Lookout executing " << LookoutRequest::headMotionTypeNames[curReq->getHeadMotionType()] 
00108        << " request, id=" << curReq->requestID << endl;
00109   curPAR = dynamic_cast<LookoutPointRequest*>(curReq);
00110 
00111   // If we're going to be computing pixel or distance modes, clear the bins first
00112   if ( curPAR != NULL && curPAR->numSamples > 1 )
00113     switch ( curPAR->getResultType() ) {
00114     case LookoutRequest::imageResult:
00115       for ( unsigned int i=0; i<pixelHistograms.size(); i++ )
00116   pixelHistograms[i].clear();
00117       break;
00118 #ifdef TGT_HAS_IR_DISTANCE
00119     case LookoutRequest::distanceResult:
00120       while ( distanceSamples.size() > 0) distanceSamples.pop();
00121       break;
00122 #endif
00123     default:
00124       break;
00125     }
00126 
00127   // now dispatch on the head motion type
00128   trackerState = inactive;
00129   switch (curReq->getHeadMotionType()) {
00130   case LookoutRequest::noMotion:
00131     erouter->addTimer(this, settle_timer, curPAR->motionSettleTime, false);
00132     break;
00133   case LookoutRequest::pointAt:
00134     moveHeadToPoint();
00135     break;
00136   case LookoutRequest::scan:
00137     setupScan();
00138     break;
00139   case LookoutRequest::track:
00140     setupTrack();
00141     break;
00142   case LookoutRequest::search:
00143     setupSearch();
00144     break;
00145   default:
00146     cout << "Lookout::executeRequest(): unknown request " << curReq->getHeadMotionType() << endl;
00147     break;
00148   };
00149 }
00150 
00151 void Lookout::relax() {
00152   motman->setPriority(pointer_id,MotionManager::kIgnoredPriority);
00153   motman->setPriority(posture_id,MotionManager::kIgnoredPriority);
00154   motman->setPriority(sequence_id,MotionManager::kIgnoredPriority);
00155 }
00156 
00157 void Lookout::moveHeadToPoint() {
00158   Point pt = curPAR->gazePt;
00159   switch ( pt.getRefFrameType() ) {
00160   case unspecified:
00161   case camcentric:
00162     cout << "Warning: Lookout gaze point " << curPAR->gazePt << " reference frame must be egocentric or allocentric" << endl;
00163     pt.setRefFrameType(egocentric);
00164   case egocentric:
00165     break;
00166   case allocentric:
00167     pt.applyTransform(VRmixin::mapBuilder.worldToLocalTranslateMatrix,egocentric);
00168     pt.applyTransform(VRmixin::mapBuilder.worldToLocalRotateMatrix,egocentric);
00169   }
00170   // point head based on Camera or IR reference frame
00171   switch ( curPAR->getResultType() ) {
00172   case LookoutRequest::noResult:
00173   case LookoutRequest::imageResult: {
00174     successSave = MMAccessor<HeadPointerMC>(pointer_id)->lookAtPoint(pt.coordX(),pt.coordY(),pt.coordZ());
00175     motman->setPriority(pointer_id,MotionManager::kStdPriority);
00176     motman->setPriority(posture_id,MotionManager::kIgnoredPriority);
00177     motman->setPriority(sequence_id,MotionManager::kIgnoredPriority);
00178   }
00179     break;
00180 #ifdef TGT_HAS_IR_DISTANCE
00181   case LookoutRequest::distanceResult: {
00182 #ifdef TGT_ERS7
00183     successSave = MMAccessor<PostureMC>(posture_id)->solveLinkVector(pt.coords,NearIRFrameOffset,Kinematics::pack(0,0,1));
00184 #else
00185     successSave = MMAccessor<PostureMC>(posture_id)->solveLinkVector(pt.coords,IRFrameOffset,Kinematics::pack(0,0,1));
00186 #endif
00187     motman->setPriority(posture_id,MotionManager::kStdPriority);
00188     motman->setPriority(pointer_id,MotionManager::kIgnoredPriority);
00189     motman->setPriority(sequence_id,MotionManager::kIgnoredPriority);
00190     }
00191     break;
00192 #endif
00193   case LookoutRequest::interestPoints:
00194     cout << "Lookout error: this scan type cannot return interest points." << endl;
00195     return;
00196   }
00197 }
00198 
00199 void Lookout::processEvent(const EventBase& event) {
00200   if ( curReq == NULL ) {
00201     if ( event.getGeneratorID() == EventBase::motmanEGID &&
00202    event.getTypeID() == EventBase::statusETID )
00203       return; // harmless motman status event from startup
00204     else {
00205       cout << "Error:  Lookout received an event when not executing a request:\n";
00206       cout << "    " << event.getDescription(true,3) << endl;
00207       return;
00208     }
00209   }
00210 
00211   switch (curReq->getHeadMotionType()) {
00212   case LookoutRequest::noMotion:
00213   case LookoutRequest::pointAt:
00214     processPointAtEvent(event);
00215     break;
00216   case LookoutRequest::scan:
00217     processScanEvent(event);
00218     break;
00219   case LookoutRequest::track:
00220     processTrackEvent(event);
00221     break;
00222   case LookoutRequest::search:
00223     processSearchEvent(event);
00224     break;
00225   default:
00226     cout << "Lookout::processEvent: unknown head motion request type: "
00227    << curReq->getHeadMotionType() << ", event: " << event.getDescription() << endl;
00228     break;
00229   };
00230 }
00231 
00232 void Lookout::processPointAtEvent(const EventBase& event) {
00233   switch (event.getGeneratorID()) {
00234   case EventBase::motmanEGID:
00235     if ( event.getSourceID() == pointer_id || event.getSourceID() == posture_id ) {
00236       // head motion complete, now wait for head to settle
00237       motman->setPriority(event.getSourceID(), MotionManager::kBackgroundPriority);
00238       erouter->addTimer(this, settle_timer, curPAR->motionSettleTime, false);
00239     }
00240     break;
00241 
00242   case EventBase::timerEGID:
00243     if (event.getSourceID() == settle_timer || event.getSourceID() == sample_timer) {
00244       switch (curReq->getResultType()) {
00245       case LookoutRequest::imageResult:
00246   erouter->addListener(this, EventBase::visRegionEGID,
00247            ProjectInterface::visRegionSID,EventBase::statusETID);
00248   break;
00249 #ifdef TGT_HAS_IR_DISTANCE
00250       case LookoutRequest::distanceResult:
00251   erouter->addListener(this, EventBase::sensorEGID, SensorSrcID::UpdatedSID);
00252   break;
00253 #endif
00254       case LookoutRequest::noResult:
00255       default:
00256   requestComplete(successSave);
00257   break;
00258       };
00259     }
00260     break;
00261 
00262   case EventBase::visRegionEGID:
00263     erouter->removeListener(this, EventBase::visRegionEGID);
00264     VRmixin::camSkS.clear();
00265     VRmixin::camShS.clear();
00266     curPAR->image.bind((curPAR->sketchFunc)());
00267     ++curPAR->sampleCounter;
00268     if ( curPAR->numSamples == 1 || findPixelModes() == true ) {
00269       curPAR->toBaseMatrix = kine->jointToBase(curPAR->joint);
00270       requestComplete(successSave);
00271     }
00272     else
00273       erouter->addTimer(this, sample_timer, curPAR->sampleInterval, false);
00274     break;
00275 
00276   case EventBase::sensorEGID:
00277     erouter->removeListener(this, EventBase::sensorEGID);
00278 #ifdef TGT_HAS_IR_DISTANCE
00279     if ( findDistanceMode() == true ) {
00280       curPAR->toBaseMatrix = kine->jointToBase(curPAR->joint);
00281       requestComplete(successSave);
00282     } else
00283 #endif
00284       erouter->addTimer(this, sample_timer, curPAR->sampleInterval, false);
00285     break;
00286 
00287   default:
00288     cout << "Lookout::processPointAtEvent: unknown event " << event.getDescription() << endl;
00289     break;
00290   };
00291 }
00292 
00293 bool Lookout::findPixelModes() {
00294   // update our counts using the new sammple
00295   for (size_t i = 0; i<pixelHistograms.size(); i++)
00296     pixelHistograms[i][curPAR->image[i]]++;
00297   if ( curPAR->sampleCounter < curPAR->numSamples )
00298     return false;
00299   // we have enough samples; compute the mode
00300   for (size_t i = 0; i<pixelHistograms.size(); i++) {
00301     unsigned int maxCount = 0;
00302     uchar maxChar = 0;
00303     for (map<uchar, unsigned int>::const_iterator it = pixelHistograms[i].begin();
00304    it != pixelHistograms[i].end(); it++)
00305       if (it->second > maxCount) {
00306   maxCount = it->second;
00307   maxChar = it->first;
00308       }
00309     curPAR->image[i] = maxChar;
00310   }
00311   return true;
00312 }
00313 
00314 #ifdef TGT_HAS_IR_DISTANCE
00315 float Lookout::getDistanceModeValue() {
00316   int const npops = distanceSamples.size() / 2;
00317   for ( int i=0; i<npops; i++ ) distanceSamples.pop();
00318   return distanceSamples.top();
00319 }
00320 
00321 inline float getIR() {
00322 #ifdef TGT_ERS7 
00323   if ( state->sensors[FarIRDistOffset] > 400 )  // far IR goes 200-1500; near IR goes 50-500
00324     return state->sensors[FarIRDistOffset];
00325   else
00326     return state->sensors[NearIRDistOffset];
00327 #else 
00328   return state->sensors[IRDistOffset];
00329 #endif
00330 }
00331 
00332 bool Lookout::findDistanceMode() {
00333   distanceSamples.push(getIR());
00334   return (int)distanceSamples.size() < curPAR->numSamples;
00335 }
00336 #endif // TGT_HAS_IR_DISTANCE
00337 
00338 void Lookout::requestComplete(bool success) {
00339   cout << "Lookout request " << curReq->requestID << " complete." << endl;
00340   erouter->removeTimer(this);
00341   erouter->removeListener(this,EventBase::sensorEGID);
00342   erouter->removeListener(this,EventBase::visSegmentEGID);
00343   erouter->removeListener(this,EventBase::visRegionEGID);
00344   erouter->removeListener(this,EventBase::visObjEGID);
00345   LookoutRequest *saveReq = curReq;
00346   curReq = NULL;
00347   curPAR = NULL;
00348   requests.pop();
00349   switch ( saveReq->getHeadMotionType() ) {
00350   case LookoutRequest::noMotion:
00351   case LookoutRequest::pointAt:
00352     switch ( saveReq->getResultType() ) {
00353     case LookoutRequest::noResult:
00354       erouter->postEvent(LookoutPointAtEvent(success,static_cast<LookoutPointRequest*>(saveReq)->toBaseMatrix,
00355                EventBase::lookoutEGID, saveReq->requestID, EventBase::deactivateETID));
00356       break;
00357     case LookoutRequest::imageResult:
00358       erouter->postEvent(LookoutSketchEvent(success,static_cast<LookoutPointRequest*>(saveReq)->image,
00359               static_cast<LookoutPointRequest*>(saveReq)->toBaseMatrix,
00360               EventBase::lookoutEGID, saveReq->requestID, EventBase::deactivateETID));
00361       break;
00362 #ifdef TGT_HAS_IR_DISTANCE
00363     case LookoutRequest::distanceResult:
00364       erouter->postEvent(LookoutIREvent(success,getDistanceModeValue(), static_cast<LookoutPointRequest*>(saveReq)->toBaseMatrix,
00365           EventBase::lookoutEGID, saveReq->requestID, EventBase::deactivateETID));
00366       break;
00367 #endif
00368     default:
00369       cout << "Lookout::requestComplete(): Unknown type returned by getResultType()\n";
00370     }
00371     break;
00372 
00373   case LookoutRequest::scan:
00374     erouter->postEvent(LookoutScanEvent(static_cast<LookoutScanRequest*>(saveReq)->tasks,
00375           EventBase::lookoutEGID,saveReq->requestID, EventBase::deactivateETID));
00376     break;
00377 
00378   case LookoutRequest::track:
00379     erouter->postEvent(EventBase(EventBase::lookoutEGID,saveReq->requestID, EventBase::deactivateETID));
00380     break;
00381 
00382   case LookoutRequest::search: {
00383     Sketch<uchar> image(VRmixin::sketchFromSeg());
00384 #ifdef TGT_HAS_CAMERA
00385     const NEWMAT::Matrix camToBase = kine->jointToBase(CameraFrameOffset);
00386 #else
00387     NEWMAT::Matrix camToBase(4,4);
00388     camToBase << ROBOOP::fourbyfourident;
00389 #endif
00390     erouter->postEvent(LookoutSketchEvent(success, image, camToBase,
00391             EventBase::lookoutEGID, saveReq->requestID, EventBase::deactivateETID));
00392     break;
00393   }
00394 
00395   default:
00396     cout << "Lookout::requestComplete(): Unknown head motion type\n";
00397   }
00398 
00399   // The postEvent above may have led to a series of functions
00400   // returning and eventually shutting down the Lookout or starting
00401   // another request, so make sure we're still in the same state
00402   // before proceeding.
00403   delete saveReq;
00404   if ( curReq == NULL && !requests.empty() )
00405     executeRequest();
00406 }
00407 
00408 typedef SegmentedColorGenerator::color_class_state color_class_state;
00409 
00410 void Lookout::storeVisionRegionDataTo(vector<Point>& data, const set<color_index>& colors, int minArea) {
00411   const unsigned char *img = 
00412     ProjectInterface::defRegionGenerator->getImage(ProjectInterface::fullLayer,0);
00413   const color_class_state *regions = reinterpret_cast<const color_class_state*> (img);
00414   for (set<color_index>::const_iterator it = colors.begin();
00415        it != colors.end(); it++)
00416     for (int i = 0; i < regions[*it].num; i++)
00417       if ((regions[*it].list+i)->area > minArea) {
00418   data.push_back(findLocationFor(regions[*it].list));
00419   cout << regions[*it].name  << " at " << data.back() << endl;
00420       }
00421       else break;
00422 }
00423   
00424 #ifdef TGT_HAS_IR_DISTANCE
00425 void Lookout::storeIRDataTo(vector<Point>& data) {
00426   NEWMAT::ColumnVector ray = Kinematics::pack(0,0,getIR());
00427   cout << "dist= " << ray(3) << ", in base frame= ";
00428 #ifdef TGT_ERS7
00429   NEWMAT::ColumnVector baseCoords = kine->jointToBase(NearIRFrameOffset)*ray;
00430 #else //not ERS7
00431   NEWMAT::ColumnVector baseCoords = kine->jointToBase(IRFrameOffset)*ray;
00432 #endif
00433   data.push_back(Point(baseCoords(1),baseCoords(2),baseCoords(3)));
00434   cout << data.back() << endl;
00435 }
00436 #endif
00437 
00438 
00439 //================ Scan Request ================
00440 
00441 void Lookout::processScanEvent(const EventBase& event) {
00442   //  cout << "Lookout::processScan: " << event.getName() << endl;
00443   static bool listeningObjEGID = false;
00444   const LookoutScanRequest* curScanReq = dynamic_cast<LookoutScanRequest*>(curReq);
00445 
00446   switch (event.getGeneratorID()) {
00447   case EventBase::motmanEGID:
00448     // head arrived at the start of motion sequence, add listeners and start scan sequence
00449     if (event.getSourceID() == pointer_id) {
00450       erouter->addTimer(this,settle_timer,curScanReq->motionSettleTime,false);
00451     }
00452     else if (event.getSourceID() == sequence_id) {
00453       motman->setPriority(sequence_id,MotionManager::kBackgroundPriority);
00454       requestComplete();
00455     }
00456     break;
00457 
00458   case EventBase::timerEGID: // time to take some measurements
00459     if ( event.getSourceID() == settle_timer )
00460       triggerScanMotionSequence();
00461     else if ( event.getSourceID() >= scan_timer && event.getSourceID()-scan_timer < curScanReq->tasks.size() ) {
00462       LookoutRequest::Task* task = curScanReq->tasks[event.getSourceID()-scan_timer];
00463       if (task->getTaskType() == LookoutRequest::Task::visRegTask) {
00464   LookoutRequest::VisionRegionTask* vrt = dynamic_cast<LookoutRequest::VisionRegionTask*>(task);
00465   storeVisionRegionDataTo(vrt->data,vrt->index,vrt->minArea);
00466       }
00467       else if (task->getTaskType() == LookoutRequest::Task::visObjTask) {
00468   erouter->addListener(this, EventBase::visSegmentEGID,
00469            ProjectInterface::visSegmentSID,EventBase::statusETID);
00470   listeningObjEGID = true;
00471       }
00472 #ifdef TGT_HAS_IR_DISTANCE
00473       else if (task->getTaskType() == LookoutRequest::Task::irTask) 
00474   storeIRDataTo(task->data);
00475 #endif
00476     }
00477     break;
00478 
00479   case EventBase::visSegmentEGID:
00480     if (listeningObjEGID)
00481       listeningObjEGID = false;
00482     else
00483       erouter->removeListener(this, EventBase::visSegmentEGID);
00484     break;
00485 
00486   case EventBase::visObjEGID:
00487     if (listeningObjEGID) {
00488       const VisionObjectEvent &voe = static_cast<const VisionObjectEvent&>(event);    
00489       for (vector<LookoutRequest::Task*>::const_iterator it = curScanReq->tasks.begin();
00490      it != curScanReq->tasks.end(); it++)
00491   if ((*it)->getTaskType() == LookoutRequest::Task::visObjTask) {
00492     LookoutRequest::VisionTask& vTask = *dynamic_cast<LookoutRequest::VisionTask*>(*it);
00493     if (vTask.index.find(event.getSourceID()) != vTask.index.end()) {
00494       vTask.data.push_back(findLocationFor(voe));
00495       cout << "VisionObject at " << vTask.data.back() << endl;
00496       break;
00497     }
00498   }
00499     }
00500     break;
00501   default:
00502     cout << "Lookout::processScan: unknown event " << event.getName() << endl;
00503     break;
00504   };
00505 }
00506 
00507 void Lookout::setupScan() {
00508   const LookoutScanRequest *curScan = dynamic_cast<const LookoutScanRequest*>(curReq);
00509   cout << "scan speed: " << curScan->scanSpeed 
00510        << "  (rad / millisec)\n";
00511   if ( !curScan->searchArea.isValid() ) {
00512     cout << "Invalid search area in LookoutScanRequest" << endl;
00513     return;
00514   }
00515   switch ( curScan->searchArea->getType() ) {
00516   case lineDataType: {
00517     const Shape<LineData> &line = ShapeRootTypeConst(curScan->searchArea,LineData);
00518     scanAlongLine(line->firstPt(), line->secondPt());
00519     break;
00520   }
00521   case polygonDataType:{
00522     const Shape<PolygonData> &poly = ShapeRootTypeConst(curScan->searchArea,PolygonData);
00523     scanAlongPolygon(poly->getVertices());
00524     break;
00525   }
00526   default:
00527     cout << "Invalid shape type for LookoutRequest searchArea: must be line or polygon" << endl;
00528   }
00529 }
00530 
00531 void Lookout::triggerScanMotionSequence() {
00532   const LookoutScanRequest* curScanReq = dynamic_cast<LookoutScanRequest*>(curReq);
00533   for (unsigned int i = 0; i < curScanReq->tasks.size(); i++) {
00534     const LookoutRequest::Task* task = curScanReq->tasks[i];
00535     if (task->getTaskType() == LookoutRequest::Task::visObjTask) {
00536       const LookoutRequest::VisionTask& vTask = *dynamic_cast<const LookoutRequest::VisionTask*>(task);
00537       for(set<color_index>::const_iterator color_it = vTask.index.begin();
00538     color_it != vTask.index.end(); color_it++)
00539   erouter->addListener(this,EventBase::visObjEGID, *color_it);
00540     }
00541     erouter->addTimer(this,scan_timer+i,0,false); // for initial measurements
00542     int snap_interval = (int)((float)task->dTheta / (float)curScanReq->scanSpeed);
00543     cout << "Lookout scan snap_interval = " << snap_interval << endl;
00544     erouter->addTimer(this, scan_timer+i, snap_interval, true);
00545   }
00546   motman->setPriority(pointer_id,MotionManager::kIgnoredPriority);
00547   motman->setPriority(posture_id,MotionManager::kIgnoredPriority);
00548   motman->setPriority(sequence_id,MotionManager::kStdPriority);
00549   MMAccessor<MediumMotionSequenceMC>(sequence_id)->play();
00550 }
00551 
00552 void Lookout::scanAlongLine(const Point& startPt, const Point& endPt) {
00553   motman->setPriority(pointer_id,MotionManager::kIgnoredPriority); // just to be safe
00554   MMAccessor<HeadPointerMC> pointer_acc(pointer_id);
00555 
00556   // first compute joint angles for final point
00557   pointer_acc->lookAtPoint(endPt.coordX(),endPt.coordY(),endPt.coordZ());
00558   std::vector<float> anglesA(NumHeadJoints);
00559   for(unsigned int i=0; i<NumHeadJoints; ++i)
00560     anglesA[i]=pointer_acc->getJointValue(i);
00561   
00562   // now compute angles for initial point, so when the motion command is executed we'll start there
00563   pointer_acc->lookAtPoint(startPt.coordX(),startPt.coordY(),startPt.coordZ());
00564   std::vector<float> anglesB(NumHeadJoints);
00565   for(unsigned int i=0; i<NumHeadJoints; ++i)
00566     anglesB[i]=pointer_acc->getJointValue(i);
00567   
00568   float total_joint_distance=0;
00569   for(unsigned int i=0; i<NumHeadJoints; ++i) {
00570     float diff = anglesA[i] - anglesB[i];
00571     total_joint_distance += diff*diff;
00572   }
00573   const unsigned int movement_time = (unsigned int)(sqrt(total_joint_distance) / dynamic_cast<const LookoutScanRequest*>(curReq)->scanSpeed);
00574   
00575   // begin at start point (the HeadPointerMC will have brought us there) and move smoothly to end point
00576   motman->setPriority(sequence_id,MotionManager::kIgnoredPriority); // just to be safe
00577   MMAccessor<MediumMotionSequenceMC> mseq_acc(sequence_id);
00578   mseq_acc->clear();
00579   mseq_acc->pause();
00580 #ifdef TGT_HAS_HEAD
00581   for(unsigned int i=0; i<NumHeadJoints; ++i)
00582     mseq_acc->setOutputCmd(HeadOffset+i,anglesA[i]);
00583 #endif
00584   mseq_acc->advanceTime(movement_time);
00585 #ifdef TGT_HAS_HEAD
00586   for(unsigned int i=0; i<NumHeadJoints; ++i)
00587     mseq_acc->setOutputCmd(HeadOffset+i,anglesB[i]);
00588 #endif
00589   motman->setPriority(posture_id,MotionManager::kIgnoredPriority); // just to be safe
00590   motman->setPriority(pointer_id,MotionManager::kStdPriority); // start head moving to where scan begins; completion will trigger the scan
00591 }
00592 
00593 void Lookout::scanAlongPolygon(vector<Point> const &vertices, const bool closed) {
00594   if ( vertices.size() == 0 )
00595     requestComplete();
00596   vector<Point> vertices_copy(vertices);
00597   const Point startPt = vertices_copy[0];
00598   if ( closed )
00599     vertices_copy.push_back(startPt);
00600   motman->setPriority(pointer_id,MotionManager::kBackgroundPriority); // just to be safe
00601   motman->setPriority(sequence_id,MotionManager::kBackgroundPriority); // just to be safe
00602   MMAccessor<HeadPointerMC> pointer_acc(pointer_id);
00603   MMAccessor<MediumMotionSequenceMC> mseq_acc(sequence_id);
00604   mseq_acc->pause();
00605   mseq_acc->clear();
00606   float const speed = dynamic_cast<const LookoutScanRequest*>(curReq)->scanSpeed;
00607   std::vector<float> anglesA(NumHeadJoints,0), anglesB(NumHeadJoints,0);
00608   for ( vector<Point>::const_iterator it = vertices_copy.begin(); it != vertices_copy.end(); ++it ) {
00609     pointer_acc->lookAtPoint((*it).coordX(),(*it).coordY(),(*it).coordZ());
00610     for(unsigned int i=0; i<NumHeadJoints; ++i)
00611       anglesB[i]=pointer_acc->getJointValue(i);
00612     if ( it != vertices_copy.begin() ) {
00613       float total_joint_distance=0;
00614       for(unsigned int i=0; i<NumHeadJoints; ++i) {
00615         float diff = anglesA[i] - anglesB[i];
00616         total_joint_distance += diff*diff;
00617       }
00618       const unsigned int movement_time = (unsigned int)(sqrt(total_joint_distance) /speed + 200);
00619       // cout << "Movement time " << movement_time << "msec to " << *it << endl;
00620       mseq_acc->advanceTime(movement_time);
00621     }
00622 #ifdef TGT_HAS_HEAD
00623     for(unsigned int i=0; i<NumHeadJoints; ++i)
00624       mseq_acc->setOutputCmd(HeadOffset+i,anglesB[i]);
00625     mseq_acc->advanceTime(200);
00626     for(unsigned int i=0; i<NumHeadJoints; ++i)
00627       mseq_acc->setOutputCmd(HeadOffset+i,anglesB[i]);
00628 #endif
00629     anglesA.swap(anglesB);
00630   }
00631   
00632   pointer_acc->lookAtPoint(startPt.coordX(),startPt.coordY(),startPt.coordZ());
00633   motman->setPriority(pointer_id,MotionManager::kStdPriority); // start head moving to where scan begins; completion will trigger the scan
00634 }
00635   
00636 //================ Track Requests ================
00637 
00638 void Lookout::setupTrack() {
00639   // point the head at the object, then start tracking it and posting status events
00640   LookoutTrackRequest *curTR = static_cast<const LookoutTrackRequest*>(curReq);
00641   const ShapeRoot &target = curTR->targetShape;
00642   curTR->cindex = ProjectInterface::getColorIndex(target->getColor());
00643   Point egoLoc = target->getCentroid();
00644   if ( target->getRefFrameType() == camcentric ) {
00645 #ifndef TGT_HAS_CAMERA
00646     std::cerr << "Lookout::setupTrack target has camcentric reference frame, but target model doesn't have a camera" << std::endl;
00647 #else
00648     const NEWMAT::Matrix camToBase = kine->jointToBase(CameraFrameOffset);
00649     egoLoc.projectToGround(camToBase,kine->calculateGroundPlane()); // assume head hasn't moved, or we're screwed
00650 #endif
00651   }
00652   trackerState = moveToAcquire;
00653   erouter->addListener(this, EventBase::visRegionEGID, ProjectInterface::visRegionSID, EventBase::deactivateETID);
00654   motman->setPriority(posture_id,MotionManager::kIgnoredPriority);
00655   motman->setPriority(sequence_id,MotionManager::kIgnoredPriority);
00656   motman->setPriority(pointer_id,MotionManager::kStdPriority);
00657   successSave = MMAccessor<HeadPointerMC>(pointer_id)->lookAtPoint(egoLoc.coordX(), egoLoc.coordY(), egoLoc.coordZ());
00658 }
00659 
00660 void Lookout::processTrackEvent(const EventBase &event) {
00661   const LookoutTrackRequest *curTR = static_cast<const LookoutTrackRequest*>(curReq);
00662   switch ( event.getGeneratorID() ) {
00663 
00664   case EventBase::visRegionEGID: {
00665     if ( trackerState == moveToAcquire ) return; // ignore vision events while slewing head to initial position
00666     const color_class_state *ccs = reinterpret_cast<const CMVision::color_class_state*>
00667       (ProjectInterface::defRegionGenerator->getImage(ProjectInterface::fullLayer,0));
00668     const color_class_state &col = ccs[curTR->cindex];
00669     if ( col.list == NULL || col.list->area <= curTR->minBlobArea ) {
00670       if ( trackerState != lost ) {
00671   trackerState = lost;
00672   erouter->addTimer(this,lost_timer,2000,false);  // wait for object to reappear
00673       }
00674       return;
00675     }
00676     // test succeeded
00677     erouter->removeTimer(this,lost_timer);
00678     trackerState = tracking;
00679     Point target = findLocationFor(col.list);
00680     MMAccessor<HeadPointerMC>(pointer_id)->lookAtPoint(target.coordX(),target.coordY(),target.coordZ());
00681   }
00682     break;
00683 
00684 
00685   case EventBase::motmanEGID:
00686     switch ( trackerState ) {
00687     case inactive:
00688       break;
00689     case moveToAcquire:
00690       trackerState = tracking;
00691       break;
00692     case tracking:
00693     case searching:
00694     case centering:
00695     case lost:
00696       break;
00697     }
00698     break;
00699     
00700   case EventBase::timerEGID:
00701     if ( event.getSourceID() == lost_timer )   // object out of sight for too long: give up? Or search?
00702       stopTrack();
00703     break;
00704 
00705   default:
00706     break;
00707   }
00708 }
00709 
00710 void Lookout::stopTrack() {
00711   if ( curReq != NULL && curReq->getHeadMotionType() == LookoutRequest::track )
00712     requestComplete(false);
00713 }
00714 
00715 Point Lookout::findLocationFor(const float normX, const float normY) {
00716   // NEWMAT::ColumnVector ground_plane = kine->calculateGroundPlane();
00717   NEWMAT::ColumnVector cameraPt(4);
00718   config->vision.computeRay(normX, normY, cameraPt(1),cameraPt(2),cameraPt(3));
00719 #ifdef TGT_HAS_IR_DISTANCE
00720   cameraPt *= getIR();
00721 #else
00722   cameraPt *= 400;
00723 #endif
00724   cameraPt(4) = 1;
00725   // groundPt = kine->projectToPlane(CameraFrameOffset, cameraPt, 
00726   //          BaseFrameOffset, ground_plane, BaseFrameOffset);
00727 #ifndef TGT_HAS_CAMERA
00728   const NEWMAT::ColumnVector groundPt = cameraPt;
00729 #else
00730   const NEWMAT::Matrix camToBase(kine->jointToBase(CameraFrameOffset));
00731   const NEWMAT::ColumnVector groundPt = camToBase * cameraPt;
00732 #endif
00733   return Point(groundPt(1),groundPt(2),groundPt(3),egocentric);
00734 }
00735 
00736   /*
00737 void Lookout::processTrackVision(float normX, float normY, bool isCurrentlyVisible) {
00738   if (!isCurrentlyVisible && landmarkInView) { // landmark just lost
00739     cout << "landmark just lost" << endl;
00740     erouter->addTimer(this,start_pan,500,false); // wait 0.5 sec before starting to look for landmark
00741     erouter->removeTimer(this,reset_pan);
00742   }
00743   else if (!landmarkInView && isCurrentlyVisible) { // landmark just found
00744     cout << "found landmark" << endl;
00745     erouter->removeTimer(this,start_pan);
00746     erouter->addTimer(this,reset_pan,1000,false);
00747   }
00748   else if (isCurrentlyVisible) { // continue tracking landmark
00749     trackObjectAt(normX,normY);
00750   }
00751   landmarkInView = isCurrentlyVisible;
00752   lm_location = findLocationFor(normX,normY);
00753   erouter->postEvent(EventBase::lookoutEGID, curReq->getRequestID(), EventBase::statusETID,0);
00754 }
00755 
00756 void Lookout::processTrackEvent(const EventBase& event) {
00757   switch (event.getGeneratorID()) {
00758   case EventBase::visObjEGID:
00759     if (event.getTypeID()==EventBase::statusETID) {
00760       const VisionObjectEvent *voe = (static_cast<const VisionObjectEvent*>(&event));
00761       const float area = voe->getBoundaryArea();
00762       processTrackVision(voe->getCenterX(), voe->getCenterY(), area > 0.03);
00763     }
00764     break;
00765   case EventBase::visSegmentEGID:
00766     if (event.getTypeID() == EventBase::statusETID) {
00767       vector<Point> pts = getRegionData();
00768       if (pts.empty()) processTrackVision(0,0,false);
00769       else processTrackVision(pts.front().coordX(),pts.front().coordY(),true);
00770     }
00771     break;
00772   case EventBase::timerEGID:
00773     switch (event.getSourceID()) {
00774     case start_pan: //! enough time passed after landmark lost
00775       cout << "landmark not seen for 0.5 sec\n";
00776       if (curReq->isSticky())
00777   lookForLandmark();
00778       else
00779   requestComplete();
00780       break;
00781     case reset_pan:
00782       cout << "reset pan motion" << endl;
00783       { MMAccessor<SmallMotionSequenceMC> (sequence_id)->setTime(0); }
00784       break;
00785     default:
00786       cout << "unknown source timer evenet" << endl;
00787       break;
00788     }
00789   case EventBase::motmanEGID:
00790     if (event.getSourceID() == sequence_id && curReq->isSticky()) { // lookForLandmark() failed
00791       cout << "Lookout::processTrack: track failed\n";
00792       requestComplete();
00793     }
00794     break;
00795   default:
00796     cout << "Lookout::processTrack: unknown event" << endl;
00797     break;
00798   };
00799 }
00800   */
00801 
00802 
00803   /*
00804 void Lookout::trackObjectAt(float horiz, float vert) {
00805   setPanPrior(false);
00806 //  findLandmarkLocation(voe);
00807 //  erouter->postEvent(EventBase::lookoutEGID, curReq->getRequestID(), EventBase::statusETID,0);
00808   double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/7.5;
00809   double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/6;
00810   if(tilt<mathutils::deg2rad(-20.0))
00811     tilt=mathutils::deg2rad(-20.0);
00812   if(tilt>mathutils::deg2rad(40.0))
00813     tilt=mathutils::deg2rad(40.0);
00814   if(pan>mathutils::deg2rad(80.0))
00815     pan=mathutils::deg2rad(80.0);
00816   if(pan<mathutils::deg2rad(-80.0))
00817     pan=mathutils::deg2rad(-80.0);
00818 
00819 #ifdef TGT_ERS7
00820   //  cout << "tilt: " << state->outputs[HeadOffset+TiltOffset] << ", nod: " << state->outputs[HeadOffset+NodOffset] << endl;
00821   if (tilt < -0.5)
00822     MMAccessor<HeadPointerMC> (pointer_id)->setJoints(tilt,pan,outputRanges[HeadOffset+NodOffset][MinRange]);
00823   else {
00824     const float act_tilt = state->outputs[HeadOffset+TiltOffset];
00825     const float nod_fact = act_tilt*act_tilt*4.0;
00826     MMAccessor<HeadPointerMC> (pointer_id)->setJoints(tilt,pan,outputRanges[HeadOffset+NodOffset][MinRange]*nod_fact);
00827   }
00828 #else
00829   MMAccessor<HeadPointerMC> (pointer_id)->setJoints(tilt,pan,0);
00830 #endif
00831 }
00832   */
00833 
00834 // ================ Search Requests ================
00835 
00836 /* Move head to the target shape.  If we encounter the desired color
00837 region along the way, stop moving.  Otherwise, upon reaching the
00838 target, begin a spiral search sequence.  If the sequence completes
00839 without finding the color region, return failure.  Else find the
00840 center of the color region, move the head there, wait for the camera
00841 to settle, take a picture, and return success. */
00842 
00843 void Lookout::setupSearch() {
00844   // point the head at expected object location, then start a spiral search
00845   LookoutSearchRequest *curSR = static_cast<const LookoutSearchRequest*>(curReq);
00846   const ShapeRoot &target = curSR->targetShape;
00847   curSR->cindex = ProjectInterface::getColorIndex(target->getColor());
00848   Point egoLoc = target->getCentroid();
00849   if ( target->getRefFrameType() == allocentric ) {
00850     egoLoc.applyTransform(VRmixin::mapBuilder.worldToLocalTranslateMatrix,egocentric);
00851     egoLoc.applyTransform(VRmixin::mapBuilder.worldToLocalRotateMatrix,egocentric);
00852   }
00853   cout << "Lookout: search target estimated at " << egoLoc << ", color=" << curSR->cindex << endl;
00854   erouter->addListener(this, EventBase::visRegionEGID,ProjectInterface::visRegionSID,EventBase::deactivateETID);
00855   MMAccessor<HeadPointerMC>(pointer_id)->lookAtPoint(egoLoc.coordX(), egoLoc.coordY(), egoLoc.coordZ());
00856   motman->setPriority(pointer_id,MotionManager::kStdPriority);
00857   trackerState = moveToAcquire;
00858 }
00859 
00860 void Lookout::processSearchEvent(const EventBase &event) {
00861   //  cout << "Lookout::processSearchEvent: " << event.getDescription() << endl;
00862   const LookoutSearchRequest *curSR = static_cast<const LookoutSearchRequest*>(curReq);
00863   switch ( event.getGeneratorID() ) {
00864 
00865   case EventBase::motmanEGID:
00866     if ( event.getSourceID() == pointer_id || event.getSourceID() == sequence_id )
00867       erouter->addTimer(this, settle_timer, 500, false);
00868     else
00869       return;
00870     break;
00871 
00872   case EventBase::visRegionEGID: {
00873     const color_class_state *ccs = reinterpret_cast<const CMVision::color_class_state*>
00874       (ProjectInterface::defRegionGenerator->getImage(ProjectInterface::fullLayer,0));
00875     const color_class_state &col = ccs[curSR->cindex];
00876     // insert test to see if desired object has been seen:
00877     if ( col.list != NULL && col.list->area >= curSR->minBlobArea ) 
00878       erouter->removeListener(this,EventBase::visRegionEGID);
00879     else
00880       return;
00881     // test succeeded
00882     const Point center = findLocationFor(col.list);
00883     cout << "Lookout search: center = " << center << "   area = " << col.list->area << "  color = " << curSR->cindex << endl;
00884     // MMAccessor<HeadPointerMC>(pointer_id)->lookAtPoint(center.coordX(),center.coordY(),center.coordZ());
00885     // motman->setPriority(pointer_id,MotionManager::kStdPriority);
00886     motman->setPriority(pointer_id,MotionManager::kIgnoredPriority);
00887     motman->setPriority(posture_id,MotionManager::kIgnoredPriority);
00888     motman->setPriority(sequence_id,MotionManager::kIgnoredPriority);
00889     trackerState = centering;
00890     erouter->addTimer(this, settle_timer, 500, false);
00891     break;
00892   }
00893 
00894   case EventBase::timerEGID:
00895     switch ( trackerState ) {
00896     case moveToAcquire:  // camera has settled, so start the search
00897       triggerSearchMotionSequence();
00898       break;
00899     case searching:
00900     case centering:      // camera has settled, so grab a picture and return
00901       requestComplete();
00902       break;
00903     default:
00904       break;
00905     }
00906 
00907   default:
00908     break;
00909   }
00910 }
00911 
00912 void Lookout::triggerSearchMotionSequence() {
00913   cout << "Lookout: beginning search motion sequence" << endl;
00914   const LookoutSearchRequest *curSR = static_cast<const LookoutSearchRequest*>(curReq);
00915   const ShapeRoot &target = curSR->targetShape;
00916   Point egoLoc = target->getCentroid();
00917   if ( target->getRefFrameType() == allocentric ) {
00918     egoLoc.applyTransform(VRmixin::mapBuilder.worldToLocalTranslateMatrix,egocentric);
00919     egoLoc.applyTransform(VRmixin::mapBuilder.worldToLocalRotateMatrix,egocentric);
00920   }
00921   int const nspirals = 5;
00922   float const xstep=50, ystep=100;
00923   HeadPointerMC hpmc_temp;
00924   std::vector<float> jointvals(0);
00925   jointvals.reserve(nspirals*4*NumHeadJoints);
00926   for (int i=1; i<=nspirals; i++) {
00927     searchAt(hpmc_temp,jointvals,egoLoc+Point(-i*xstep,        0));
00928     searchAt(hpmc_temp,jointvals,egoLoc+Point(      0,   i*ystep));
00929     searchAt(hpmc_temp,jointvals,egoLoc+Point( i*xstep,        0));
00930     searchAt(hpmc_temp,jointvals,egoLoc+Point(       0, -i*ystep));
00931   }
00932   // Calculate all the joint positions first (above), then lock the
00933   // motionsequence command and quickly copy them over.
00934   MMAccessor<MediumMotionSequenceMC> mseq_acc(sequence_id);
00935   mseq_acc->clear();
00936 #ifdef TGT_HAS_HEAD
00937   for ( std::vector<float>::const_iterator it = jointvals.begin(); it != jointvals.end(); ) {
00938     mseq_acc->advanceTime(800);
00939     for(unsigned int i=0; i<NumHeadJoints; ++i)
00940       mseq_acc->setOutputCmd(HeadOffset+i,*(it++));
00941   }
00942   mseq_acc->advanceTime(800);
00943 #endif
00944   mseq_acc->play();
00945   motman->setPriority(sequence_id,MotionManager::kStdPriority);
00946   trackerState = searching;
00947 }
00948 
00949 void Lookout::searchAt(HeadPointerMC &hpmc_temp,
00950            std::vector<float> &jointvals,
00951            const Point &target) {
00952   hpmc_temp.lookAtPoint(target.coordX() ,target.coordY() ,target.coordZ());
00953   for(unsigned int i=0; i<NumHeadJoints; ++i)
00954     jointvals.push_back(hpmc_temp.getJointValue(i));
00955 }
00956 
00957 } // namespace

DualCoding 4.0
Generated Thu Nov 22 00:52:36 2007 by Doxygen 1.5.4