DualCoding: Pilot.h Source File

00001 //-*-c++-*-
00002 #ifndef INCLUDED_Pilot_h_
00003 #define INCLUDED_Pilot_h_
00004 
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Motion/MotionManager.h"
00007 
00008 class EventBase;
00009 
00010 namespace DualCoding {
00011 
00012 class ShapeRoot;
00013 
00014 class PilotRequest {
00015 public:
00016 
00017   enum requestType_t {
00018     reactiveWalk,
00019     waypointWalk,
00020     gotoShapeWalk,
00021   } requestType;
00022 
00023   typedef struct Motion {
00024     enum PilotMotionType_t {
00025       transX=0,
00026       transY,
00027       rotate,
00028     } type;
00029 
00030     float val; // val of translation(mm/sec)/rotation(rad/sec)
00031     Motion& operator=(const Motion& other) {
00032       if (&other == this) return *this;
00033       type = other.type;
00034       val = other.val;
00035       return *this;
00036     }
00037 
00038     Motion(const Motion& m) : type(m.type), val(m.val) {}
00039     Motion(PilotMotionType_t t, float s) : type(t), val(s) {}
00040   };
00041 
00042   typedef struct Constraint {
00043     enum PilotConstraintType_t {
00044       coordX,
00045       coordY,
00046       angle,
00047       distance,
00048     } type;
00049 
00050     float val; // holds address of constraint
00051     Constraint& operator=(const Constraint& other) { 
00052       if (&other == this) return *this;
00053       type = other.type;
00054       val = other.val;//(NULL==other.val) ? NULL : &(*other.val);
00055       return *this;
00056     }
00057     Constraint(PilotConstraintType_t t, float v) : type(t), val(v) {}//&v) {}
00058     Constraint(const Constraint& r) : type(r.type), val(r.val) {} //&(*r.val)) {}
00059   };
00060 
00061   unsigned int request_id;
00062   vector<Motion> motions;
00063   vector<Constraint> constraints;
00064 
00065 private:
00066   PilotRequest(requestType_t _reqType, unsigned int _request_id, 
00067          const vector<Motion>& _motions, const vector<Constraint>& _constraints)
00068     : requestType(_reqType), request_id(_request_id), motions(_motions), constraints(_constraints) 
00069   { if (constraints.size() > 2) constraints.resize(2, Constraint(constraints.front()));
00070     if (motions.size() > 2) motions.resize(2, Motion(motions.front())); }
00071   PilotRequest(requestType_t _reqType, unsigned int _request_id, const Motion& _motion, 
00072          const vector<Constraint>& _constraints)
00073     : requestType(_reqType), request_id(_request_id), motions(), constraints(_constraints) 
00074   { if (constraints.size() > 2) constraints.resize(2, Constraint(constraints.front()));
00075     motions.push_back(_motion); }
00076     
00077 public:
00078   // reactive walk
00079   static PilotRequest Rotate(unsigned int _request_id, float speed, const vector<Constraint>& _constraints)
00080   { return PilotRequest(reactiveWalk, _request_id, Motion(Motion::rotate,speed), _constraints); }
00081   static PilotRequest translateX(unsigned int _request_id, float speed, const vector<Constraint>& _constraints)
00082   { return PilotRequest(reactiveWalk, _request_id, Motion(Motion::transX,speed), _constraints); }
00083   static PilotRequest translateY(unsigned int _request_id, float speed, const vector<Constraint>& _constraints)
00084   { return PilotRequest(reactiveWalk, _request_id, Motion(Motion::transY,speed), _constraints); }
00085 
00086   // waypoint walk 
00087   PilotRequest(unsigned int _request_id, const Motion& _motion)
00088     : requestType(waypointWalk), request_id(_request_id), motions(), constraints() { motions.push_back(_motion); }
00089   static PilotRequest Rotate(unsigned int _request_id, float dest)
00090   { return PilotRequest(_request_id, Motion(Motion::rotate,dest)); }
00091   static PilotRequest translateX(unsigned int _request_id, float dest)
00092   { return PilotRequest(_request_id, Motion(Motion::transX,dest)); }
00093   static PilotRequest translateY(unsigned int _request_id, float dest)
00094   { return PilotRequest(_request_id, Motion(Motion::transY,dest)); }
00095 
00096   // gotoShape walk
00097   static PilotRequest GotoShape(unsigned int _req_id)
00098   { return PilotRequest(gotoShapeWalk,_req_id,vector<Motion>(),vector<Constraint>()); }
00099 
00100   
00101   PilotRequest& operator=(const PilotRequest& other) {
00102     if (&other == this) return *this;
00103     requestType = other.requestType;
00104     request_id = other.request_id;
00105     motions = other.motions;
00106     constraints = other.constraints;
00107     return *this;
00108   }
00109 };
00110 
00111 
00112 class Pilot : public BehaviorBase {
00113 private:
00114   vector<PilotRequest> requests;
00115   //  vector<unsigned int> colors;
00116 
00117   void removeCurrentRequest() {
00118     if (!requests.empty())
00119       requests.erase(requests.begin());
00120   }
00121   void executeRequest() {}//;
00122   /*  
00123   void processReactiveWalk(const EventBase& e);
00124   void processWaypointWalk(const EventBase& e);
00125   void processGotoShape(const EventBase& e);
00126   enum RequestIDs {
00127     lookAtID,
00128     trackID,
00129   };
00130   */
00131 public:
00132   //! Constructor
00133   Pilot() : BehaviorBase("Pilot"), requests() {} //;
00134 
00135   //  virtual void DoStart();
00136   //  virtual void DoStop();
00137   //  virtual void processEvent(const ::EventBase &e);
00138   virtual void executeRequest(const PilotRequest &req) {
00139     if (!requests.empty()) {
00140       cout << "Pilot busy\n";
00141       return;
00142     }
00143     requests.push_back(req);
00144     executeRequest();
00145   }
00146   void gotoShape(const ShapeRoot&) {}//;
00147   void turnBy(AngTwoPi){}//;
00148 
00149   //  MotionManager::MC_ID walker_id;
00150   //  MotionManager::MC_ID waypoint_id;
00151 };
00152 
00153 } // namespace
00154 
00155 #endif