00001
00002 #ifndef INCLUDED_MotionStressTestBehavior_h_
00003 #define INCLUDED_MotionStressTestBehavior_h_
00004
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Motion/MotionManager.h"
00007 #include "Motion/MotionSequenceMC.h"
00008 #include <queue>
00009
00010
00011
00012 class MotionStressTestBehavior : public BehaviorBase {
00013 public:
00014
00015 MotionStressTestBehavior() : BehaviorBase("MotionStressTestBehavior"), nextLeg(RBkLegOrder), curMotions() {}
00016
00017 virtual void DoStart() {
00018 BehaviorBase::DoStart();
00019
00020 SharedObject<MotionSequenceMC<MotionSequence::SizeSmall> > ms;
00021 ms->setPlayTime(3000);
00022 ms->setOutputCmd(LFrLegOffset+ElevatorOffset,outputRanges[LFrLegOffset+ElevatorOffset][MaxRange]);
00023 ms->setOutputCmd(RFrLegOffset+ElevatorOffset,outputRanges[RFrLegOffset+ElevatorOffset][MaxRange]);
00024 ms->setOutputCmd(LBkLegOffset+ElevatorOffset,outputRanges[LBkLegOffset+ElevatorOffset][MaxRange]);
00025 ms->setOutputCmd(RBkLegOffset+ElevatorOffset,outputRanges[RBkLegOffset+ElevatorOffset][MaxRange]);
00026 ms->setOutputCmd(LFrLegOffset+KneeOffset,0);
00027 ms->setOutputCmd(RFrLegOffset+KneeOffset,0);
00028 ms->setOutputCmd(LBkLegOffset+KneeOffset,0);
00029 ms->setOutputCmd(RBkLegOffset+KneeOffset,0);
00030 for(unsigned int i=HeadOffset; i<HeadOffset+NumHeadJoints; i++)
00031 ms->setOutputCmd(i,0);
00032 MotionManager::MC_ID id=motman->addPrunableMotion(ms);
00033 curMotions.push(id);
00034 cout << get_time() << "\tAdded id " << id << endl;
00035 addMS(LFrLegOrder,3000);
00036 addMS(RFrLegOrder,4000);
00037 addMS(LBkLegOrder,5000);
00038 erouter->addListener(this,EventBase::motmanEGID);
00039 }
00040
00041 virtual void DoStop() {
00042 erouter->removeListener(this);
00043 while(!curMotions.empty()) {
00044 motman->removeMotion(curMotions.front());
00045 curMotions.pop();
00046 }
00047 BehaviorBase::DoStop();
00048 }
00049
00050 virtual void processEvent(const EventBase& e) {
00051 if(e.getTypeID()==EventBase::deactivateETID) {
00052 if(e.getSourceID()!=curMotions.front()) {
00053 cout << e.getSourceID() << " is not mine or is out of order" << endl;
00054 } else {
00055 curMotions.pop();
00056 }
00057 cout << get_time() << "\t Removed id " << e.getSourceID() << endl;
00058 addMS(nextLeg,3000);
00059 nextLeg=static_cast<LegOrder_t>((nextLeg+1)%4);
00060 }
00061 }
00062
00063 void addMS(LegOrder_t leg,unsigned int delay=0) {
00064 unsigned int index=leg*JointsPerLeg+RotatorOffset;
00065 SharedObject<MotionSequenceMC<MotionSequence::SizeSmall> > ms;
00066 ms->setPlayTime(delay);
00067 ms->setOutputCmd(index,outputRanges[index][MaxRange]);
00068 ms->setPlayTime(delay+2000);
00069 ms->setOutputCmd(index,outputRanges[index][MinRange]);
00070 ms->setPlayTime(delay+4000);
00071 ms->setOutputCmd(index,outputRanges[index][MaxRange]);
00072 MotionManager::MC_ID id=motman->addPrunableMotion(ms);
00073 curMotions.push(id);
00074 cout << get_time() << "\tAdded id " << id << endl;
00075 }
00076
00077 static std::string getClassDescription() { return "uses a separate MotionCommand for each of several joints to test for region leaks"; }
00078 virtual std::string getDescription() const { return getClassDescription(); }
00079
00080 protected:
00081 LegOrder_t nextLeg;
00082 std::queue<MotionManager::MC_ID> curMotions;
00083 };
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096 #endif