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 
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 //this could be cleaned up event-wise (only use a timer when out of view)
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   //cout << "Pos: " << horiz << ' ' << vert << endl;
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 /*! @file
00081  * @brief Implements WalkToTargetMachine, a state machine for walking towards a visual target
00082  * @author ejt (Creator)
00083  *
00084  * $Author: ejt $
00085  * $Name: tekkotsu-2_2 $
00086  * $Revision: 1.14 $
00087  * $State: Exp $
00088  * $Date: 2004/10/17 01:16:10 $
00089  */
00090 

Tekkotsu v2.2
Generated Tue Oct 19 14:19:16 2004 by Doxygen 1.3.9.1