00001
00002 #ifndef INCLUDED_StepTest_h_
00003 #define INCLUDED_StepTest_h_
00004
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Motion/WalkMC.h"
00007 #include "Motion/MotionManager.h"
00008 #include "Motion/MMAccessor.h"
00009 #include "Events/TextMsgEvent.h"
00010 #include "Wireless/Socket.h"
00011
00012
00013 class StepTest : public BehaviorBase {
00014 public:
00015
00016 StepTest() : BehaviorBase("StepTest"), walk_id(MotionManager::invalid_MC_ID) {}
00017
00018 virtual void DoStart() {
00019 BehaviorBase::DoStart();
00020 walk_id=motman->addPersistentMotion(SharedObject<WalkMC>());
00021 erouter->addListener(this,EventBase::textmsgEGID);
00022 }
00023
00024 virtual void DoStop() {
00025 motman->removeMotion(walk_id);
00026 walk_id=MotionManager::invalid_MC_ID;
00027 erouter->removeListener(this);
00028 BehaviorBase::DoStop();
00029 }
00030
00031 virtual void processEvent(const EventBase& e) {
00032 const TextMsgEvent * txt = dynamic_cast<const TextMsgEvent*>(&e);
00033 if(txt==NULL)
00034 serr->printf("WARNING: Illegal event in StepTest '%s'\n",e.getName().c_str());
00035 else {
00036 std::string s=txt->getText();
00037 if(s.find("StepTest ")==0) {
00038 float dx,dy,da;
00039 int n;
00040 int numread=sscanf(s.c_str(),"StepTest %f %f %f %d",&dx,&dy,&da,&n);
00041
00042 if(numread==1) {
00043 MMAccessor<WalkMC> walk_acc(walk_id);
00044 walk_acc->setStepThreshold(dx);
00045 } else if(numread<4) {
00046 serr->printf("ERROR: StepTest needs four values 'dx dy da steps'\n");
00047 return;
00048 } else {
00049 MMAccessor<WalkMC> walk_acc(walk_id);
00050 walk_acc->setTargetDisplacement(dx*n,dy*n,da*n,n);
00051 sout->printf("resulting velocity: %f %f %f\n",walk_acc->getTargetVelocity().x,walk_acc->getTargetVelocity().y,walk_acc->getTargetVelocity().z);
00052 }
00053 }
00054 }
00055 }
00056
00057 static std::string getClassDescription() { return "tests taking a certain number of steps"; }
00058 virtual std::string getDescription() const { return getClassDescription(); }
00059
00060 protected:
00061 MotionManager::MC_ID walk_id;
00062 };
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075 #endif