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