Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WalkNode.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_WalkNode_h_
00003 #define INCLUDED_WalkNode_h_
00004 
00005 #include "Behaviors/StateNode.h"
00006 #include "Motion/MotionManager.h"
00007 #include "Motion/WalkMC.h"
00008 #include "Motion/MMAccessor.h"
00009 #include "Events/LocomotionEvent.h"
00010 #include "Events/EventRouter.h"
00011 
00012 //! A StateNode for walking in a direction
00013 class WalkNode : public StateNode {
00014 public:
00015   //! lets us interpret values as either distances or velocities
00016   enum WalkMode_t {
00017     VelocityWalkMode, //!< #x, #y, #a will be interpreted as mm/s
00018     DistanceWalkMode //!< #x, #y, #a will be interpreted as millimeters
00019   };
00020 
00021 public:
00022   //!constructor
00023   WalkNode()
00024     : StateNode("WalkNode"), walkid(MotionManager::invalid_MC_ID), walkidIsMine(true), x(0), y(0), a(0), n(-1), walkMode()
00025   {}
00026 
00027   //!constructor, positive @a yvel is counter-clockwise from above (to match coordinate system), assumes velocity
00028   WalkNode(float xvel, float yvel, float avel)
00029     : StateNode("WalkNode"), walkid(MotionManager::invalid_MC_ID), walkidIsMine(true), x(xvel), y(yvel), a(avel), n(-1), walkMode(VelocityWalkMode)
00030   {}
00031 
00032   //!constructor, positive @a yvel is counter-clockwise from above (to match coordinate system), assumes distance
00033   WalkNode(float xvel, float yvel, float avel, int steps)
00034     : StateNode("WalkNode"), walkid(MotionManager::invalid_MC_ID), walkidIsMine(true), x(xvel), y(yvel), a(avel), n(steps), walkMode(DistanceWalkMode)
00035   {}
00036 
00037   //!constructor, positive @a yvel is counter-clockwise from above (to match coordinate system), assumes velocity
00038   WalkNode(const std::string& name, float xvel, float yvel, float avel)
00039     : StateNode("WalkNode",name), walkid(MotionManager::invalid_MC_ID), walkidIsMine(true), x(xvel), y(yvel), a(avel), n(-1), walkMode(VelocityWalkMode)
00040   {}
00041 
00042   //!constructor, positive @a yvel is counter-clockwise from above (to match coordinate system), assumes distance
00043   WalkNode(const std::string& name, float xvel, float yvel, float avel, int steps)
00044     : StateNode("WalkNode",name), walkid(MotionManager::invalid_MC_ID), walkidIsMine(true), x(xvel), y(yvel), a(avel), n(steps), walkMode(DistanceWalkMode)
00045   {}
00046 
00047   //!destructor, check if we need to call our teardown
00048   ~WalkNode() {
00049     if(issetup)
00050       teardown();
00051   }
00052 
00053   //! sets the velocity of the walk
00054   void setDisplacement(float xd, float yd, float ad, int np = -1) {
00055     //cout << "SET_DISPLACEMENT" << endl;
00056     //updateWalk(xd,yd,ad,np,false);
00057     storeValues(xd, yd, ad, np, DistanceWalkMode);
00058   }
00059 
00060   //! sets the velocity of the walk
00061   void setVelocity(float xvel, float yvel, float avel, int np = -1) {
00062     //updateWalk(xvel,yvel,avel,np);
00063     storeValues(xvel, yvel, avel, np, VelocityWalkMode);
00064   }
00065   
00066   //! sets the velocity in x direction (positive is forward)
00067   void setXVelocity(float xvel) { x=xvel; storeValues(xvel, y, a, n, VelocityWalkMode); }
00068 
00069   //! returns the velocity in x direction (positive is forward)
00070   float getXVelocity() { return x; }
00071 
00072   //! sets the velocity in y direction (positive is forward)
00073   void setYVelocity(float yvel) { y=yvel; storeValues(x, yvel, a, n, VelocityWalkMode); }
00074 
00075   //! returns the velocity in y direction (positive is forward)
00076   float getYVelocity() { return y; }
00077 
00078   //! sets the velocity of the turn, positive is counter-clockwise from above (to match coordinate system)
00079   void setAVelocity(float avel) { a=avel; storeValues(x, y, avel, n, VelocityWalkMode); }
00080 
00081   //! returns the velocity of the turn, positive is counter-clockwise from above (to match coordinate system)
00082   float getAVelocity() { return a; }
00083 
00084   virtual void DoStart() {
00085     StateNode::DoStart();
00086     if (walkid != MotionManager::invalid_MC_ID) {
00087       erouter->addListener(this, EventBase::locomotionEGID, walkid, EventBase::statusETID);
00088     }
00089 
00090     updateWMC();
00091   }
00092 
00093   virtual void DoStop() {
00094     //added the 0
00095     //cout << "-----------WALK NODE: DoStop()" << endl;
00096     erouter->removeListener(this);
00097     if(walkid!=MotionManager::invalid_MC_ID) {
00098       MMAccessor<WalkMC> walk(walkid);
00099       walk->setTargetVelocity(0,0,0,0);
00100     }
00101     //updateWalk(0,0,0,0);
00102     StateNode::DoStop();
00103   }
00104 
00105         //! receive locomotionEGID status event and throw stateMachineEGID status event, ie a completion event
00106         virtual void processEvent(const EventBase& e) {
00107     //cout << "PROCESSING WALK EVENTS" << endl;
00108     if (e.getGeneratorID() == EventBase::locomotionEGID) {
00109       const LocomotionEvent le = *reinterpret_cast<const LocomotionEvent*>(&e);
00110       //cout << "LE description: " << le.getDescription() << endl;
00111       //cout << "LE X: " << le.x << " = 0? " << (le.x == 0) << endl;
00112       //cout << "LE Y: " << le.y << " = 0? " << (le.y == 0) << endl;
00113       //cout << "LE A: " << le.a << " = 0? " << (le.a == 0) << endl;
00114       if (le.x == 0 && le.y == 0 && le.a == 0) {
00115         //cout << "Posting Completion Event for Walk." << endl;
00116         postCompletionEvent();
00117       }
00118     }
00119         }
00120 
00121 
00122   //! removes #walkid if #walkidIsMine
00123   virtual void teardown() {
00124     if(walkidIsMine) {
00125       motman->removeMotion(walkid);
00126       walkid=MotionManager::invalid_MC_ID;
00127     }
00128     StateNode::teardown();
00129   }
00130 
00131   //! use this to force the WalkNode to use a shared WalkMC - set to MotionManager::invalid_MC_ID to reset to internally generated walk
00132   virtual void setWalkID(MotionManager::MC_ID id) {
00133     if(walkidIsMine) {
00134       motman->removeMotion(walkid);
00135       walkid=MotionManager::invalid_MC_ID;
00136     }
00137     erouter->removeListener(this, EventBase::locomotionEGID);
00138     walkid=id;
00139     walkidIsMine=(id==MotionManager::invalid_MC_ID);
00140     erouter->addListener(this, EventBase::locomotionEGID, walkid, EventBase::statusETID);
00141   }
00142 
00143   //! use this to access the WalkMC that the WalkNode is using
00144   virtual MotionManager::MC_ID getWalkID() { return walkid; }
00145 
00146   //! returns true if #walkid was created (and will be destroyed) by this WalkNode - false if assigned by setWalkID()
00147   virtual bool ownsWalkID() { return walkidIsMine; }
00148 
00149 protected:
00150   //! stores the values and if active, calls updateWMC()
00151   void storeValues(float xp, float yp, float ap, int np, WalkMode_t wmode) {
00152     x = xp;
00153     y = yp;
00154     a = ap;
00155     n = np;
00156     walkMode = wmode;
00157 
00158     if (isActive()) {
00159       updateWMC();
00160     }
00161   }
00162 
00163   //! makes the appropriate calls on the WalkMC
00164   void updateWMC() {
00165     if(walkid==MotionManager::invalid_MC_ID) {
00166       SharedObject<WalkMC> walk;
00167       MotionManager::MC_ID id = motman->addPersistentMotion(walk);
00168       setWalkID(id);
00169       walkidIsMine=true;
00170     }
00171     MMAccessor<WalkMC> walk(walkid);
00172     switch(walkMode) {
00173     case VelocityWalkMode:
00174       walk->setTargetVelocity(x,y,a,n);
00175       break;
00176     case DistanceWalkMode:
00177       walk->setTargetDisplacement(x,y,a,n); // WalkMC will calculate velocities.
00178       break;
00179     default:
00180       std::cout << "Unknown Walk Mode" << std::endl;
00181       break;
00182     }
00183   }
00184 
00185   /*
00186   void updateWMC() {
00187     if(walkid==MotionManager::invalid_MC_ID) {
00188       SharedObject<WalkMC> walk;
00189       switch(walkMode) {
00190       case VelocityWalkMode:
00191   walk->setTargetVelocity(x,y,a,n);
00192   break;
00193       case DistanceWalkMode:
00194   walk->setTargetDisplacement(x,y,a,n); // WalkMC will calculate velocities.
00195   break;
00196       default:
00197   cout << "Unknown Walk Mode" << endl;
00198   break;
00199       }
00200       MotionManager::MC_ID id = motman->addPersistentMotion(walk);
00201       setWalkID(id);
00202       walkidIsMine=true;
00203     } else {
00204       MMAccessor<WalkMC> walk(walkid);
00205       switch(walkMode) {
00206       case VelocityWalkMode:
00207   walk->setTargetVelocity(x,y,a,n);
00208   break;
00209       case DistanceWalkMode:
00210   walk->setTargetDisplacement(x,y,a,n); // WalkMC will calculate velocities.
00211   break;
00212       default:
00213   cout << "Unknown Walk Mode" << endl;
00214   break;
00215       }
00216     }
00217   }
00218   */
00219 
00220   /*
00221   //!if the walk is invalid, create; then set xya
00222   void updateWalk(float xp, float yp, float ap, int np=-1, bool isVelocity=true) {
00223     cout << "NNNNNNNNNPPPPPPPPP: " << np << endl;
00224     vector3d velocities;
00225     if(walkid==MotionManager::invalid_MC_ID) {
00226       SharedObject<WalkMC> walk;
00227       if (isVelocity)
00228         walk->setTargetVelocity(xp,yp,ap,np);
00229       else
00230         walk->setTargetDisplacement(xp,yp,ap,np); // WalkMC will calculate velocities.
00231       velocities = walk->getCurVelocity();
00232       MotionManager::MC_ID id = motman->addPersistentMotion(walk);
00233       setWalkID(id);
00234       walkidIsMine=true;
00235     } else {
00236       MMAccessor<WalkMC> walk(walkid);
00237       if (isVelocity)
00238         walk->setTargetVelocity(xp,yp,ap,np);
00239       else
00240         walk->setTargetDisplacement(xp,yp,ap,np); // WalkMC will calculate velocities.
00241 
00242       velocities = walk->getCurVelocity();  // extract the velocities WalkMC calculated
00243     }
00244     
00245     x = velocities.x;
00246     y = velocities.y;
00247     a = velocities.z;
00248     n = np;
00249   }
00250   */
00251 
00252   MotionManager::MC_ID walkid; //!< the current WalkMC
00253   bool walkidIsMine; //!< true if the walk was created in updateWalk (instead of assigned externally)
00254   float x; //!< velocity in x direction (positive is forward), or distance if #walkMode is DistanceWalkMode
00255   float y; //!< velocity in y direction (positive is dog's left), or distance if #walkMode is DistanceWalkMode
00256   float a; //!< velocity of the turn, positive is counter-clockwise from above (to match coordinate system), or distance if #walkMode is DistanceWalkMode
00257   int n; //!< number of steps (-1 means walk forever)
00258 
00259   WalkMode_t walkMode; //!< the current interpretation of #x, #y, and #a
00260 };
00261 
00262 /*! @file
00263  * @brief Describes WalkNode, a  StateNode for walking in a direction
00264  * @author ejt (Creator)
00265  *
00266  * $Author: ejt $
00267  * $Name: tekkotsu-2_4_1 $
00268  * $Revision: 1.15 $
00269  * $State: Exp $
00270  * $Date: 2005/08/07 04:11:03 $
00271  */
00272 
00273 #endif

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:49 2005 by Doxygen 1.4.4