Homepage Demos Overview Downloads Tutorials Reference
Credits

AutoGetupBehavior.h

Go to the documentation of this file.
00001 //-*-c++-*-
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 "Shared/SharedObject.h"
00009 #include "Motion/MotionManager.h"
00010 #include "Motion/MotionSequenceMC.h"
00011 #include "Shared/Config.h"
00012 #include "SoundPlay/SoundManager.h"
00013 
00014 //! a little background behavior to keep the robot on its feet
00015 class AutoGetupBehavior : public BehaviorBase {
00016 public:
00017   //! constructor
00018   AutoGetupBehavior() : BehaviorBase(), back(0), side(0), gamma(.9), sensitivity(.85*.85), waiting(false) {}
00019   //! destructor
00020   virtual ~AutoGetupBehavior() {}
00021 
00022   //! Listens for the SensorSourceID::UpdatedSID
00023   virtual void DoStart() {
00024     BehaviorBase::DoStart();
00025     erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID);
00026   }
00027   //! Stops listening for events
00028   virtual void DoStop() {
00029     erouter->removeListener(this);
00030     BehaviorBase::DoStop();
00031   }
00032   //! Run appropriate motion script if the robot falls over
00033   virtual void processEvent(const EventBase &event) {
00034     if(event.getGeneratorID()==EventBase::motmanEGID) {
00035       //previous attempt at getting up has completed
00036       cout << "Getup complete" << 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       //fallen down
00045       cout << "I've fallen!" << endl;
00046       sndman->PlayFile("yipper.wav");
00047       std::string gu;
00048       //config->motion.makePath will return a path relative to config->motion.root (from config file read at boot)
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< MotionSequenceMC<MotionSequence::SizeMedium> > 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   virtual std::string getName() const { return "AutoGetupBehavior"; }
00062   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"; }
00063 
00064 protected:
00065   float back;          //!< exponential average of backwards accel
00066   float side;          //!< exponential average of sideways accel
00067   float gamma;         //!< default 0.9, gamma parameter for exponential average of above
00068   float sensitivity;   //!< default 0.85*0.85, squared threshold to consider having fallen over, use values 0-1
00069   bool  waiting;       //!< true while we're waiting to hear from completion of MotionSequence, won't try again until this is cleared
00070 };
00071 
00072 /*! @file
00073  * @brief Defines AutoGetupBehavior, a little background behavior to keep the robot on its feet
00074  * @author ejt (Creator)
00075  *
00076  * $Author: ejt $
00077  * $Name: tekkotsu-2_2 $
00078  * $Revision: 1.12 $
00079  * $State: Exp $
00080  * $Date: 2004/10/17 01:16:10 $
00081  */
00082 
00083 #endif

Tekkotsu v2.2
Generated Tue Oct 19 14:19:13 2004 by Doxygen 1.3.9.1