00001
00002 #ifndef INCLUDED_WorldStateVelDaemon_h_
00003 #define INCLUDED_WorldStateVelDaemon_h_
00004
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Events/EventRouter.h"
00007 #include "Events/LocomotionEvent.h"
00008 #include "Shared/WorldState.h"
00009 #include "Events/EventTrapper.h"
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 class WorldStateVelDaemon : public BehaviorBase, public EventTrapper {
00020 public:
00021
00022 WorldStateVelDaemon() : BehaviorBase(), estopTime(1), old_x(0), old_y(0), old_a(0) {}
00023
00024 virtual void DoStart() {
00025 BehaviorBase::DoStart();
00026 erouter->addTrapper(this,EventBase::locomotionEGID);
00027 erouter->addListener(this,EventBase::estopEGID);
00028 }
00029
00030 virtual void DoStop() {
00031 erouter->forgetListener(this);
00032 BehaviorBase::DoStop();
00033 }
00034
00035
00036 virtual bool trapEvent(const EventBase& e) {
00037 const LocomotionEvent& le=dynamic_cast<const LocomotionEvent&>(e);
00038 old_x=le.x;
00039 old_y=le.y;
00040 old_a=le.a;
00041 if(!estopTime) {
00042 state->vel_x=le.x;
00043 state->vel_y=le.y;
00044 state->vel_a=le.a;
00045 state->vel_time=le.getTimeStamp();
00046 return false;
00047 }
00048 return true;
00049 }
00050
00051 virtual void processEvent(const EventBase& e) {
00052 if(e.getTypeID()==EventBase::deactivateETID) {
00053 if(estopTime) {
00054 estopTime=0;
00055 LocomotionEvent *le = new LocomotionEvent(EventBase::locomotionEGID,e.getSourceID(),EventBase::statusETID,e.getTimeStamp()-state->vel_time);
00056 le->setXYA(old_x,old_y,old_a);
00057 erouter->postEvent(le);
00058 }
00059 } else {
00060 if(!estopTime) {
00061 float older_x=old_x;
00062 float older_y=old_y;
00063 float older_a=old_a;
00064 erouter->postEvent(new LocomotionEvent(EventBase::locomotionEGID,e.getSourceID(),EventBase::statusETID,e.getTimeStamp()-state->vel_time));
00065 estopTime=e.getTimeStamp();
00066 old_x=older_x;
00067 old_y=older_y;
00068 old_a=older_a;
00069 }
00070 }
00071 }
00072
00073 virtual std::string getName() const { return "WorldStateVelDaemon"; }
00074
00075 static std::string getClassDescription() { return "Keeps the WorldState's velocity fields up to date"; }
00076
00077 protected:
00078 unsigned int estopTime;
00079 float old_x;
00080 float old_y;
00081 float old_a;
00082 };
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095 #endif