MotionStressTestBehavior.hGo to the documentation of this file.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<SmallMotionSequenceMC> ms;
00021 ms->setTime(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
00064 void addMS(LegOrder_t leg,unsigned int delay=0) {
00065 unsigned int index=leg*JointsPerLeg+RotatorOffset;
00066 SharedObject<SmallMotionSequenceMC> ms;
00067 ms->setTime(delay);
00068 ms->setOutputCmd(index,outputRanges[index][MaxRange]);
00069 ms->advanceTime(2000);
00070 ms->setOutputCmd(index,outputRanges[index][MinRange]);
00071 ms->advanceTime(4000);
00072 ms->setOutputCmd(index,outputRanges[index][MaxRange]);
00073 MotionManager::MC_ID id=motman->addPrunableMotion(ms);
00074 curMotions.push(id);
00075 cout << get_time() << "\tAdded id " << id << endl;
00076 }
00077
00078 static std::string getClassDescription() { return "uses a separate MotionCommand for each of several joints to test for region leaks"; }
00079 virtual std::string getDescription() const { return getClassDescription(); }
00080
00081 protected:
00082 LegOrder_t nextLeg;
00083 std::queue<MotionManager::MC_ID> curMotions;
00084 };
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097 #endif
|