Homepage Demos Overview Downloads Tutorials Reference
Credits

LookForSoundBehavior.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_LookForSoundBehavior_h_
00003 #define INCLUDED_LookForSoundBehavior_h_
00004 
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Motion/HeadPointerMC.h"
00007 #include "Events/DataEvent.h"
00008 
00009 //! Turns head to sound source, estimated by average volume difference between left and right ears
00010 class LookForSoundBehavior : public BehaviorBase {
00011 public:
00012   //! constructor
00013   LookForSoundBehavior() : BehaviorBase(), mc_id(MotionManager::invalid_MC_ID){}
00014 
00015   virtual void DoStart()
00016   {
00017     BehaviorBase::DoStart();
00018 
00019     // You'll listen for sound and move your head
00020     mc_id = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00021     erouter->addListener(this,EventBase::micOSndEGID);
00022   }
00023 
00024   virtual void DoStop()
00025   {
00026     motman->removeMotion(mc_id);
00027     erouter->removeListener(this);
00028     BehaviorBase::DoStop();
00029   }
00030 
00031   virtual void processEvent(const EventBase& event)
00032   {
00033     if( event.getGeneratorID() == EventBase::micOSndEGID) {
00034       // Get to the sound buffer, inevitable warning on line 37
00035       // getData() is not specified for const data
00036       const DataEvent<const OSoundVectorData*> *de = 
00037         reinterpret_cast<const DataEvent<const OSoundVectorData*>*>( &event);
00038 
00039       OSoundVectorData *svd = const_cast<OSoundVectorData*>(de->getData());
00040       const short *d = ( const short *)svd->GetData(0);
00041 
00042       // Measure the energy of both channels
00043       // Samples are interleaved [l,r]
00044       double l = 0, r = 0;
00045       int sz = svd->GetInfo(0)->frameSize;
00046       for( int i = 0 ; i != sz ; i++){
00047         l += abs( d[2*i]);
00048         r += abs( d[2*i+1]);
00049       }
00050 
00051       // If there is sufficient energy coming in
00052       if( l+r > sz*1000.){
00053         MMAccessor<HeadPointerMC> mc(mc_id);
00054         double cur = state->outputs[ERS7Info::HeadOffset+1];
00055         if( r > 1.3*l)
00056           // Move your head righward
00057           mc->setJoints( 0, cur-.2*M_PI/(r/l), 0);
00058         if( l > 1.3*r)
00059           // Move your head leftward
00060           mc->setJoints( 0, cur+.2*M_PI/(l/r), 0);
00061       }
00062     }
00063   }
00064 
00065   virtual std::string getName() const { return "LookForSoundBehavior"; }
00066 
00067   static std::string getClassDescription() { return "Looking for Sound Behavior Class"; }
00068   
00069 protected:
00070   MotionManager::MC_ID mc_id;
00071 };
00072 
00073 #endif

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