00001
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
00012
00013
00014
00015 template<class ParticleT>
00016 class DeadReckoningBehavior : public BehaviorBase, public HolonomicMotionModel<ParticleT> {
00017 public:
00018
00019 explicit DeadReckoningBehavior(const std::string& name="DeadReckoningBehavior") : BehaviorBase(name), HolonomicMotionModel<ParticleT>() {}
00020
00021
00022 DeadReckoningBehavior(float xVariance, float yVariance, float aVariance)
00023 : BehaviorBase("DeadReckoningBehavior"), HolonomicMotionModel<ParticleT>(xVariance,yVariance,aVariance) {}
00024
00025 virtual void DoStart() {
00026 BehaviorBase::DoStart();
00027 HolonomicMotionModel<ParticleT>::setVelocity(state->vel_x, state->vel_y, state->vel_a);
00028 erouter->addListener(this, EventBase::locomotionEGID );
00029
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
00050 DeadReckoningBehavior(const std::string& classname, const std::string& instancename) : BehaviorBase(classname, instancename), HolonomicMotionModel<ParticleT>() {}
00051 };
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064 #endif