00001
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
00149
00150 curPAR = dynamic_cast<LookoutPointRequest*>(curReq);
00151
00152
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
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
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;
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
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
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
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 )
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
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
00435
00436
00437
00438
00439
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
00450
00451
00452
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
00491
00492 void Lookout::processScanEvent(const EventBase& ev) {
00493
00494 static bool listeningObjEGID = false;
00495 const LookoutScanRequest* curScanReq = dynamic_cast<LookoutScanRequest*>(curReq);
00496
00497 switch (ev.getGeneratorID()) {
00498 case EventBase::motmanEGID:
00499
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:
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);
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);
00605 MMAccessor<HeadPointerMC> pointer_acc(pointer_id);
00606
00607
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
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
00627 motman->setPriority(sequence_id,MotionManager::kIgnoredPriority);
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);
00641 motman->setPriority(pointer_id,MotionManager::kStdPriority);
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);
00652 motman->setPriority(sequence_id,MotionManager::kBackgroundPriority);
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
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);
00687 }
00688
00689
00690
00691 void Lookout::setupTrack() {
00692
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());
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;
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);
00726 }
00727 return;
00728 }
00729
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 )
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
00778
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
00791
00792
00793
00794
00795
00796
00797
00798
00799
00800
00801
00802
00803
00804
00805
00806
00807
00808
00809
00810
00811
00812
00813
00814
00815
00816
00817
00818
00819
00820
00821
00822
00823
00824
00825
00826
00827
00828
00829
00830
00831
00832
00833
00834
00835
00836
00837
00838
00839
00840
00841
00842
00843
00844
00845
00846
00847
00848
00849
00850
00851
00852
00853
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864
00865
00866
00867
00868
00869
00870
00871
00872
00873
00874
00875
00876
00877
00878
00879
00880
00881
00882
00883
00884
00885
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896 void Lookout::setupSearch() {
00897
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
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
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
00945 if ( col.list != NULL && col.list->area >= curSR->minBlobArea )
00946 erouter->removeListener(this,EventBase::visRegionEGID);
00947 else
00948 return;
00949
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
00954
00955
00956 MMAccessor<HeadPointerMC>(pointer_id)->lookAtPoint(centerPt.coordX(),centerPt.coordY(),centerPt.coordZ());
00957 motman->setPriority(pointer_id,MotionManager::kStdPriority);
00958
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:
00970 triggerSearchMotionSequence();
00971 break;
00972 case searching:
00973 case centering:
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
01006
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 }