WalkToTargetNode.cc

Go to the documentation of this file.
00001 #include "Motion/WalkMC.h"
00002 #ifdef TGT_HAS_WALK
00003 
00004 #include "WalkToTargetNode.h"
00005 #include "Motion/HeadPointerMC.h"
00006 #include "Shared/RobotInfo.h"
00007 #include "IPC/SharedObject.h"
00008 #include "Motion/MMAccessor.h"
00009 #include "Events/VisionObjectEvent.h"
00010 #include "Shared/WorldState.h"
00011 #include "Behaviors/Transitions/TimeOutTrans.h"
00012 #include "Shared/ERS7Info.h"
00013 #ifdef TGT_HAS_IR_DISTANCE
00014 #  include "Behaviors/Transitions/VisualTargetCloseTrans.h"
00015 #endif
00016 
00017 #if defined(TGT_HAS_CAMERA) && ( defined(TGT_HAS_LEGS) || defined(TGT_HAS_WHEELS) )
00018 #include "Behaviors/Controls/BehaviorSwitchControl.h"
00019 #include "Shared/ProjectInterface.h"
00020 REGISTER_CONTROL_INSTANCE(WalkToTarget,new BehaviorSwitchControlBase(new WalkToTargetNode("Walk To Target (ball)",ProjectInterface::visPinkBallSID)),DEFAULT_TK_MENU"/State Machine Demos");
00021 #endif
00022 
00023 
00024 void WalkToTargetNode::doStart() {
00025   headpointer_id = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00026   walker_id = motman->addPersistentMotion(SharedObject<WalkMC>());
00027 
00028   erouter->addListener(this,EventBase::visObjEGID,tracking);
00029 }
00030 
00031 void WalkToTargetNode::doStop() {
00032   erouter->removeListener(this);
00033 
00034   motman->removeMotion(headpointer_id);
00035   headpointer_id=MotionManager::invalid_MC_ID;
00036   motman->removeMotion(walker_id);
00037   walker_id=MotionManager::invalid_MC_ID;
00038 }
00039 
00040 //this could be cleaned up event-wise (only use a timer when out of view)
00041 void WalkToTargetNode::doEvent() {
00042   static float horiz=0,vert=0;
00043   const VisionObjectEvent *ve = dynamic_cast<const VisionObjectEvent*>(event);
00044   if(ve!=NULL && event->getTypeID()==EventBase::statusETID) {
00045     horiz=ve->getCenterX();
00046     vert=ve->getCenterY();
00047   } else
00048     return;
00049 
00050   // for portability, look to see if the host hardware has a head pan & tilt joints
00051   const unsigned int panIdx = capabilities.findOutputOffset(ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::PanOffset]);
00052   const unsigned int tiltIdx = capabilities.findOutputOffset(ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::TiltOffset]);
00053   if(panIdx==-1U || tiltIdx==-1U)
00054     return; // guess not...
00055   
00056   //cout << "Pos: " << horiz << ' ' << vert << endl;
00057 
00058   float tilt=state->outputs[tiltIdx]-vert*(float)M_PI/6;
00059   float pan=state->outputs[panIdx]-horiz*(float)M_PI/7.5f;
00060   if(tilt>outputRanges[tiltIdx][MaxRange])
00061     tilt=outputRanges[tiltIdx][MaxRange];
00062   if(tilt<outputRanges[tiltIdx][MinRange]*3/4)
00063     tilt=outputRanges[tiltIdx][MinRange]*3/4;
00064   if(pan>outputRanges[panIdx][MaxRange]*2/3)
00065     pan=outputRanges[panIdx][MaxRange]*2/3;
00066   if(pan<outputRanges[panIdx][MinRange]*2/3)
00067     pan=outputRanges[panIdx][MinRange]*2/3;
00068   MMAccessor<HeadPointerMC>(headpointer_id)->setJoints(tilt,pan,0); // note no variable name, one-line scope
00069   
00070   {
00071     MMAccessor<WalkMC> walker(walker_id);
00072     if(pan<-.05 || pan>.05)
00073       walker->setTargetVelocity(100,0,pan);
00074     else
00075       walker->setTargetVelocity(160,0,0);
00076   }
00077 }
00078 
00079 Transition* WalkToTargetNode::newDefaultLostTrans(StateNode* dest) {
00080   return new TimeOutTrans(dest,1500,EventBase::visObjEGID,tracking);
00081 }
00082 
00083 Transition* WalkToTargetNode::newDefaultCloseTrans(StateNode* dest) {
00084 #ifdef TGT_HAS_IR_DISTANCE
00085   return new VisualTargetCloseTrans(dest,tracking);
00086 #else
00087   (void)dest;
00088   return NULL;
00089 #endif
00090 }
00091 
00092 #endif
00093 
00094 /*! @file
00095  * @brief Implements WalkToTargetNode, a state node for walking towards a visual target
00096  * @author ejt (Creator)
00097  */