00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #ifndef __MotionRatingBehaviorControl_h_
00013 #define __MotionRatingBehaviorControl_h_
00014
00015 #include "../BehaviorControl.h"
00016 #include "Tools/Module/SolutionRequest.h"
00017
00018 #include "MotionRatingBehaviorMessage.h"
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 class MotionRatingBehaviorControl : public BehaviorControl
00030 {
00031 public:
00032
00033
00034
00035
00036 MotionRatingBehaviorControl(BehaviorControlInterfaces& interfaces);
00037
00038
00039 ~MotionRatingBehaviorControl();
00040
00041 virtual bool handleMessage(InMessage& message);
00042 virtual void execute();
00043
00044 protected:
00045 enum { sleep, localise, prepare, wait, localisepre, walk, rotate, stop, localiseaft, report };
00046
00047 unsigned long pingTimeout;
00048 unsigned long lastMessage;
00049
00050 int path;
00051 int state;
00052 int paramSetId;
00053 int walkCount;
00054 int currentWalk;
00055 bool paused;
00056
00057 double distTolerance;
00058 double angleTolerance;
00059
00060 struct Track {
00061 Pose2D start;
00062 Pose2D end;
00063 };
00064
00065 int pathOffset;
00066
00067 Track track;
00068
00069 Pose2D startpoint, endpoint;
00070
00071 unsigned long starttime;
00072 unsigned long endtime;
00073 unsigned long startlocalise;
00074 unsigned long startrotate;
00075
00076 double totalDistance;
00077 unsigned long totalTime;
00078
00079 Vector2<double> p1, p2;
00080
00081 bool showMessages;
00082
00083 private:
00084
00085 void reset();
00086 void setPoints();
00087
00088 bool gotoPoint ( Pose2D, bool );
00089 bool gotoPoint ( Vector2<double>, bool );
00090 void setWalkMotionRequest ( double distance, double phi, double theta, bool safe );
00091
00092 bool stand();
00093 void playDead();
00094
00095 bool doLocalise();
00096
00097 MotionRatingBehaviorMessage::States encodeState();
00098
00099
00100
00101
00102 };
00103
00104 #endif// __MotionRatingBehaviorControl_h_
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114