WalkToTargetNode.ccGo to the documentation of this file.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 "Behaviors/Transitions/VisualTargetCloseTrans.h"
00009
00010 void WalkToTargetNode::DoStart() {
00011 StateNode::DoStart();
00012
00013 headpointer_id = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00014 walker_id = motman->addPersistentMotion(SharedObject<WalkMC>());
00015
00016 erouter->addListener(this,EventBase::visObjEGID,tracking);
00017 }
00018
00019 void WalkToTargetNode::DoStop() {
00020 erouter->removeListener(this);
00021
00022 motman->removeMotion(headpointer_id);
00023 headpointer_id=MotionManager::invalid_MC_ID;
00024 motman->removeMotion(walker_id);
00025 walker_id=MotionManager::invalid_MC_ID;
00026
00027 StateNode::DoStop();
00028 }
00029
00030
00031 void WalkToTargetNode::processEvent(const EventBase& event) {
00032 static float horiz=0,vert=0;
00033 const VisionObjectEvent *ve = dynamic_cast<const VisionObjectEvent*>(&event);
00034 if(ve!=NULL && event.getTypeID()==EventBase::statusETID) {
00035 horiz=ve->getCenterX();
00036 vert=ve->getCenterY();
00037 } else
00038 return;
00039
00040
00041
00042 double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/6;
00043 double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/7.5;
00044 if(tilt>outputRanges[HeadOffset+TiltOffset][MaxRange])
00045 tilt=outputRanges[HeadOffset+TiltOffset][MaxRange];
00046 if(tilt<outputRanges[HeadOffset+TiltOffset][MinRange]*3/4)
00047 tilt=outputRanges[HeadOffset+TiltOffset][MinRange]*3/4;
00048 if(pan>outputRanges[HeadOffset+PanOffset][MaxRange]*2/3)
00049 pan=outputRanges[HeadOffset+PanOffset][MaxRange]*2/3;
00050 if(pan<outputRanges[HeadOffset+PanOffset][MinRange]*2/3)
00051 pan=outputRanges[HeadOffset+PanOffset][MinRange]*2/3;
00052 {MMAccessor<HeadPointerMC>(headpointer_id)->setJoints(tilt,pan,0);}
00053
00054 {
00055 MMAccessor<WalkMC> walker(walker_id);
00056 if(pan<-.05 || pan>.05)
00057 walker->setTargetVelocity(100,0,pan);
00058 else
00059 walker->setTargetVelocity(160,0,0);
00060 }
00061 }
00062
00063 Transition* WalkToTargetNode::newDefaultLostTrans(StateNode* dest) {
00064 return new TimeOutTrans(dest,1500,EventBase::visObjEGID,tracking);
00065 }
00066
00067 Transition* WalkToTargetNode::newDefaultCloseTrans(StateNode* dest) {
00068 return new VisualTargetCloseTrans(dest,tracking);
00069 }
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
|