00001 #include "Behaviors/Transitions/CompletionTrans.h"
00002 #include "Behaviors/Transitions/EventTrans.h"
00003 #include "Behaviors/Transitions/TimeOutTrans.h"
00004 #include "Events/EventRouter.h"
00005 #include "Events/LocomotionEvent.h"
00006
00007 #include "MapBuilder.h"
00008 #include "Lookout.h"
00009 #include "VRmixin.h"
00010
00011 #include "ShapeTarget.h"
00012
00013 #include "Pilot.h"
00014
00015 using namespace std;
00016
00017 namespace DualCoding {
00018
00019 Pilot::Pilot() :
00020 StateNode("Pilot"), requests(), curReq(NULL), idCounter(0),
00021 waypointwalk_id(MotionManager::invalid_MC_ID), posture_id(MotionManager::invalid_MC_ID),
00022 lastDisplayParticleTime(0),
00023 dispatchNode(NULL)
00024 {
00025 setRetain(false);
00026 }
00027
00028
00029 void Pilot::DoStart() {
00030 StateNode::DoStart();
00031 SharedObject<WaypointWalkMC> waypointwalk_mc;
00032 waypointwalk_id = motman->addPersistentMotion(waypointwalk_mc,MotionManager::kIgnoredPriority);
00033 SharedObject<PostureMC> posture_mc;
00034 posture_id = motman->addPersistentMotion(posture_mc,MotionManager::kIgnoredPriority);
00035 if ( verbosity & PVstart )
00036 cout << "Pilot starting up: waypointwalk_id=" << waypointwalk_id
00037 << ", posture_id=" << posture_id << endl;
00038 }
00039
00040 void Pilot::DoStop() {
00041 if ( verbosity & PVstart )
00042 cout << "Pilot is shutting down." << endl;
00043 abort();
00044 motman->removeMotion(waypointwalk_id);
00045 waypointwalk_id = MotionManager::invalid_MC_ID;
00046 motman->removeMotion(posture_id);
00047 posture_id = MotionManager::invalid_MC_ID;
00048 StateNode::DoStop();
00049 }
00050
00051 void Pilot::abort() {
00052 if ( dispatchNode )
00053 dispatchNode->DoStop();
00054 curReq = NULL;
00055 while (!requests.empty()) {
00056 delete requests.front();
00057 requests.pop();
00058 }
00059 motman->setPriority(waypointwalk_id,MotionManager::kIgnoredPriority);
00060 motman->setPriority(posture_id,MotionManager::kIgnoredPriority);
00061 }
00062
00063 void Pilot::setup() {
00064 StateNode::setup();
00065 dispatchNode = new Dispatch;
00066 addNode(dispatchNode);
00067 }
00068
00069 unsigned int Pilot::executeRequest(const PilotRequest &req) {
00070 requests.push(new PilotRequest(req));
00071 const unsigned int id = ++idCounter;
00072 requests.back()->requestID = id;
00073 executeRequest();
00074 return id;
00075 }
00076
00077 void Pilot::executeRequest() {
00078
00079
00080 if ( curReq == NULL & !requests.empty() ) {
00081 curReq = requests.front();
00082 if ( verbosity & PVexecute )
00083 cout << "Pilot executing request " << curReq->requestID << endl;
00084 dispatchNode->DoStart();
00085 }
00086 }
00087
00088 void Pilot::requestComplete(const bool result) {
00089 if ( curReq == NULL ) {
00090 cout << "Pilot::requestComplete had NULL curReq !!!!!!!!!!!!!!!!" << endl;
00091 return;
00092 }
00093 if ( curReq->trackRequest != NULL )
00094 VRmixin::lookout.stopTrack();
00095 const unsigned int reqID = curReq->requestID;
00096 delete curReq;
00097 curReq = NULL;
00098 requests.pop();
00099 motman->setPriority(waypointwalk_id,MotionManager::kIgnoredPriority);
00100 motman->setPriority(posture_id,MotionManager::kIgnoredPriority);
00101 if ( verbosity & PVcomplete )
00102 cout << "Pilot request " << reqID << " complete." << endl;
00103 erouter->postEvent(EventBase(EventBase::pilotEGID, reqID, EventBase::deactivateETID,0, "Pilot Completion",float(result)));
00104 executeRequest();
00105 }
00106
00107
00108
00109 void Pilot::Dispatch::setup() {
00110 VisualRoutinesStateNode::setup();
00111 successNode = new Success;
00112 addNode(successNode);
00113 failureNode = new Failure;
00114 addNode(failureNode);
00115
00116
00117
00118 walkNode = new Walk;
00119 addNode(walkNode);
00120 walkNode->setMC(pilot.waypointwalk_id);
00121 walkNode->addTransition(new CompletionTrans(successNode));
00122
00123
00124
00125 visualsearchNode = new VisualSearch;
00126 addNode(visualsearchNode);
00127 VisualSearchHaveMap *visualsearchhavemapNode = new VisualSearchHaveMap;
00128 addNode(visualsearchhavemapNode);
00129 VisualSearchWalk *visualsearchwalkNode = new VisualSearchWalk;
00130 visualsearchwalkNode->setMC(pilot.waypointwalk_id);
00131 addNode(visualsearchwalkNode);
00132
00133 visualsearchNode->addTransition(new EventTrans(visualsearchhavemapNode, EventBase::mapbuilderEGID,
00134 &(visualsearchNode->mapbuilder_id), EventBase::deactivateETID));
00135 visualsearchhavemapNode->addTransition(new CompletionTrans(visualsearchwalkNode));
00136 visualsearchwalkNode->addTransition(new CompletionTrans(visualsearchNode));
00137
00138
00139
00140 gotoshapeNode = new GotoShape;
00141 gotoshapeNode->setMC(pilot.waypointwalk_id);
00142 addNode(gotoshapeNode);
00143 gotoshapeNode->addTransition(new CompletionTrans(successNode));
00144
00145
00146
00147 gotoTargetNode = new GotoTarget;
00148 gotoTargetNode->setMC(pilot.waypointwalk_id);
00149 addNode(gotoTargetNode);
00150
00151 CreepToShapeStand *gotoTargetStandNode = new CreepToShapeStand;
00152 gotoTargetStandNode->setMC(pilot.posture_id);
00153 addNode(gotoTargetStandNode);
00154
00155 CreepToShapeWait *gotoTargetWaitNode = new CreepToShapeWait;
00156 addNode(gotoTargetWaitNode);
00157
00158 CreepToShapeBuildMap *gotoTargetBuildMapNode = new CreepToShapeBuildMap;
00159 addNode(gotoTargetBuildMapNode);
00160
00161 gotoTargetNode->buildTargetNode = new BuildTarget;
00162 addNode(gotoTargetNode->buildTargetNode);
00163
00164 gotoTargetNode->addTransition(new CompletionTrans(gotoTargetStandNode));
00165 gotoTargetStandNode->addTransition(new CompletionTrans(gotoTargetWaitNode));
00166 gotoTargetWaitNode->addTransition(new TimeOutTrans(gotoTargetBuildMapNode,2000));
00167 gotoTargetBuildMapNode->addTransition(new EventTrans(gotoTargetNode->buildTargetNode,
00168 EventBase::mapbuilderEGID,
00169 &CreepToShapeBuildMap::mapreq_id,
00170 EventBase::deactivateETID));
00171 gotoTargetNode->buildTargetNode->addTransition(new CompletionTrans(gotoTargetNode));
00172
00173
00174
00175 pushTargetNode = new PushTarget;
00176 pushTargetNode->setMC(pilot.waypointwalk_id);
00177 addNode(pushTargetNode);
00178
00179 CreepToShapeStand *pushTargetStandNode = new CreepToShapeStand;
00180 pushTargetStandNode->setMC(pilot.posture_id);
00181 addNode(pushTargetStandNode);
00182
00183 CreepToShapeWait *pushTargetWaitNode = new CreepToShapeWait;
00184 addNode(pushTargetWaitNode);
00185
00186 CreepToShapeBuildMap *pushTargetBuildMapNode = new CreepToShapeBuildMap;
00187 addNode(pushTargetBuildMapNode);
00188
00189 pushTargetNode->buildTargetNode = new BuildTarget;
00190 addNode(pushTargetNode->buildTargetNode);
00191
00192 pushTargetNode->addTransition(new CompletionTrans(pushTargetStandNode));
00193 pushTargetStandNode->addTransition(new CompletionTrans(pushTargetWaitNode));
00194 pushTargetWaitNode->addTransition(new TimeOutTrans(pushTargetBuildMapNode,2000));
00195 pushTargetBuildMapNode->addTransition(new EventTrans(pushTargetNode->buildTargetNode,
00196 EventBase::mapbuilderEGID,
00197 &CreepToShapeBuildMap::mapreq_id,
00198 EventBase::deactivateETID));
00199 pushTargetNode->buildTargetNode->addTransition(new CompletionTrans(pushTargetNode));
00200
00201
00202
00203 creeptoshapeNode = new CreepToShape;
00204 creeptoshapeNode->setMC(pilot.waypointwalk_id);
00205 addNode(creeptoshapeNode);
00206
00207 CreepToShapeStand *creeptoshapestandNode = new CreepToShapeStand;
00208 creeptoshapestandNode->setMC(pilot.posture_id);
00209 addNode(creeptoshapestandNode);
00210
00211 CreepToShapeWait *creeptoshapewaitNode = new CreepToShapeWait;
00212 addNode(creeptoshapewaitNode);
00213
00214 CreepToShapeBuildMap *creeptoshapebuildmapNode = new CreepToShapeBuildMap;
00215 addNode(creeptoshapebuildmapNode);
00216
00217 CreepToShapeLocalize *creeptoshapelocalizeNode = new CreepToShapeLocalize;
00218 addNode(creeptoshapelocalizeNode);
00219
00220 creeptoshapeNode->addTransition(new CompletionTrans(creeptoshapestandNode));
00221 creeptoshapestandNode->addTransition(new CompletionTrans(creeptoshapewaitNode));
00222 creeptoshapewaitNode->addTransition(new TimeOutTrans(creeptoshapebuildmapNode,500));
00223 creeptoshapebuildmapNode->addTransition(new EventTrans(creeptoshapelocalizeNode,
00224 EventBase::mapbuilderEGID,
00225 &CreepToShapeBuildMap::mapreq_id,
00226 EventBase::deactivateETID));
00227 creeptoshapelocalizeNode->addTransition(new CompletionTrans(creeptoshapeNode));
00228
00229
00230 #ifdef TGT_HAS_IR_DISTANCE
00231 #ifdef TGT_ERS7
00232 ircliffNode = new IRCliff;
00233 addNode(ircliffNode);
00234 #endif //TGT_ERS7
00235 irobstacleNode = new IRObstacle;
00236 addNode(irobstacleNode);
00237 #endif // TGT_HAS_IR_DISTANCE
00238
00239
00240
00241 localizeNode = new Localize;
00242 addNode(localizeNode);
00243
00244 }
00245
00246 void Pilot::Dispatch::DoStart() {
00247 VisualRoutinesStateNode::DoStart();
00248 #ifdef TGT_HAS_IR_DISTANCE
00249 #ifdef TGT_ERS7
00250 if ( pilot.curReq->avoidCliffs )
00251 ircliffNode->DoStart();
00252 #endif // TGT_ERS7
00253 if ( pilot.curReq->avoidObstacles )
00254 irobstacleNode->DoStart();
00255 #endif // TGT_HAS_IR_DISTANCE
00256 if ( pilot.curReq->trackRequest )
00257 if ( lookout.busy() )
00258 cout << "Error: Pilot cannot launch track request because the Lookout is busy!" << endl;
00259 else
00260 lookout.executeRequest(*pilot.curReq->trackRequest);
00261 if ( pilot.curReq->localizationInterval > 0 )
00262 localizeNode->DoStart();
00263 else if ( pilot.curReq->localizationDisplayInterval > 0 )
00264 cout << "Pilot warning: localizationDisplayInterval > 0 but no localization requested." << endl;
00265 switch ( pilot.curReq->getRequestType() ) {
00266 case PilotRequest::walk:
00267 walkNode->DoStart();
00268 break;
00269 case PilotRequest::visualSearch:
00270 visualsearchNode->DoStart();
00271 break;
00272 case PilotRequest::gotoShape:
00273 gotoshapeNode->DoStart();
00274 break;
00275 case PilotRequest::gotoTarget:
00276 gotoTargetNode->DoStart();
00277 break;
00278 case PilotRequest::pushTarget:
00279 pushTargetNode->DoStart();
00280 break;
00281 case PilotRequest::creepToShape:
00282 if ( pilot.curReq->mapBuilderRequest == NULL ||
00283 pilot.curReq->mapBuilderRequest->worldTargets.size() == 0 ||
00284 pilot.curReq->trackRequest != NULL ) {
00285 cout << "Pilot error: creeptoShape requires mapBuilderRequest with a worldTarget, and no trackRequest" << endl;
00286 VRmixin::pilot.failure();
00287 }
00288 else
00289 creeptoshapeNode->DoStart();
00290 break;
00291 default:
00292 cout << "Pilot request type " << pilot.curReq->getRequestType() << " not yet implemented." << endl;
00293 VRmixin::pilot.failure();
00294 }
00295 }
00296
00297
00298
00299 void Pilot::Success::DoStart() {
00300 StateNode::DoStart();
00301 if ( Pilot::getVerbosity() & Pilot::PVsuccess )
00302 cout << "Pilot result: Success" << endl;
00303 VRmixin::pilot.dispatchNode->DoStop();
00304 VRmixin::pilot.requestComplete(true);
00305 }
00306
00307 void Pilot::Failure::DoStart() {
00308 StateNode::DoStart();
00309 if ( Pilot::getVerbosity() & Pilot::PVfailure )
00310 cout << "Pilot result: Failure" << endl;
00311 VRmixin::pilot.dispatchNode->DoStop();
00312 VRmixin::pilot.requestComplete(false);
00313 }
00314
00315
00316
00317 void Pilot::Walk::DoStart() {
00318 WaypointWalkNode::DoStart();
00319 PilotRequest &req = *VRmixin::pilot.curReq;
00320 getMC()->getWaypointList().clear();
00321 getMC()->addEgocentricWaypoint(req.dx/1000, req.dy/1000, req.da, true, 0.1);
00322 getMC()->go();
00323 }
00324
00325
00326
00327 void Pilot::VisualSearch::DoStart() {
00328 VisualRoutinesStateNode::DoStart();
00329 if ( pilot.curReq->mapBuilderRequest == NULL ) {
00330 cout << "Pilot received visualSearch request with invalid mapBuilderRequest field" << endl;
00331 pilot.failure();
00332 }
00333 else if ( pilot.curReq->exitTest == NULL && pilot.curReq->searchRotationAngle != 0 ) {
00334 cout << "Pilot received visualSearch request with no exitTest and nonzero searchRotationAngle" << endl;
00335 pilot.failure();
00336 }
00337 else
00338 mapbuilder_id = VRmixin::mapBuilder.executeRequest(*pilot.curReq->mapBuilderRequest);
00339 }
00340
00341 void Pilot::VisualSearchHaveMap::DoStart() {
00342 VisualRoutinesStateNode::DoStart();
00343 if ( (*pilot.curReq->exitTest)() )
00344 pilot.success();
00345 else if ( pilot.curReq->searchRotationAngle == 0 )
00346 pilot.failure();
00347 else
00348 postCompletionEvent();
00349 }
00350
00351 void Pilot::VisualSearchWalk::DoStart() {
00352 WaypointWalkNode::DoStart();
00353 getMC()->getWaypointList().clear();
00354 getMC()->addEgocentricWaypoint(0,0,VRmixin::pilot.curReq->searchRotationAngle,true,0);
00355 getMC()->go();
00356 }
00357
00358
00359
00360 void Pilot::GotoShape::DoStart() {
00361 WaypointWalkNode::DoStart();
00362 const ShapeRoot &target = VRmixin::pilot.curReq->targetShape;
00363 Point const targetLoc(target->getCentroid());
00364 getMC()->getWaypointList().clear();
00365 switch ( target->getRefFrameType() ) {
00366 case unspecified:
00367 case camcentric:
00368 cout << "Pilot: GotoShape target " << target << " has suspect reference frame type; assuming egocentric." << endl;
00369
00370 case egocentric: {
00371 AngSignPi bearing = atan2(targetLoc.coordY(),targetLoc.coordX());
00372 float const distance = targetLoc.distanceFrom(Point(0,0));
00373 cout << "Pilot egocentric gotoShape: rotate by " << float(bearing)*180/M_PI << " deg., then forward "
00374 << distance << " mm" << endl;
00375 getMC()->addEgocentricWaypoint(0,0,bearing,true,0);
00376 getMC()->addEgocentricWaypoint(distance/1000,0,0,true,0.1);
00377 break;
00378 }
00379 case allocentric:
00380 cout << "Pilot alloocentric gotoShape: " << targetLoc
00381 << " current position: " << VRmixin::theAgent->getCentroid() << endl;
00382 getMC()->addAbsoluteWaypoint(targetLoc.coordX()/1000, targetLoc.coordY()/1000, 0, true, 0.1);
00383 }
00384 getMC()->go();
00385 }
00386
00387
00388
00389
00390 void Pilot::GotoTarget::DoStart() {
00391 WaypointWalkNode::DoStart();
00392
00393
00394 bool buildFrontLeft = true, buildFrontRight = true, buildBackLeft = true, buildBackRight = true, lookAtCentroid = true;
00395 int maxRetries = 10;
00396 if (VRmixin::pilot.curReq->buildTargetParamsFn != NULL) {
00397 (*VRmixin::pilot.curReq->buildTargetParamsFn)(&buildFrontLeft, &buildFrontRight, &buildBackLeft, &buildBackRight, &lookAtCentroid, &maxRetries);
00398 }
00399 if (buildTargetNode != NULL)
00400 buildTargetNode->setParams(buildFrontLeft, buildFrontRight, buildBackLeft, buildBackRight, lookAtCentroid, maxRetries);
00401
00402 Point waypoint;
00403 AngSignPi angle;
00404 int result = getNextWaypoint( &waypoint, &angle );
00405 if (result < 0) {
00406 VRmixin::pilot.failure();
00407 return;
00408 }
00409 else if (result > 0) {
00410 VRmixin::pilot.success();
00411 return;
00412 }
00413
00414 if ( waypoint != Point(0, 0, 0) ) {
00415 if (abs(angle) <= M_PI / 6) {
00416
00417 float currentAngle = getMC()->getCurA() + angle;
00418 getMC()->getWaypointList().clear();
00419 getMC()->addEgocentricWaypoint(waypoint.coordX() / 1000, waypoint.coordY() / 1000, (float)currentAngle, false, 0.1f);
00420 getMC()->go();
00421 }
00422 else {
00423
00424 angle = (angle >= 0) ? M_PI / 6 : -M_PI / 6;
00425 getMC()->getWaypointList().clear();
00426 getMC()->addEgocentricWaypoint(0, 0, (float)angle, true, 0.1f);
00427 getMC()->go();
00428 }
00429 }
00430 else {
00431
00432 getMC()->getWaypointList().clear();
00433 getMC()->addEgocentricWaypoint(0, 0, (float)angle, true, 0.1f);
00434 getMC()->go();
00435 }
00436
00437 }
00438
00439
00440
00441
00442 int Pilot::GotoTarget::getNextWaypoint(Point *point, AngSignPi *angle) {
00443 const Shape<TargetData>& target = ShapeRootTypeConst( VRmixin::pilot.curReq->targetShape, TargetData );
00444 if (!target.isValid()) {
00445 return -1;
00446 }
00447 if (target->getRefFrameType() != egocentric) {
00448 return -1;
00449 }
00450
00451 bool pointOK = true;
00452 bool angleOK = false;
00453
00454 const float orientation = target->getOrientation();
00455
00456
00457 Point desiredPoint( VRmixin::pilot.curReq->positionRelativeToTarget.coordX() * cos(orientation) - VRmixin::pilot.curReq->positionRelativeToTarget.coordY() * sin(orientation),
00458 VRmixin::pilot.curReq->positionRelativeToTarget.coordX() * sin(orientation) + VRmixin::pilot.curReq->positionRelativeToTarget.coordY() * cos(orientation),
00459 VRmixin::pilot.curReq->positionRelativeToTarget.coordZ() );
00460 Point presentPoint = target->getCentroid() * -1;
00461
00462
00463 *point = Point(0, 0, 0);
00464 if (presentPoint.distanceFrom(desiredPoint) > 100) {
00465 pointOK = false;
00466
00467
00468 AngSignPi desiredAngle = atan2(desiredPoint.coordY(), desiredPoint.coordX());
00469 AngSignPi presentAngle = atan2(presentPoint.coordY(), presentPoint.coordX());
00470
00471
00472 if (abs((double)(presentAngle - desiredAngle)) <= VRmixin::pilot.curReq->approachAngle) {
00473 *point = desiredPoint + target->getCentroid();
00474 }
00475
00476 else {
00477
00478 AngSignPi diff = desiredAngle - presentAngle;
00479 if (abs((double)diff) < VRmixin::pilot.curReq->subtendAngle) {
00480 presentAngle = desiredAngle;
00481 }
00482 else if ((double)diff >= 0) {
00483 presentAngle += VRmixin::pilot.curReq->subtendAngle;
00484 }
00485 else {
00486 presentAngle -= VRmixin::pilot.curReq->subtendAngle;
00487 }
00488 *point = getPointAtAngle(presentAngle);
00489 }
00490
00491
00492 *point = Point( point->coordX(), point->coordY(), 0 );
00493 float distance = point->distanceFrom( Point( 0, 0, 0) );
00494 float scaleFactor = distance < 200 ? 1.0 : 200 / distance;
00495 *point = *point * scaleFactor;
00496 }
00497
00498 Point delta = target->getCentroid() - *point;
00499 *angle = atan2(delta.coordY(), delta.coordX());
00500
00501
00502 if (pointOK && (abs( (float)*angle / M_PI * 180.0f ) <= 15)) {
00503 *angle = 0;
00504 angleOK = true;
00505 }
00506
00507 return ( pointOK && angleOK ) ? 1 : 0;
00508 }
00509
00510
00511
00512
00513 Point Pilot::GotoTarget::getPointAtAngle(AngSignPi angle) {
00514 Point point(VRmixin::pilot.curReq->safeDistanceAroundTarget * cos(angle),
00515 VRmixin::pilot.curReq->safeDistanceAroundTarget * sin(angle),
00516 VRmixin::pilot.curReq->positionRelativeToTarget.coordZ());
00517 point += VRmixin::pilot.curReq->targetShape->getCentroid();
00518 return point;
00519 }
00520
00521
00522
00523 void Pilot::PushTarget::DoStart() {
00524 WaypointWalkNode::DoStart();
00525
00526
00527 bool buildFrontLeft = true, buildFrontRight = true, buildBackLeft = true, buildBackRight = true, lookAtCentroid = true;
00528 int maxRetries = 10;
00529 if (VRmixin::pilot.curReq->buildTargetParamsFn != NULL) {
00530 (*VRmixin::pilot.curReq->buildTargetParamsFn)(&buildFrontLeft, &buildFrontRight, &buildBackLeft, &buildBackRight, &lookAtCentroid, &maxRetries);
00531 }
00532 if (buildTargetNode != NULL)
00533 buildTargetNode->setParams(buildFrontLeft, buildFrontRight, buildBackLeft, buildBackRight, lookAtCentroid, maxRetries);
00534
00535 const Shape<TargetData>& target = ShapeRootTypeConst( VRmixin::pilot.curReq->targetShape, TargetData );
00536
00537 double orientation = target->getOrientation();
00538
00539 Point desiredPoint = Point( VRmixin::pilot.curReq->positionRelativeToTarget.coordX() * cos(orientation) - VRmixin::pilot.curReq->positionRelativeToTarget.coordY() * sin(orientation),
00540 VRmixin::pilot.curReq->positionRelativeToTarget.coordX() * sin(orientation) + VRmixin::pilot.curReq->positionRelativeToTarget.coordY() * cos(orientation),
00541 VRmixin::pilot.curReq->positionRelativeToTarget.coordZ() )
00542 + target->getCentroid();
00543
00544
00545 AngSignPi pushAngle = (double)VRmixin::pilot.curReq->angleToPushTarget + orientation;
00546 LineData pushLine(VRmixin::localShS, desiredPoint, pushAngle);
00547
00548
00549 Point origin(0, 0, 0);
00550 double totalDisplacement = 200.0;
00551 double xDisplacement = 200.0;
00552 double yDisplacement = pushLine.perpendicularDistanceFrom(origin);
00553
00554 Point leftPoint(pushLine.getCentroid().coordX(), pushLine.getCentroid().coordY() + 100, pushLine.getCentroid().coordZ());
00555 if (pushLine.pointsOnSameSide( leftPoint, origin )) {
00556 yDisplacement = -yDisplacement;
00557 }
00558
00559 float scaleFactor = abs(yDisplacement) < totalDisplacement ? 1.0 : totalDisplacement / yDisplacement;
00560 yDisplacement = yDisplacement * scaleFactor;
00561 totalDisplacement -= abs(yDisplacement);
00562 if (totalDisplacement <= 0) {
00563 xDisplacement = 0;
00564 }
00565 else {
00566 scaleFactor = xDisplacement < totalDisplacement ? 1.0 : totalDisplacement / xDisplacement;
00567 xDisplacement = xDisplacement * scaleFactor;
00568 }
00569
00570 if (abs((float)pushAngle) <= M_PI / 6) {
00571
00572
00573 float currentAngle = getMC()->getCurA() + pushAngle;
00574
00575 getMC()->getWaypointList().clear();
00576 getMC()->addEgocentricWaypoint(0, 0, (float)pushAngle, true, 0.1f);
00577 if (abs(yDisplacement) > 10) {
00578 getMC()->addEgocentricWaypoint(0, yDisplacement / 1000, currentAngle, false, 0.1f);
00579 }
00580 if (abs(xDisplacement) > 10) {
00581 getMC()->addEgocentricWaypoint(xDisplacement / 1000, 0, currentAngle, false, 0.1f);
00582 }
00583 getMC()->go();
00584 }
00585 else {
00586
00587 pushAngle = (pushAngle >= 0) ? M_PI / 6 : -M_PI / 6;
00588 getMC()->getWaypointList().clear();
00589 getMC()->addEgocentricWaypoint(0, 0, (float)pushAngle, true, 0.1f);
00590 getMC()->go();
00591 }
00592 }
00593
00594
00595 void Pilot::BuildTarget::DoStart() {
00596 VisualRoutinesStateNode::DoStart();
00597
00598 std::cout << "BuildTarget: DoStart" << std::endl;
00599
00600 erouter->addListener(this, EventBase::mapbuilderEGID);
00601
00602 triesLeft = maxRetries;
00603 generateMapBuilderRequest();
00604 }
00605
00606 void Pilot::BuildTarget::DoStop() {
00607 std::cout << "BuildTarget: DoStop" << std::endl;
00608
00609 myState = none;
00610 VisualRoutinesStateNode::DoStop();
00611 }
00612
00613 void Pilot::BuildTarget::processEvent(const EventBase& event) {
00614 switch ( event.getGeneratorID() ) {
00615
00616 case EventBase::mapbuilderEGID: {
00617 if ((myState == build) && (event.getTypeID() == EventBase::deactivateETID)) {
00618 GET_SHAPE(target, TargetData, localShS);
00619 if (target.isValid()) {
00620 target->printParams();
00621 }
00622
00623 generateMapBuilderRequest();
00624 }
00625
00626 break;
00627 };
00628 default:
00629 std::cout << "BuildTarget: Unexpected event: " << event.getDescription() << std::endl;
00630 }
00631 }
00632
00633 void Pilot::BuildTarget::setParams(bool _buildFrontLeft, bool _buildFrontRight, bool _buildBackLeft, bool _buildBackRight, bool _lookAtCentroid, int _maxRetries) {
00634 buildFrontLeft = _buildFrontLeft;
00635 buildFrontRight = _buildFrontRight;
00636 buildBackLeft = _buildBackLeft;
00637 buildBackRight = _buildBackRight;
00638 lookAtCentroid = _lookAtCentroid;
00639 maxRetries = _maxRetries;
00640 }
00641
00642 void Pilot::BuildTarget::generateMapBuilderRequest() {
00643 keepBestTarget();
00644 GET_SHAPE(target, TargetData, localShS);
00645 if (target.isValid()) {
00646
00647 Point nextPt;
00648 if (getNextPoint(&nextPt)) {
00649 VRmixin::pilot.curReq->targetShape = target;
00650
00651
00652 myState = complete;
00653 postCompletionEvent();
00654 return;
00655 }
00656
00657 myState = build;
00658
00659 if ( VRmixin::pilot.curReq->buildTargetMapBuilderRequestFn != NULL ) {
00660 MapBuilderRequest* mapreq = (*VRmixin::pilot.curReq->buildTargetMapBuilderRequestFn)(nextPt);
00661 VRmixin::mapBuilder.executeRequest(*mapreq);
00662 delete mapreq;
00663 }
00664 else {
00665 cout << "BuildTarget: No MapBuilderRequest function provided." << endl;
00666 myState = error;
00667 VRmixin::pilot.failure();
00668 return;
00669 }
00670 }
00671 else {
00672 cout << "BuildTarget: Cannot find target in build phase." << endl;
00673 myState = error;
00674 VRmixin::pilot.failure();
00675 return;
00676 }
00677 }
00678
00679 void Pilot::BuildTarget::keepBestTarget() {
00680 Shape<TargetData> best;
00681 int bestCount = 0;
00682 std::vector<Shape<TargetData> >target_vector = select_type<TargetData>(localShS);
00683 std::vector<Shape<TargetData> >::iterator targetIterator;
00684
00685
00686 for (targetIterator = target_vector.begin(); targetIterator != target_vector.end(); targetIterator++) {
00687 Shape<TargetData> target = *(targetIterator);
00688 int targetCount = 0;
00689 if (target->getFrontLeftPt().isValid())
00690 targetCount++;
00691 if (target->getFrontRightPt().isValid())
00692 targetCount++;
00693 if (target->getBackLeftPt().isValid())
00694 targetCount++;
00695 if (target->getBackRightPt().isValid())
00696 targetCount++;
00697 if (!best.isValid() || (targetCount > bestCount) || ((targetCount == bestCount) && (target->getLength() > best->getLength()))) {
00698 best = target;
00699 bestCount = targetCount;
00700 }
00701 }
00702 if (best.isValid()) {
00703 best->setName("target");
00704 for (targetIterator = target_vector.begin(); targetIterator != target_vector.end(); targetIterator++) {
00705 Shape<TargetData> other_target = *(targetIterator);
00706 if (best != other_target) {
00707 localShS.deleteShape(other_target);
00708 }
00709 }
00710 }
00711 }
00712
00713 bool Pilot::BuildTarget::getNextPoint(Point *nextPoint) {
00714 if (triesLeft <= 0) {
00715 return true;
00716 }
00717 triesLeft--;
00718
00719 GET_SHAPE(target, TargetData, localShS);
00720 if (target.isValid()) {
00721 EndPoint &frontLeft = target->getFrontLeftPt();
00722 EndPoint &frontRight = target->getFrontRightPt();
00723 EndPoint &backLeft = target->getBackLeftPt();
00724 EndPoint &backRight = target->getBackRightPt();
00725
00726 Point zero(0, 0, 0);
00727
00728 if (buildFrontLeft && target->isFrontValid() && !frontLeft.isValid() && ((frontLeftPt == zero) || (frontLeftPt != frontLeft))) {
00729 frontLeftPt = frontLeft;
00730 cout << "Looking at front left pt: " << frontLeft << endl;
00731 *nextPoint = frontLeft;
00732 return false;
00733 }
00734 else if (buildFrontRight && target->isFrontValid() && !frontRight.isValid() && ((frontRightPt == zero) || (frontRightPt != frontRight))) {
00735 frontRightPt = frontRight;
00736 cout << "Looking at front right pt: " << frontRight << endl;
00737 *nextPoint = frontRight;
00738 return false;
00739 }
00740 else if (buildBackLeft && target->isBackValid() && !backLeft.isValid() && ((backLeftPt == zero) || (backLeftPt != backLeft))) {
00741 backLeftPt = backLeft;
00742 cout << "Looking at back left pt: " << backLeft << endl;
00743 *nextPoint = backLeft;
00744 return false;
00745 }
00746 else if (buildBackRight && target->isBackValid() && !backRight.isValid() && ((backRightPt == zero) || (backRightPt != backRight))) {
00747 backRightPt = backRight;
00748 cout << "Looking at back right pt: " << backRight << endl;
00749 *nextPoint = backRight;
00750 return false;
00751 }
00752 else if (lookAtCentroid) {
00753 lookAtCentroid = false;
00754 if ((buildFrontLeft || buildFrontRight) && ((centroid == zero) || (centroid != target->getFrontCentroid()))) {
00755 centroid = target->getCentroid();
00756 cout << "Looking at centroid: " << centroid << endl;
00757 *nextPoint = centroid;
00758 return false;
00759 }
00760 else if (!buildFrontLeft && !buildFrontRight && (buildBackLeft || buildBackRight) && ((centroid == zero) || (centroid != target->getBackCentroid()))) {
00761 centroid = target->getBackCentroid();
00762 cout << "Looking at back centroid: " << centroid << endl;
00763 *nextPoint = centroid;
00764 return false;
00765 }
00766 }
00767 }
00768 return true;
00769 }
00770
00771
00772
00773 void Pilot::CreepToShape::DoStart() {
00774 WaypointWalkNode::DoStart();
00775 cout << " >>>>> CreepToShape" << endl;
00776 if ( VRmixin::lookout.busy() )
00777 cout << "Error: Pilot cannot launch track request because the Lookout is busy!" << endl;
00778 else
00779 VRmixin::lookout.executeRequest(LookoutTrackRequest(VRmixin::pilot.curReq->mapBuilderRequest->worldTargets.front()->targetShape));
00780 const ShapeRoot &target = VRmixin::pilot.curReq->targetShape;
00781 Point const targetLoc(target->getCentroid());
00782 getMC()->getWaypointList().clear();
00783 switch ( target->getRefFrameType() ) {
00784 case unspecified:
00785 case camcentric:
00786 cout << "Pilot: GotoShape target " << target << " has suspect reference frame type; assuming egocentric." << endl;
00787
00788 case egocentric: {
00789
00790 float distance = targetLoc.xyNorm();
00791 AngSignPi bearing = atan2(targetLoc.coordY(),targetLoc.coordX());
00792 cout << "Pilot egocentric creepToShape: rotate by " << float(bearing)*(180.0/M_PI) << " deg., then forward "
00793 << distance << " mm" << endl;
00794 if ( distance >= 200 ) {
00795 getMC()->addEgocentricWaypoint(0,0,bearing,true,0);
00796 getMC()->addEgocentricWaypoint(distance/1000,0,0,true,0.06);
00797 VRmixin::mapBuilder.moveAgent(0.0, 0.0, float(bearing));
00798 VRmixin::mapBuilder.moveAgent(distance, 0.0, 0.0);
00799 } else {
00800 getMC()->addEgocentricWaypoint(distance/1000, 0, VRmixin::theAgent->getOrientation(), false, 0.06);
00801 VRmixin::mapBuilder.moveAgent(distance, 0.0, 0.0);
00802 }
00803 break;
00804 }
00805 case allocentric:
00806 Point const agentLoc = VRmixin::theAgent->getCentroid();
00807 float const distance = targetLoc.xyDistanceFrom(agentLoc);
00808 if ( distance < 50 ) {
00809 VRmixin::pilot.success();
00810 return;
00811 }
00812 float const scaleFactor = distance < 200 ? 1.0 : 200/distance;
00813 Point const waypointLoc = agentLoc + (targetLoc-agentLoc)*scaleFactor;
00814 cout << "Pilot creepToShape: "
00815 << "current position: " << agentLoc
00816 << " next waypoint: " << waypointLoc
00817 << " target loc: " << targetLoc << endl;
00818 if ( distance > 200 )
00819 getMC()->addAbsoluteWaypoint(waypointLoc.coordX()/1000, waypointLoc.coordY()/1000, 0, true, 0.06);
00820 else
00821 getMC()->addAbsoluteWaypoint(waypointLoc.coordX()/1000, waypointLoc.coordY()/1000, VRmixin::theAgent->getOrientation(), false, 0.06);
00822 float const heading = atan2(waypointLoc.coordY(),waypointLoc.coordX());
00823 VRmixin::mapBuilder.setAgent(Point(waypointLoc.coordX(),
00824 waypointLoc.coordY(),
00825 agentLoc.coordZ(),
00826 allocentric),
00827 heading);
00828 break;
00829 }
00830 getMC()->go();
00831 }
00832
00833 void Pilot::CreepToShapeStand::DoStop() {
00834 PostureNode::DoStop();
00835 motman->setPriority(VRmixin::pilot.posture_id,MotionManager::kBackgroundPriority);
00836 }
00837
00838 void Pilot::CreepToShapeBuildMap::DoStart() {
00839 VisualRoutinesStateNode::DoStart();
00840 VRmixin::lookout.stopTrack();
00841 if ( VRmixin::pilot.curReq->mapBuilderRequest )
00842 VRmixin::mapBuilder.executeRequest(*VRmixin::pilot.curReq->mapBuilderRequest, &mapreq_id);
00843 else if ( VRmixin::pilot.curReq->mapBuilderRequestFn ) {
00844 MapBuilderRequest* mapreq = (*VRmixin::pilot.curReq->mapBuilderRequestFn)();
00845 VRmixin::mapBuilder.executeRequest(*mapreq, &mapreq_id);
00846 delete mapreq;
00847 }
00848 }
00849
00850 void Pilot::CreepToShapeLocalize::DoStart() {
00851 StateNode::DoStart();
00852 cout << " >>>>> CreepToShape Localize" << endl;
00853 if ( VRmixin::pilot.curReq->localizationTest != NULL &&
00854 (*VRmixin::pilot.curReq->localizationTest)() == false ) {
00855 VRmixin::pilot.failure();
00856 return;
00857 }
00858 VRmixin::particleFilter->setMinAcceptableWeight(-3);
00859 for ( int i=0; i<8; i++ )
00860 VRmixin::particleFilter->update();
00861 VRmixin::particleFilter->setAgent();
00862 VRmixin::particleFilter->displayParticles();
00863 Point const loc = VRmixin::theAgent->getCentroid();
00864 MMAccessor<WaypointWalkMC>(VRmixin::pilot.waypointwalk_id)->
00865 setCurPos(loc.coordX()/1000, loc.coordY()/1000, VRmixin::theAgent->getOrientation());
00866
00867
00868 postCompletionEvent();
00869 }
00870
00871 void Pilot::CreepToShapeLocalize::processEvent(const EventBase&) {
00872 postCompletionEvent();
00873 }
00874
00875
00876 #ifdef TGT_HAS_IR_DISTANCE
00877 #ifdef TGT_ERS7
00878
00879 void Pilot::IRCliff::DoStart() {
00880 StateNode::DoStart();
00881 erouter->addListener(this,EventBase::sensorEGID, SensorSrcID::UpdatedSID, EventBase::statusETID);
00882 }
00883
00884 void Pilot::IRCliff::processEvent(const EventBase &) {
00885 if ( state->sensors[ChestIRDistOffset] > VRmixin::pilot.curReq->cliffThreshold ) {
00886 cout << "Pilot: cliff detected!" << endl;
00887 VRmixin::pilot.failure();
00888 }
00889 }
00890 #endif //TGT_ERS7
00891 #endif //TGT_HAS_IR_DISTANCE
00892
00893
00894 #ifdef TGT_HAS_IR_DISTANCE
00895
00896 inline float getIR() {
00897 #ifdef TGT_ERS7
00898 if ( state->sensors[FarIRDistOffset] > 400 )
00899 return state->sensors[FarIRDistOffset];
00900 else
00901 return state->sensors[NearIRDistOffset];
00902 #else
00903 return state->sensors[IRDistOffset];
00904 #endif
00905 }
00906
00907 void Pilot::IRObstacle::DoStart() {
00908 StateNode::DoStart();
00909 erouter->addListener(this,EventBase::sensorEGID, SensorSrcID::UpdatedSID, EventBase::statusETID);
00910 }
00911
00912 void Pilot::IRObstacle::processEvent(const EventBase &) {
00913 if ( getIR() < VRmixin::pilot.curReq->obstacleThreshold ) {
00914 cout << "Pilot: obstacle detected!" << endl;
00915 VRmixin::pilot.failure();
00916 }
00917 }
00918 #endif // TGT_HAS_IR_DISTANCE
00919
00920
00921
00922 void Pilot::Localize::DoStart() {
00923 StateNode::DoStart();
00924 erouter->addTimer(this, 1, VRmixin::pilot.curReq->localizationInterval, true);
00925 if ( VRmixin::pilot.curReq->mapBuilderRequest->immediateRequest() == false )
00926 erouter->addListener(this,EventBase::mapbuilderEGID);
00927 }
00928
00929
00930
00931
00932
00933
00934
00935 void Pilot::Localize::processEvent(const EventBase& event) {
00936 switch ( event.getGeneratorID() ) {
00937 case EventBase::timerEGID:
00938 mapbuilder_id = VRmixin::mapBuilder.executeRequest(*VRmixin::pilot.curReq->mapBuilderRequest);
00939 if ( VRmixin::pilot.curReq->mapBuilderRequest->immediateRequest() )
00940 processMap(event.getTimeStamp());
00941 break;
00942 case EventBase::mapbuilderEGID:
00943 if ( event.getSourceID() == mapbuilder_id && event.getTypeID() == EventBase::deactivateETID )
00944 processMap(event.getTimeStamp());
00945 break;
00946 default:
00947 break;
00948 }
00949 }
00950
00951 void Pilot::Localize::processMap(unsigned int timestamp) {
00952 if ( VRmixin::pilot.curReq->localizationTest == NULL || (*VRmixin::pilot.curReq->localizationTest)() ) {
00953 VRmixin::particleFilter->update();
00954 VRmixin::particleFilter->setAgent();
00955 Point loc = VRmixin::theAgent->getCentroid();
00956 MMAccessor<WaypointWalkMC>(VRmixin::pilot.waypointwalk_id)->
00957 setCurPos(loc.coordX()/1000, loc.coordY()/1000, VRmixin::theAgent->getOrientation());
00958 int const dispInt = VRmixin::pilot.curReq->localizationDisplayInterval;
00959 if ( dispInt > 0 )
00960 if ( timestamp >= VRmixin::pilot.lastDisplayParticleTime+dispInt ) {
00961 VRmixin::particleFilter->displayParticles(VRmixin::pilot.curReq->localizationDisplayParticles);
00962 VRmixin::pilot.lastDisplayParticleTime = timestamp;
00963 }
00964 }
00965 }
00966
00967 Pilot::PilotVerbosity_t Pilot::verbosity = -1U;
00968
00969 unsigned int Pilot::CreepToShapeBuildMap::mapreq_id = 0;
00970
00971 }