Homepage Demos Overview Downloads Tutorials Reference
Credits

WalkToTargetMachine.cc

Go to the documentation of this file.
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 //! 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(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   headpointer_id = motman->addMotion(SharedObject<HeadPointerMC>());
00022   walker_id = motman->addMotion(SharedObject<WalkMC>());
00023   erouter->addListener(this,EventBase::visObjEGID,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 void WalkToTargetMachine::teardown() {
00034   closeTrans=NULL;
00035   timeout=NULL;
00036 }
00037 
00038 //this could be cleaned up event-wise (only use a timer when out of view)
00039 void WalkToTargetMachine::processEvent(const EventBase& event) {
00040   if(timeout)
00041     timeout->resetTimer();
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   //cout << "Pos: " << horiz << ' ' << vert << endl;
00051 
00052   double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/6;
00053   double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/7.5;
00054   if(tilt>outputRanges[HeadOffset+TiltOffset][MaxRange])
00055     tilt=outputRanges[HeadOffset+TiltOffset][MaxRange];
00056   if(tilt<outputRanges[HeadOffset+TiltOffset][MinRange]*3/4)
00057     tilt=outputRanges[HeadOffset+TiltOffset][MinRange]*3/4;
00058   if(pan>outputRanges[HeadOffset+PanOffset][MaxRange]*2/3)
00059     pan=outputRanges[HeadOffset+PanOffset][MaxRange]*2/3;
00060   if(pan<outputRanges[HeadOffset+PanOffset][MinRange]*2/3)
00061     pan=outputRanges[HeadOffset+PanOffset][MinRange]*2/3;
00062   HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id);
00063   headpointer->setJoints(tilt,pan,0);
00064   motman->checkinMotion(headpointer_id);
00065 
00066   WalkMC * walker = (WalkMC*)motman->checkoutMotion(walker_id);
00067   if(pan<-.05 || pan>.05)
00068     walker->setTargetVelocity(100,0,pan);
00069   else
00070     walker->setTargetVelocity(160,0,0);
00071   motman->checkinMotion(walker_id);
00072 }
00073 
00074 /*! @file
00075  * @brief Implements WalkToTargetMachine, a state machine for walking towards a visual target
00076  * @author ejt (Creator)
00077  *
00078  * $Author: ejt $
00079  * $Name: tekkotsu-2_0 $
00080  * $Revision: 1.10 $
00081  * $State: Exp $
00082  * $Date: 2003/12/05 20:26:34 $
00083  */
00084 

Tekkotsu v2.0
Generated Wed Jan 21 03:20:30 2004 by Doxygen 1.3.4