00001 #include "WalkToTargetNode.h"
00002 #include "Motion/HeadPointerMC.h"
00003 #include "Motion/WalkMC.h"
00004 #include "Motion/MMAccessor.h"
00005 #include "Events/VisionObjectEvent.h"
00006 #include "Shared/WorldState.h"
00007 #include "Behaviors/Transitions/TimeOutTrans.h"
00008 #include "Shared/RobotInfo.h"
00009 #include "Shared/ERS7Info.h"
00010 #ifdef TGT_HAS_IR_DISTANCE
00011 # include "Behaviors/Transitions/VisualTargetCloseTrans.h"
00012 #endif
00013
00014 void WalkToTargetNode::DoStart() {
00015 StateNode::DoStart();
00016
00017 headpointer_id = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00018 walker_id = motman->addPersistentMotion(SharedObject<WalkMC>());
00019
00020 erouter->addListener(this,EventBase::visObjEGID,tracking);
00021 }
00022
00023 void WalkToTargetNode::DoStop() {
00024 erouter->removeListener(this);
00025
00026 motman->removeMotion(headpointer_id);
00027 headpointer_id=MotionManager::invalid_MC_ID;
00028 motman->removeMotion(walker_id);
00029 walker_id=MotionManager::invalid_MC_ID;
00030
00031 StateNode::DoStop();
00032 }
00033
00034
00035 void WalkToTargetNode::processEvent(const EventBase& event) {
00036 static float horiz=0,vert=0;
00037 const VisionObjectEvent *ve = dynamic_cast<const VisionObjectEvent*>(&event);
00038 if(ve!=NULL && event.getTypeID()==EventBase::statusETID) {
00039 horiz=ve->getCenterX();
00040 vert=ve->getCenterY();
00041 } else
00042 return;
00043
00044
00045 const unsigned int panIdx = capabilities.findOutputOffset(ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::PanOffset]);
00046 const unsigned int tiltIdx = capabilities.findOutputOffset(ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::TiltOffset]);
00047 if(panIdx==-1U || tiltIdx==-1U)
00048 return;
00049
00050
00051
00052 double tilt=state->outputs[tiltIdx]-vert*M_PI/6;
00053 double pan=state->outputs[panIdx]-horiz*M_PI/7.5;
00054 if(tilt>outputRanges[tiltIdx][MaxRange])
00055 tilt=outputRanges[tiltIdx][MaxRange];
00056 if(tilt<outputRanges[tiltIdx][MinRange]*3/4)
00057 tilt=outputRanges[tiltIdx][MinRange]*3/4;
00058 if(pan>outputRanges[panIdx][MaxRange]*2/3)
00059 pan=outputRanges[panIdx][MaxRange]*2/3;
00060 if(pan<outputRanges[panIdx][MinRange]*2/3)
00061 pan=outputRanges[panIdx][MinRange]*2/3;
00062 MMAccessor<HeadPointerMC>(headpointer_id)->setJoints(tilt,pan,0);
00063
00064 {
00065 MMAccessor<WalkMC> walker(walker_id);
00066 if(pan<-.05 || pan>.05)
00067 walker->setTargetVelocity(100,0,pan);
00068 else
00069 walker->setTargetVelocity(160,0,0);
00070 }
00071 }
00072
00073 Transition* WalkToTargetNode::newDefaultLostTrans(StateNode* dest) {
00074 return new TimeOutTrans(dest,1500,EventBase::visObjEGID,tracking);
00075 }
00076
00077 Transition* WalkToTargetNode::newDefaultCloseTrans(StateNode* dest) {
00078 #ifdef TGT_HAS_IR_DISTANCE
00079 return new VisualTargetCloseTrans(dest,tracking);
00080 #else
00081 (void)dest;
00082 return NULL;
00083 #endif
00084 }
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096