00001
00002 #ifndef INCLUDED_AutoGetupBehavior_h_
00003 #define INCLUDED_AutoGetupBehavior_h_
00004
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Shared/WorldState.h"
00007 #include "Events/EventRouter.h"
00008 #include "IPC/SharedObject.h"
00009 #include "Motion/MotionManager.h"
00010 #include "Motion/MotionSequenceMC.h"
00011 #include "Shared/Config.h"
00012 #include "Sound/SoundManager.h"
00013
00014
00015 class AutoGetupBehavior : public BehaviorBase {
00016 public:
00017
00018 AutoGetupBehavior() : BehaviorBase("AutoGetupBehavior"), back(0), side(0), gamma(.9), sensitivity(.85*.85), waiting(false) {}
00019
00020 virtual ~AutoGetupBehavior() {}
00021
00022
00023 virtual void DoStart() {
00024 BehaviorBase::DoStart();
00025 erouter->addListener(this,EventBase::sensorEGID,SensorSrcID::UpdatedSID);
00026 }
00027
00028 virtual void DoStop() {
00029 erouter->removeListener(this);
00030 BehaviorBase::DoStop();
00031 }
00032
00033 virtual void processEvent(const EventBase &event) {
00034 if(event.getGeneratorID()==EventBase::motmanEGID) {
00035
00036 std::cout << "Getup complete" << std::endl;
00037 erouter->removeListener(this,EventBase::motmanEGID);
00038 waiting=false;
00039 return;
00040 }
00041 back=back*gamma+(1-gamma)*state->sensors[BAccelOffset];
00042 side=side*gamma+(1-gamma)*state->sensors[LAccelOffset];
00043 if(!waiting && back*back+side*side>sensitivity*WorldState::g*WorldState::g) {
00044
00045 std::cout << "I've fallen!" << std::endl;
00046 sndman->playFile("yipper.wav");
00047 std::string gu;
00048
00049 if(fabs(back)<fabs(side))
00050 gu=config->motion.makePath("gu_side.mot");
00051 else if(back<0)
00052 gu=config->motion.makePath("gu_back.mot");
00053 else
00054 gu=config->motion.makePath("gu_front.mot");
00055 SharedObject<MediumMotionSequenceMC> getup(gu.c_str());
00056 MotionManager::MC_ID id=motman->addPrunableMotion(getup,MotionManager::kHighPriority);
00057 erouter->addListener(this,EventBase::motmanEGID,id,EventBase::deactivateETID);
00058 waiting=true;
00059 }
00060 }
00061 static std::string getClassDescription() { return "Monitors gravity's influence on the accelerometers - if it seems the robot has fallen over, it runs appropriate getup script"; }
00062 virtual std::string getDescription() const { return getClassDescription(); }
00063
00064 protected:
00065 float back;
00066 float side;
00067 float gamma;
00068 float sensitivity;
00069 bool waiting;
00070 };
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083 #endif