WalkToTargetMachine.cc Source File

00001 #include "WalkToTargetMachine.h"
00002 #include "Motion/HeadPointerMC.h"
00003 #include "Motion/WalkMC.h"
00004 #include "Vision/Vision.h"
00005 #include "Shared/WorldState.h"
00006 
00007 //! Converts degrees to radians
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(new VisualTargetCloseTrans(this,close,tracking));
00014   if(lost!=NULL)
00015     addTransition(timeout=new TimeOutTrans(this,lost,500));
00016 }
00017 
00018 
00019 void WalkToTargetMachine::DoStart() {
00020   StateNode::DoStart();
00021   headpointer_id = motman->addMotion(SharedObject<HeadPointerMC>());
00022   walker_id = motman->addMotion(SharedObject<WalkMC>());
00023   erouter->addListener(this,EventBase::visionEGID,tracking);
00024 }
00025 
00026 void WalkToTargetMachine::DoStop() {
00027   erouter->forgetListener(this);
00028   motman->removeMotion(headpointer_id);
00029   motman->removeMotion(walker_id);
00030   StateNode::DoStop();
00031 }
00032 
00033 //this could be cleaned up event-wise (only use a timer when out of view)
00034 void WalkToTargetMachine::processEvent(const EventBase& event) {
00035   if(timeout)
00036     timeout->resetTimer();
00037   static float horiz=0,vert=0;
00038   const VisionEvent *ve = dynamic_cast<const VisionEvent*>(&event);
00039   if(ve!=NULL && event.getTypeID()==EventBase::statusETID) {
00040     horiz=ve->getCenterX();
00041     vert=ve->getCenterY();
00042   } else
00043     return;
00044 
00045   //cout << "Pos: " << horiz << ' ' << vert << endl;
00046 
00047   double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/6;
00048   double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/7.5;
00049   if(tilt>outputRanges[HeadOffset+TiltOffset][MaxRange])
00050     tilt=outputRanges[HeadOffset+TiltOffset][MaxRange];
00051   if(tilt<outputRanges[HeadOffset+TiltOffset][MinRange]*3/4)
00052     tilt=outputRanges[HeadOffset+TiltOffset][MinRange]*3/4;
00053   if(pan>outputRanges[HeadOffset+PanOffset][MaxRange]*2/3)
00054     pan=outputRanges[HeadOffset+PanOffset][MaxRange]*2/3;
00055   if(pan<outputRanges[HeadOffset+PanOffset][MinRange]*2/3)
00056     pan=outputRanges[HeadOffset+PanOffset][MinRange]*2/3;
00057   HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id);
00058   headpointer->setJoints(tilt,pan,0);
00059   motman->checkinMotion(headpointer_id);
00060 
00061   WalkMC * walker = (WalkMC*)motman->checkoutMotion(walker_id);
00062   if(pan<-.05 || pan>.05)
00063     walker->setTargetVelocity(100,0,pan);
00064   else
00065     walker->setTargetVelocity(160,0,0);
00066   motman->checkinMotion(walker_id);
00067 }
00068 
00069 /*! @file
00070  * @brief Implements WalkToTargetMachine, a state machine for walking towards a visual target
00071  * @author ejt (Creator)
00072  *
00073  * $Author: ejt $
00074  * $Name: tekkotsu-1_5 $
00075  * $Revision: 1.6 $
00076  * $State: Rel $
00077  * $Date: 2003/09/25 15:26:23 $
00078  */
00079