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