00001
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
00013 class WalkNode : public StateNode {
00014 public:
00015
00016 enum WalkMode_t {
00017 VelocityWalkMode,
00018 DistanceWalkMode
00019 };
00020
00021 public:
00022
00023 WalkNode()
00024 : StateNode("WalkNode"), walkid(MotionManager::invalid_MC_ID), walkidIsMine(true), x(0), y(0), a(0), n(-1), walkMode()
00025 {}
00026
00027
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
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
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
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
00048 ~WalkNode() {
00049 if(issetup)
00050 teardown();
00051 }
00052
00053
00054 void setDisplacement(float xd, float yd, float ad, int np = -1) {
00055
00056
00057 storeValues(xd, yd, ad, np, DistanceWalkMode);
00058 }
00059
00060
00061 void setVelocity(float xvel, float yvel, float avel, int np = -1) {
00062
00063 storeValues(xvel, yvel, avel, np, VelocityWalkMode);
00064 }
00065
00066
00067 void setXVelocity(float xvel) { x=xvel; storeValues(xvel, y, a, n, VelocityWalkMode); }
00068
00069
00070 float getXVelocity() { return x; }
00071
00072
00073 void setYVelocity(float yvel) { y=yvel; storeValues(x, yvel, a, n, VelocityWalkMode); }
00074
00075
00076 float getYVelocity() { return y; }
00077
00078
00079 void setAVelocity(float avel) { a=avel; storeValues(x, y, avel, n, VelocityWalkMode); }
00080
00081
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
00095
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
00102 StateNode::DoStop();
00103 }
00104
00105
00106 virtual void processEvent(const EventBase& e) {
00107
00108 if (e.getGeneratorID() == EventBase::locomotionEGID) {
00109 const LocomotionEvent le = *reinterpret_cast<const LocomotionEvent*>(&e);
00110
00111
00112
00113
00114 if (le.x == 0 && le.y == 0 && le.a == 0) {
00115
00116 postCompletionEvent();
00117 }
00118 }
00119 }
00120
00121
00122
00123 virtual void teardown() {
00124 if(walkidIsMine) {
00125 motman->removeMotion(walkid);
00126 walkid=MotionManager::invalid_MC_ID;
00127 }
00128 StateNode::teardown();
00129 }
00130
00131
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
00144 virtual MotionManager::MC_ID getWalkID() { return walkid; }
00145
00146
00147 virtual bool ownsWalkID() { return walkidIsMine; }
00148
00149 protected:
00150
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
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);
00178 break;
00179 default:
00180 std::cout << "Unknown Walk Mode" << std::endl;
00181 break;
00182 }
00183 }
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
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;
00253 bool walkidIsMine;
00254 float x;
00255 float y;
00256 float a;
00257 int n;
00258
00259 WalkMode_t walkMode;
00260 };
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273 #endif