Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

Lookout.cc Go to the documentation of this file. //-*-c++-*- #include "Events/EventRouter.h" #include "Events/LocomotionEvent.h" #include "Events/LookoutEvents.h" #include "Events/VisionObjectEvent.h" #include "Motion/HeadPointerMC.h" #include "Motion/PostureMC.h" #include "Motion/MMAccessor.h" #include "Motion/MotionSequenceMC.h" #include "Motion/MotionManager.h" #include "Shared/ProjectInterface.h" #include "Shared/WorldState.h" #include "Vision/RegionGenerator.h" #include "VRmixin.h" #include "LookoutRequests.h" #include "Lookout.h" namespace DualCoding { // some stuff for convenience typedef SegmentedColorGenerator::color_class_state color_class_state; inline float getIR() { #ifdef TGT_ERS7 return state->sensors[NearIRDistOffset]; #else return state->sensors[IRDistOffset]; #endif } Lookout::Lookout() : BehaviorBase("Lookout"), pointer_id(MotionManager::invalid_MC_ID), sequence_id(MotionManager::invalid_MC_ID), requests(), curReq(NULL), landmarkInView(true), idCounter(0) {} void Lookout::DoStart() { BehaviorBase::DoStart(); } void Lookout::DoStop() { curReq = NULL; while (!requests.empty()) { if (requests.front() != NULL) delete requests.front(); requests.pop(); } motman->removeMotion(pointer_id); pointer_id = MotionManager::invalid_MC_ID; motman->removeMotion(sequence_id); sequence_id = MotionManager::invalid_MC_ID; BehaviorBase::DoStop(); } /* vector<Point> Lookout::getVisionObjectData() { } vector<Point> Lookout::getIRData() { } */ void Lookout::processScan(const EventBase& event) { // cout << "Lookout::processScan: " << event.getName() << endl; static bool listeningObjEGID = false; static ScanRequest* curScanReq = dynamic_cast<ScanRequest*>(curReq); if (curScanReq != curReq) curScanReq = dynamic_cast<ScanRequest*>(curReq); switch (event.getGeneratorID()) { case EventBase::motmanEGID: if (event.getTypeID() != EventBase::deactivateETID) return; // head arrived at the start of motion sequence, add listeners and start scan sequence if (event.getSourceID() == pointer_id) { pointer_id = MotionManager::invalid_MC_ID; motman->setPriority(sequence_id,MotionManager::kStdPriority); MMAccessor<SmallMotionSequenceMC>(sequence_id)->play(); for (unsigned int i = 0; i < curScanReq->tasks.size(); i++) { const ScanRequest::Task* task = curScanReq->tasks[i]; if (task->getTaskType() == ScanRequest::Task::visObj) { const ScanRequest::VisionTask& vTask = *dynamic_cast<const ScanRequest::VisionTask*>(task); for(set<int>::const_iterator color_it = vTask.index.begin(); color_it != vTask.index.end(); color_it++) erouter->addListener(this,EventBase::visObjEGID, *color_it); } erouter->addTimer(this,scan_timer+i,0,false); // for initial measurements erouter->addTimer(this,scan_timer+i, (int) ((float) task->interval/(float)curScanReq->scanSpeed),true); } } else if (event.getSourceID() == sequence_id) { sequence_id = MotionManager::invalid_MC_ID; requestComplete(); } break; case EventBase::timerEGID: // time to take some measurements if (event.getSourceID()-scan_timer < curScanReq->tasks.size()) { ScanRequest::Task* task = curScanReq->tasks[event.getSourceID()-scan_timer]; if (task->getTaskType() == ScanRequest::Task::visReg) { ScanRequest::VisionRegionTask* vrt = dynamic_cast<ScanRequest::VisionRegionTask*>(task); storeVisionRegionDataTo(vrt->data,vrt->index,vrt->minArea); } else if (task->getTaskType() == ScanRequest::Task::visObj) { erouter->addListener(this, EventBase::visSegmentEGID, ProjectInterface::visSegmentSID,EventBase::statusETID); listeningObjEGID = true; } else if (task->getTaskType() == ScanRequest::Task::ir) storeIRDataTo(task->data); } break; case EventBase::visSegmentEGID: if (listeningObjEGID) listeningObjEGID = false; else erouter->removeListener(this, EventBase::visSegmentEGID); break; case EventBase::visObjEGID: if (listeningObjEGID) { const VisionObjectEvent voe = *static_cast<const VisionObjectEvent*>(&event); for (vector<ScanRequest::Task*>::iterator it = curScanReq->tasks.begin(); it != curScanReq->tasks.end(); it++) if ((*it)->getTaskType() == ScanRequest::Task::visObj) { ScanRequest::VisionTask& vTask = *dynamic_cast<ScanRequest::VisionTask*>(*it); if (vTask.index.find(event.getSourceID()) != vTask.index.end()) { vTask.data.push_back(findLocationFor(&voe)); cout << "VisionObject at " << vTask.data.back() << endl; break; } } } break; default: cout << "Lookout::processScan: unknown event " << event.getName() << endl; break; }; } void Lookout::storeVisionRegionDataTo(vector<Point>& data, const set<int>& colors, int minArea) { const unsigned char *img = ProjectInterface::defRegionGenerator->getImage(ProjectInterface::fullLayer,0); const color_class_state *regions = reinterpret_cast<const color_class_state*> (img); for (set<int>::const_iterator it = colors.begin(); it != colors.end(); it++) for (int i = 0; i < regions[*it].num; i++) if ((regions[*it].list+i)->area > minArea) { data.push_back(findLocationFor(regions[*it].list)); cout << regions[*it].name << " at " << data.back() << endl; } else break; } void Lookout::storeIRDataTo(vector<Point>& data) { NEWMAT::ColumnVector ray = Kinematics::pack(0,0,getIR()); cout << "dist= " << ray(3) << ", in base frame= "; #ifdef TGT_ERS7 NEWMAT::ColumnVector baseCoords = kine->jointToBase(NearIRFrameOffset)*ray; #else //not ERS7 NEWMAT::ColumnVector baseCoords = kine->jointToBase(IRFrameOffset)*ray; #endif data.push_back(Point(baseCoords(1),baseCoords(2),baseCoords(3))); cout << data.back() << endl; } bool Lookout::findPixelModes(StoreModeImageRequest& modeRequest) { static size_t npixels = modeRequest.image->getNumPixels(); static vector<map<uchar, unsigned int> > colorCount(npixels); if (modeRequest.numImages-- == 0) { // finished collecting samples, find modes for (size_t i = 0; i<npixels; i++) { unsigned int maxCount = 0; uchar maxChar = 0; for (map<uchar, unsigned int>::const_iterator it = colorCount[i].begin(); it != colorCount[i].end(); it++) if (it->second > maxCount) { maxCount = it->second; maxChar = it->first; } modeRequest.image[i] = maxChar; colorCount[i].clear(); } return true; } else { for (size_t i = 0; i<npixels; i++) colorCount[i][modeRequest.image[i]]++; return false; } } void Lookout::processPointAt(const EventBase& event) { // cout << "Lookout::processPointAt: " << event.getName() << endl; switch (event.getGeneratorID()) { case EventBase::motmanEGID: // head motion complete, wait for duration if (event.getSourceID() == pointer_id && event.getTypeID() == EventBase::deactivateETID) { pointer_id = MotionManager::invalid_MC_ID; erouter->addTimer(this, dur_timer, dynamic_cast<const PointAtBase*>(curReq)->duration, false); } break; case EventBase::timerEGID: if (event.getSourceID() == dur_timer) { PointAtBase* baseReq = dynamic_cast<PointAtBase*>(curReq); switch (baseReq->getPointAtType()) { case PointAtBase::storeImage: erouter->addListener(this, EventBase::visSegmentEGID, ProjectInterface::visSegmentSID,EventBase::statusETID); break; case PointAtBase::measureDist: erouter->addListener(this, EventBase::sensorEGID, SensorSourceID::UpdatedSID); break; default: requestComplete(); break; }; break; } case EventBase::visSegmentEGID: { erouter->removeListener(this, EventBase::visSegmentEGID); PointAtBase* baseReq = dynamic_cast<PointAtBase*>(curReq); baseReq->toBaseMatrix = kine->jointToBase(baseReq->joint); VRmixin::camSkS.clear(); StoreImageRequest& storeImageReq = *dynamic_cast<StoreImageRequest*>(curReq); storeImageReq.image = (*storeImageReq.sketchFunc)(); if (StoreModeImageRequest* modeReq = dynamic_cast<StoreModeImageRequest*>(curReq)) if ( ! findPixelModes(*modeReq) ) { erouter->addTimer(this, dur_timer, modeReq->interval, false); return; } requestComplete(); } break; case EventBase::sensorEGID: { MeasureDistanceRequest* measureDistReq = dynamic_cast<MeasureDistanceRequest*>(curReq); measureDistReq->toBaseMatrix = kine->jointToBase(measureDistReq->joint); measureDistReq->val = getIR(); requestComplete(); } break; default: cout << "Lookout::processPointAt: unknown event " << event.getName() << endl; break; }; } /* void Lookout::processTrackVision(float normX, float normY, bool isCurrentlyVisible) { if (!isCurrentlyVisible && landmarkInView) { // landmark just lost cout << "landmark just lost" << endl; erouter->addTimer(this,start_pan,500,false); // wait 0.5 sec before starting to look for landmark erouter->removeTimer(this,reset_pan); } else if (!landmarkInView && isCurrentlyVisible) { // landmark just found cout << "found landmark" << endl; erouter->removeTimer(this,start_pan); erouter->addTimer(this,reset_pan,1000,false); } else if (isCurrentlyVisible) { // continue tracking landmark trackObjectAt(normX,normY); } landmarkInView = isCurrentlyVisible; lm_location = findLocationFor(normX,normY); erouter->postEvent(EventBase::lookoutEGID, curReq->getRequestID(), EventBase::statusETID,0); } void Lookout::processTrack(const EventBase& event) { switch (event.getGeneratorID()) { case EventBase::visObjEGID: if (event.getTypeID()==EventBase::statusETID) { const VisionObjectEvent *voe = (static_cast<const VisionObjectEvent*>(&event)); const float area = voe->getBoundaryArea(); processTrackVision(voe->getCenterX(), voe->getCenterY(), area > 0.03); } break; case EventBase::visSegmentEGID: if (event.getTypeID() == EventBase::statusETID) { vector<Point> pts = getRegionData(); if (pts.empty()) processTrackVision(0,0,false); else processTrackVision(pts.front().coordX(),pts.front().coordY(),true); } break; case EventBase::timerEGID: switch (event.getSourceID()) { case start_pan: //! enough time passed after landmark lost cout << "landmark not seen for 0.5 sec\n"; if (curReq->isSticky()) lookForLandmark(); else requestComplete(); break; case reset_pan: cout << "reset pan motion" << endl; { MMAccessor<SmallMotionSequenceMC> (sequence_id)->setTime(0); } break; default: cout << "unknown source timer evenet" << endl; break; } case EventBase::motmanEGID: if (event.getSourceID() == sequence_id && curReq->isSticky()) { // lookForLandmark() failed cout << "Lookout::processTrack: track failed\n"; requestComplete(); } break; default: cout << "Lookout::processTrack: unknown event" << endl; break; }; } */ unsigned int Lookout::executeRequest(const LookoutRequest& req) { switch (req.getHeadMotionType()) { case LookoutRequest::none: switch (dynamic_cast<const PointAtBase*>(&req)->getPointAtType()) { case PointAtBase::storeImage: if (dynamic_cast<const StoreModeImageRequest*>(&req)) pushRequest<StoreModeImageRequest>(req); else pushRequest<StoreImageRequest>(req); break; case PointAtBase::measureDist: pushRequest<MeasureDistanceRequest>(req); break; default: cout << "Lookout::executeRequest: unknown PointAtRequest type\n"; break; }; break; case LookoutRequest::pointAt: switch (dynamic_cast<const PointAtRequest*>(&req)->getPointAtType()) { case PointAtBase::none: pushRequest<PointAtRequest>(req); break; case PointAtBase::storeImage: if (dynamic_cast<const StoreModeImageAtRequest*>(&req)) pushRequest<StoreModeImageAtRequest>(req); else pushRequest<StoreImageAtRequest>(req); break; case PointAtBase::measureDist: pushRequest<MeasureDistanceAtRequest>(req); break; default: cout << "Lookout::executeRequest: unknown PointAtRequest type\n"; break; }; break; case LookoutRequest::scan: if (dynamic_cast<const ScanRequest*>(&req)->getScanType() == ScanRequest::line) pushRequest<ScanAlongLineRequest>(req); else pushRequest<ScanAreaRequest>(req); break; case LookoutRequest::track: pushRequest<TrackRequest>(req); break; default: cout << "Lookout::executeRequest: unknown request type " << req.getHeadMotionType() << endl; }; requests.back()->requestID = idCounter++; // cout << " request " << requests.back()->requestID << " added to request queue\n"; if (requests.size() == 1) executeRequest(); return requests.back()->requestID; } // triggers action to start completing each type of front of request queue void Lookout::executeRequest() { curReq = requests.front(); // cout << "Executing Lookout Request #" << curReq->requestID << endl; switch (curReq->getHeadMotionType()) { case LookoutRequest::none: erouter->addTimer(this,dur_timer,dynamic_cast<const PointAtBase*>(curReq)->duration,false); break; case LookoutRequest::pointAt: { const Point& pt = dynamic_cast<const PointAtRequest*>(curReq)->gazePt; if (dynamic_cast<const PointAtBase*>(curReq)->getPointAtType() == PointAtBase::measureDist) { SharedObject<PostureMC> pstmc; #ifdef TGT_ERS7 pstmc->solveLinkVector(pt.coords,NearIRFrameOffset,Kinematics::pack(0,0,1)); #else pstmc->solveLinkVector(pt.coords,IRFrameOffset,Kinematics::pack(0,0,1)); #endif pointer_id = motman->addPrunableMotion(pstmc); } else { SharedObject<HeadPointerMC> hpmc; hpmc->lookAtPoint(pt.coordX(),pt.coordY(),pt.coordZ()); pointer_id = motman->addPrunableMotion(hpmc); } erouter->addListener(this,EventBase::motmanEGID,pointer_id); } break; case LookoutRequest::scan: erouter->addListener(this,EventBase::motmanEGID); scan(); break; default: cout << "Lookout::executeRequest(): unknown request " << curReq->getHeadMotionType() << endl; break; }; } void Lookout::requestComplete() { // remove all timer and listeners for now erouter->removeListener(this); erouter->removeTimer(this); if (curReq->getHeadMotionType() == LookoutRequest::scan) { erouter->postEvent(LookoutScanEvent (dynamic_cast<ScanRequest*>(curReq)->tasks, EventBase::lookoutEGID,curReq->requestID, EventBase::deactivateETID)); } else if (curReq->getHeadMotionType() == LookoutRequest::pointAt || curReq->getHeadMotionType() == LookoutRequest::none) { const NEWMAT::Matrix& toBase = dynamic_cast<const PointAtBase*>(curReq)->toBaseMatrix; switch (dynamic_cast<const PointAtBase*>(curReq)->getPointAtType()) { case PointAtRequest::none: erouter->postEvent(LookoutPointAtEvent(toBase,EventBase::lookoutEGID, curReq->requestID, EventBase::deactivateETID)); break; case PointAtRequest::storeImage: erouter->postEvent(LookoutSketchEvent (dynamic_cast<StoreImageRequest*>(curReq)->image, toBase,EventBase::lookoutEGID, curReq->requestID, EventBase::deactivateETID)); break; case PointAtRequest::measureDist: erouter->postEvent(LookoutIREvent (dynamic_cast<const MeasureDistanceRequest*>(curReq)->val, toBase,EventBase::lookoutEGID, curReq->requestID, EventBase::deactivateETID)); break; default: cout << "Lookout::requestComplete(): Unknown type returned by getPointAtType()\n"; }; } requests.pop(); // delete curReq; curReq = NULL; if (!requests.empty()) executeRequest(); } void Lookout::processEvent(const EventBase& event) { cout << "Lookout::processEvent got " << event.getDescription() << endl; if (NULL==curReq) { cout << "Should not get any event when executing no request\n"; return; } switch (curReq->getHeadMotionType()) { case LookoutRequest::scan: processScan(event); break; /* case LookoutRequest::track: processTrack(event); break; case LookoutRequest::localize: // ?????? break; */ case LookoutRequest::pointAt: case LookoutRequest::none: processPointAt(event); break; default: cout << "Lookout::processEvent: unknown request type: " << event.getName() << endl; break; }; } /* void Lookout::trackObjectAt(float horiz, float vert) { setPanPrior(false); // findLandmarkLocation(voe); // erouter->postEvent(EventBase::lookoutEGID, curReq->getRequestID(), EventBase::statusETID,0); double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/7.5; double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/6; if(tilt<mathutils::deg2rad(-20.0)) tilt=mathutils::deg2rad(-20.0); if(tilt>mathutils::deg2rad(40.0)) tilt=mathutils::deg2rad(40.0); if(pan>mathutils::deg2rad(80.0)) pan=mathutils::deg2rad(80.0); if(pan<mathutils::deg2rad(-80.0)) pan=mathutils::deg2rad(-80.0); #ifdef TGT_ERS7 // cout << "tilt: " << state->outputs[HeadOffset+TiltOffset] << ", nod: " << state->outputs[HeadOffset+NodOffset] << endl; if (tilt < -0.5) MMAccessor<HeadPointerMC> (pointer_id)->setJoints(tilt,pan,outputRanges[HeadOffset+NodOffset][MinRange]); else { const float act_tilt = state->outputs[HeadOffset+TiltOffset]; const float nod_fact = act_tilt*act_tilt*4.0; MMAccessor<HeadPointerMC> (pointer_id)->setJoints(tilt,pan,outputRanges[HeadOffset+NodOffset][MinRange]*nod_fact); } #else MMAccessor<HeadPointerMC> (pointer_id)->setJoints(tilt,pan,0); #endif } */ Point Lookout::findLocationFor(float normX, float normY) { NEWMAT::ColumnVector ground_plane = kine->calculateGroundPlane(); NEWMAT::ColumnVector cameraPt(4), groundPt(4); config->vision.computeRay(normX, normY, cameraPt(1),cameraPt(2),cameraPt(3)); cameraPt(4) = 1; groundPt = kine->projectToPlane(CameraFrameOffset, cameraPt, BaseFrameOffset, ground_plane, BaseFrameOffset); return Point(groundPt(1),groundPt(2),groundPt(3),egocentric); } void Lookout::scan() { cout << "scan speed: " << dynamic_cast<const ScanRequest*>(curReq)->scanSpeed << " (rad / millisec)\n"; if (dynamic_cast<const ScanRequest*>(curReq)->getScanType()==ScanRequest::line) { const ScanAlongLineRequest& scanLineReq = *dynamic_cast<const ScanAlongLineRequest*>(curReq); scanAlongLine(scanLineReq.beginPt, scanLineReq.endPt); } else { const ScanAreaRequest& scanLineReq = *dynamic_cast<const ScanAreaRequest*>(curReq); scanArea(scanLineReq.left, scanLineReq.right, scanLineReq.far, scanLineReq.near); } } void Lookout::scanAlongLine(const Point& startPt, const Point& endPt) { SharedObject<HeadPointerMC> hpmc; hpmc->lookAtPoint(endPt.coordX(),endPt.coordY(),endPt.coordZ()); const float pan1 = hpmc->getJointValue(PanOffset); const float tilt1 = hpmc->getJointValue(TiltOffset); const float roll1 = hpmc->getJointValue(RollOffset); hpmc->lookAtPoint(startPt.coordX(),startPt.coordY(),startPt.coordZ()); const float pan0 = hpmc->getJointValue(PanOffset); const float tilt0 = hpmc->getJointValue(TiltOffset); const float roll0 = hpmc->getJointValue(RollOffset); const unsigned int time = (unsigned int) (sqrt((pan1-pan0)*(pan1-pan0)+(tilt1-tilt0)*(tilt1-tilt0)+(roll1-roll0)*(roll1-roll0)) / dynamic_cast<const ScanRequest*>(curReq)->scanSpeed); SharedObject<SmallMotionSequenceMC> scanmc; scanmc->pause(); scanmc->setOutputCmd(HeadOffset+TiltOffset,tilt0); scanmc->setOutputCmd(HeadOffset+RollOffset,roll0); scanmc->setOutputCmd(HeadOffset+PanOffset,pan0); scanmc->advanceTime(time); scanmc->setOutputCmd(HeadOffset+TiltOffset,tilt1); scanmc->setOutputCmd(HeadOffset+RollOffset,roll1); scanmc->setOutputCmd(HeadOffset+PanOffset,pan1); pointer_id = motman->addPrunableMotion(hpmc); // moves head to where scan begins; // actual scan motion sequence (paused), played when pointer_id above is completed sequence_id = motman->addPrunableMotion(scanmc,MotionManager::kIgnoredPriority); } void Lookout::scanArea(coordinate_t left, coordinate_t right, coordinate_t far, coordinate_t near) { const float& scanSpeed = dynamic_cast<const ScanRequest*>(curReq)->scanSpeed; static const float tiltJointValDelta = mathutils::deg2rad(15.0); // const int tiltTime = (int) (tiltJointValDelta / scanSpeed); const float z_coord = -150;//1/wmap.ground_plane(3); unsigned short counter = 1; // or 3 if start from left SharedObject<HeadPointerMC> hpmc; //! < virtual headpointer, never gets added to motman near = (near > 100) ? near : 100; const Point firstPt(near, (counter < 2) ? right : left, z_coord); // start of motion sequence hpmc->lookAtPoint(firstPt.coordX(),firstPt.coordY(),firstPt.coordZ()); float tiltJointVal = hpmc->getJointValue(TiltOffset); // define scan waypoints in terms of joint angles instead of base frame coordinates (x,y,z) // this way you can make sure to cover entire region but not scanning the same region twice // would be harder to achieve this if I did this in terms of base frame coords hpmc->lookAtPoint(far,left,z_coord); const float tiltJointValFar = hpmc->getJointValue(TiltOffset); const float panJointValLeftFar = hpmc->getJointValue(PanOffset); hpmc->lookAtPoint(far,right,z_coord); const float panJointValRiteFar = hpmc->getJointValue(PanOffset); hpmc->lookAtPoint(near,left,z_coord); float panJointValLeft = hpmc->getJointValue(PanOffset); hpmc->lookAtPoint(near,right,z_coord); float panJointValRite = hpmc->getJointValue(PanOffset); const int panTime = (int) (fabs(panJointValLeft-panJointValRite)/scanSpeed); const float panJointLeftDelta = (tiltJointValFar-tiltJointVal < 0.01) ? 0 : (panJointValLeftFar-panJointValLeft)/(tiltJointValFar-tiltJointVal)*tiltJointValDelta; const float panJointRiteDelta = (tiltJointValFar-tiltJointVal < 0.01) ? 0 : (panJointValRiteFar-panJointValRite)/(tiltJointValFar-tiltJointVal)*tiltJointValDelta; SharedObject<SmallMotionSequenceMC> scanmc; scanmc->pause(); while (true) { counter++; counter = counter % 4; if (counter%2 == 1) { tiltJointVal += tiltJointValDelta; if (tiltJointVal > tiltJointValFar) break; if (counter == 1) panJointValRite += panJointRiteDelta; else panJointValLeft += panJointLeftDelta; } scanmc->advanceTime((counter%2==1) ? tiltTime : panTime); scanmc->setOutputCmd(HeadOffset+TiltOffset, tiltJointVal); scanmc->setOutputCmd(HeadOffset+RollOffset,0); scanmc->setOutputCmd(HeadOffset+PanOffset, (counter < 2) ? panJointValRite : panJointValLeft); } hpmc->lookAtPoint(firstPt.coordX(),firstPt.coordY(),firstPt.coordZ()); pointer_id = motman->addPrunableMotion(hpmc); // moves head to where scan begins // actual scan motion sequence (paused), played when pointer_id above is completed sequence_id = motman->addPrunableMotion(scanmc,MotionManager::kIgnoredPriority); } } // namespace

DualCoding 3.0beta

Generated Wed Oct 4 00:01:53 2006 by Doxygen 1.4.7