Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

DeadReckoningBehavior.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_DeadReckoningBehavior_h_
00003 #define INCLUDED_DeadReckoningBehavior_h_
00004 
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Events/EventRouter.h"
00007 #include "Events/LocomotionEvent.h"
00008 #include "Motion/HolonomicMotionModel.h"
00009 #include "Shared/WorldState.h"
00010 
00011 //! Subscribes to LocomotionEvents and attempts to track robot movement over time using a fairly generic HolonomicMotionModel
00012 /*! Can be used as a ParticleFilter::MotionModel, or as a component of other behaviors.
00013  *
00014  *  If you want a regular report on position, the behavior will output current position on timer events */
00015 template<class ParticleT>
00016 class DeadReckoningBehavior : public BehaviorBase, public HolonomicMotionModel<ParticleT> {
00017 public:
00018   //! constructor
00019   explicit DeadReckoningBehavior(const std::string& name="DeadReckoningBehavior") : BehaviorBase(name), HolonomicMotionModel<ParticleT>() {}
00020   
00021   //! constructor
00022   DeadReckoningBehavior(float xVariance, float yVariance, float aVariance)
00023   : BehaviorBase("DeadReckoningBehavior"), HolonomicMotionModel<ParticleT>(xVariance,yVariance,aVariance) {}
00024   
00025   virtual void DoStart() {
00026     BehaviorBase::DoStart(); // do this first (required)
00027     HolonomicMotionModel<ParticleT>::setVelocity(state->vel_x, state->vel_y, state->vel_a);
00028     erouter->addListener(this, EventBase::locomotionEGID );
00029     //erouter->addTimer(this, 0, 500);
00030   }
00031   
00032   virtual void processEvent(const EventBase& event) {
00033     if (event.getGeneratorID() == EventBase::locomotionEGID) {
00034       const LocomotionEvent &locoevt = dynamic_cast<const LocomotionEvent&>(event);
00035       HolonomicMotionModel<ParticleT>::setVelocity(locoevt.x,locoevt.y,locoevt.a,locoevt.getTimeStamp());
00036     } else if (event.getGeneratorID() == EventBase::timerEGID) {
00037       float tempx;
00038       float tempy;
00039       float tempa;
00040       HolonomicMotionModel<ParticleT>::getPosition(tempx, tempy, tempa);
00041       std::cout << "DEADPOS " << tempx << " " << tempy << " " << tempa << std::endl;
00042     }
00043   }
00044   
00045   static std::string getClassDescription() { return "Subscribes to LocomotionEvents and attempts to track robot movement over time"; }
00046   virtual std::string getDescription() const { return getClassDescription(); }
00047   
00048 protected:
00049   //! constructor, accepts separate classname and instance name parameters from subclass
00050   DeadReckoningBehavior(const std::string& classname, const std::string& instancename) : BehaviorBase(classname, instancename), HolonomicMotionModel<ParticleT>() {}
00051 };
00052 
00053 /*! @file
00054 * @brief Defines DeadReckoningBehavior, which subscribes to LocomotionEvents and attempts to track robot movement over time using a fairly generic HolonomicMotionModel
00055 * @author Ethan Tira-Thompson (ejt) (Creator)
00056 *
00057 * $Author: ejt $
00058 * $Name: tekkotsu-4_0 $
00059 * $Revision: 1.2 $
00060 * $State: Exp $
00061 * $Date: 2007/11/11 23:57:20 $
00062 */
00063 
00064 #endif

Tekkotsu v4.0
Generated Thu Nov 22 00:54:52 2007 by Doxygen 1.5.4