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

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:44 2016 by Doxygen 1.6.3