GT2005: Modules/HeadControl/GT2005HeadControl/GT2005HeadControlBasicBehaviors.h Source File

00001 /** 
00002 * @file Modules/HeadControl/GT2005HeadControl/GT2005HeadControlBasicBehaviors.h
00003 *
00004 * Declaration of basic behaviors defined in "basic-behaviors.xml".
00005 *
00006 * @author Martin Lötzsch
00007 */
00008 
00009 #ifndef __GT2005HeadControlBasicBehaviors_h_
00010 #define __GT2005HeadControlBasicBehaviors_h_
00011 
00012 #include "Tools/Xabsl2/Xabsl2Engine/Xabsl2Engine.h"
00013 
00014 
00015 /** 
00016 * @class GT2005HeadControlBasicBehavior
00017 *
00018 * Base class for all basic behaviors in the GT2004HeadControl
00019 *
00020 * @author Martin Lötzsch
00021 */
00022 class GT2005HeadControlBasicBehavior : public Xabsl2BasicBehavior, public HeadControlInterfaces
00023 {
00024 public:
00025 /*
00026 * Constructor.
00027 * @param name The name of the basic behavior
00028 * @param errorHandler Is invoked when errors occur
00029 * @param interfaces The interfaces of the HeadControl module
00030 * @param headControl A reference to the headControl module
00031 * @param headPathPlanner A reference to the head path planner 
00032 * @param lastScanWasLeft A reference to the variable "lastScanWasLeft" in the HeadControl
00033 * @param cameraInfor A reference to the camera info in the head control
00034   */
00035   GT2005HeadControlBasicBehavior(const char* name, 
00036     Xabsl2ErrorHandler& errorHandler,
00037     HeadControlInterfaces interfaces,
00038     GT2005HeadControl& headControl,
00039     GT2005HeadPathPlanner& headPathPlanner,
00040     bool& lastScanWasLeft,
00041     CameraInfo& cameraInfo)
00042     : Xabsl2BasicBehavior(name, errorHandler),
00043     HeadControlInterfaces(interfaces),
00044     headControl(headControl),
00045     headPathPlanner(headPathPlanner),
00046     lastScanWasLeft(lastScanWasLeft),
00047     cameraInfo(cameraInfo)
00048   {}
00049   
00050   /**  Executes the basic behavior. */
00051   virtual void execute() = 0;
00052   
00053 protected:
00054   /** A reference to the GT2004HeadControl */
00055   GT2005HeadControl& headControl;
00056 
00057   /** A reference to the head path planner */
00058   GT2005HeadPathPlanner& headPathPlanner;
00059 
00060   /** A reference to the variable "lastScanWasLeft" in the head control */
00061   bool& lastScanWasLeft;
00062 
00063   /** A reference to a camara info in the head control */
00064   CameraInfo& cameraInfo;
00065   
00066   /** current odometry data of last call to head control */
00067   OdometryData lastOdometryData;
00068   /** time of last good self localization */
00069   unsigned long lastTimeOfGoodSL;
00070 
00071   /** Number of executed scans */
00072   int panCount;
00073 
00074  /** The time when the skill was executed last, used by GrabBallHigh */
00075   unsigned long timeOfLastExecution;
00076 };
00077 
00078 /** 
00079 * Declares a class for a basic behavior in the GT2004HeadControl 
00080 * @param className The name of the basic behavior class
00081 * @param xmlName The string that is used to register the basic behavior at the engine.
00082 */
00083 #define DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(className,xmlName) \
00084   class className : public GT2005HeadControlBasicBehavior \
00085 { \
00086 public:\
00087   className(Xabsl2ErrorHandler& errorHandler, HeadControlInterfaces& interfaces, GT2005HeadControl& headControl,GT2005HeadPathPlanner& headPathPlanner,bool& lastScanWasLeft,CameraInfo& cameraInfo) \
00088   : GT2005HeadControlBasicBehavior(xmlName, errorHandler, interfaces, headControl,headPathPlanner,lastScanWasLeft,cameraInfo) {} \
00089   virtual void execute();\
00090 };  
00091 
00092 /** classes for the basic behaviors which have neither parameters nor additional members */
00093 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookAtBall,"look-at-ball");
00094 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookAtBallAndClosestLandmark, "look-at-ball-and-closest-landmark");
00095 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookAtMostInformativeLandmark, "look-at-most-informative-landmark");
00096 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorBeginBallSearchAtBallPositionSeen, "begin-ball-search-at-seen");
00097 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorBeginBallSearchAtBallPositionCommunicated, "begin-ball-search-at-communicated");
00098 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorBeginBallSearchAtBallPositionPropagated, "begin-ball-search-at-propagated");
00099 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookAroundAtSeenBall, "look-around-at-seen-ball");
00100 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorFindBall,"find-ball");
00101 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorReturnToBall,"return-to-ball");
00102 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorScanBackToBall,"scan-back-to-ball");
00103 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorScanAwayFromBall,"scan-away-from-ball");
00104 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorGrabBall,"grab-ball");
00105 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorGrabBallHigh,"grab-ball-high");
00106 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorReleaseBall,"release-ball");
00107 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorWaitForGrab,"wait-for-grab");
00108 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookAtCloseLandmark,"look-at-close-landmark");
00109 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorSearchForBallLeft,"search-for-ball-left");
00110 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorSearchForBallRight,"search-for-ball-right");
00111 
00112 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorHoldBall,"hold-ball");
00113 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorOpenChallenge,"open-challenge");
00114 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorNone,"none");
00115 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookLeft,"look-left");
00116 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookRight,"look-right");
00117 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorScanForObstacles,"scan-for-obstacles");
00118 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorSearchForLandmarks,"search-for-landmarks");
00119 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorSearchForLandmarksHeadLow,"search-for-landmarks-head-low");
00120 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookAtBluePinkLandmark,"look-at-blue-pink-landmark");
00121 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookStraightAhead,"look-straight-ahead");
00122 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookTowardOpponentGoal,"look-toward-opponent-goal");
00123 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookBetweenFeet,"look-between-feet");
00124 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookToStars,"look-to-stars");
00125 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorSnapAtFinger,"snap-at-finger");
00126 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookParallelToGround,"look-parallel-to-ground");
00127 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorDirect,"direct");
00128 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorStayAsForced,"stay-as-forced");
00129 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorWatchOrigin,"watch-origin");
00130 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorCalibrateHeadSpeed,"calibrate-head-speed");
00131 
00132 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorRealSlowScan,"real-slow-scan");
00133 DECLARE_GT2005_HEADCONTROL_BASIC_BEHAVIOR(GT2005BasicBehaviorLookBetweenFeetForCarriedBall,"look-between-feet-for-carried-ball");
00134 /** The basic behavior "directed-scan-for-obstacles" */
00135 class GT2005BasicBehaviorDirectedScanForObstacles : public GT2005HeadControlBasicBehavior \
00136 { 
00137 public:
00138   /** Constructor */
00139   GT2005BasicBehaviorDirectedScanForObstacles(Xabsl2ErrorHandler& errorHandler, HeadControlInterfaces& interfaces, GT2005HeadControl& headControl,GT2005HeadPathPlanner& headPathPlanner,bool& lastScanWasLeft,CameraInfo& cameraInfo) 
00140   : GT2005HeadControlBasicBehavior("directed-scan-for-obstacles", errorHandler, interfaces, headControl,headPathPlanner,lastScanWasLeft,cameraInfo), leftOrRight(1) {} 
00141 
00142   /** Executes the basic behavior */
00143   virtual void execute();
00144 
00145 private:
00146   double leftOrRight;
00147   double lastPan;
00148 };
00149 
00150 
00151 
00152 /** The basic behavior "directed-scan-for-landmarks" */
00153 class GT2005BasicBehaviorDirectedScanForLandmarks : public GT2005HeadControlBasicBehavior \
00154 { 
00155 public:
00156   /** Constructor */
00157   GT2005BasicBehaviorDirectedScanForLandmarks(Xabsl2ErrorHandler& errorHandler, HeadControlInterfaces& interfaces, GT2005HeadControl& headControl,GT2005HeadPathPlanner& headPathPlanner,bool& lastScanWasLeft,CameraInfo& cameraInfo) 
00158   : GT2005HeadControlBasicBehavior("directed-scan-for-landmarks", errorHandler, interfaces, headControl,headPathPlanner,lastScanWasLeft,cameraInfo), leftOrRight(1) {} 
00159 
00160   /** Executes the basic behavior */
00161   virtual void execute();
00162 
00163   bool nextLandmarkIsWithinReach;
00164 
00165 private:
00166   double leftOrRight;
00167   int currentLandmark;
00168   int nextLandmark;
00169   int waitSomeBeforeLookingAtNextLandmark;
00170 };
00171 
00172 
00173 /** Initializes a GT2005HeadControl basic behavior with the right parameter list */
00174 #define INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehavior) \
00175 basicBehavior(errorHandler, interfaces, headControl, headPathPlanner, lastScanWasLeft, cameraInfo)
00176 
00177 
00178 /**
00179 * @class GT2005HeadControlBasicBehaviors
00180 *
00181 * Creates and registers the basic behaviors
00182 */
00183 class GT2005HeadControlBasicBehaviors
00184 {
00185 public:
00186   /** Constructor */
00187   GT2005HeadControlBasicBehaviors(Xabsl2ErrorHandler& errorHandler, HeadControlInterfaces& interfaces, GT2005HeadControl& headControl,GT2005HeadPathPlanner& headPathPlanner,bool& lastScanWasLeft,CameraInfo& cameraInfo)
00188     : errorHandler(errorHandler), headControl(headControl),
00189     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookAtBall),
00190     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookAtCloseLandmark),
00191     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookAtMostInformativeLandmark),
00192     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookAtBallAndClosestLandmark),
00193     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorBeginBallSearchAtBallPositionSeen),
00194     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorBeginBallSearchAtBallPositionPropagated),
00195     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorBeginBallSearchAtBallPositionCommunicated),
00196     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorFindBall),
00197     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorReturnToBall),
00198     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorScanAwayFromBall),
00199     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorScanBackToBall),
00200     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorGrabBall),
00201     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorGrabBallHigh),
00202     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorReleaseBall),
00203     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorWaitForGrab),
00204     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorDirectedScanForLandmarks),
00205     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorDirectedScanForObstacles),
00206     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookAroundAtSeenBall),
00207     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorSearchForBallLeft),
00208     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorSearchForBallRight),
00209 
00210     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorHoldBall),
00211     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorOpenChallenge),
00212     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorNone),
00213     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookLeft),
00214     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookRight),
00215     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorScanForObstacles),
00216     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorSearchForLandmarks),
00217     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorSearchForLandmarksHeadLow),
00218     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookAtBluePinkLandmark),
00219     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookStraightAhead),
00220     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookTowardOpponentGoal),
00221     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookBetweenFeet),
00222     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookToStars),
00223     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorSnapAtFinger),
00224     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookParallelToGround),
00225     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorDirect),
00226     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorStayAsForced),
00227     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorWatchOrigin),
00228     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorCalibrateHeadSpeed),
00229 
00230     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorRealSlowScan),
00231     INIT_GT2005_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookBetweenFeetForCarriedBall)
00232   {}
00233   
00234   /** Registers basic behaviors at the engine */
00235   void registerBasicBehaviors(Xabsl2Engine& engine);  
00236   
00237   //!@name Basic Behaviors
00238   //!@{
00239   GT2005BasicBehaviorLookAtBall basicBehaviorLookAtBall;
00240   GT2005BasicBehaviorFindBall basicBehaviorFindBall;
00241   GT2005BasicBehaviorReturnToBall basicBehaviorReturnToBall;
00242   GT2005BasicBehaviorScanAwayFromBall basicBehaviorScanAwayFromBall;
00243   GT2005BasicBehaviorDirectedScanForLandmarks basicBehaviorDirectedScanForLandmarks;
00244   GT2005BasicBehaviorDirectedScanForObstacles basicBehaviorDirectedScanForObstacles;
00245   GT2005BasicBehaviorScanBackToBall basicBehaviorScanBackToBall;
00246   GT2005BasicBehaviorGrabBall basicBehaviorGrabBall;
00247   GT2005BasicBehaviorGrabBallHigh basicBehaviorGrabBallHigh;
00248   GT2005BasicBehaviorReleaseBall basicBehaviorReleaseBall;
00249   GT2005BasicBehaviorWaitForGrab basicBehaviorWaitForGrab;
00250   GT2005BasicBehaviorBeginBallSearchAtBallPositionSeen basicBehaviorBeginBallSearchAtBallPositionSeen;
00251   GT2005BasicBehaviorBeginBallSearchAtBallPositionCommunicated basicBehaviorBeginBallSearchAtBallPositionCommunicated;
00252   GT2005BasicBehaviorBeginBallSearchAtBallPositionPropagated basicBehaviorBeginBallSearchAtBallPositionPropagated;
00253   GT2005BasicBehaviorLookAtCloseLandmark basicBehaviorLookAtCloseLandmark;
00254   GT2005BasicBehaviorLookAtBallAndClosestLandmark basicBehaviorLookAtBallAndClosestLandmark;
00255   GT2005BasicBehaviorLookAtMostInformativeLandmark basicBehaviorLookAtMostInformativeLandmark;
00256   GT2005BasicBehaviorLookAroundAtSeenBall basicBehaviorLookAroundAtSeenBall;
00257   GT2005BasicBehaviorSearchForBallLeft basicBehaviorSearchForBallLeft;
00258   GT2005BasicBehaviorSearchForBallRight basicBehaviorSearchForBallRight;
00259 
00260   GT2005BasicBehaviorHoldBall basicBehaviorHoldBall; 
00261   GT2005BasicBehaviorOpenChallenge basicBehaviorOpenChallenge; 
00262   GT2005BasicBehaviorNone basicBehaviorNone; 
00263   GT2005BasicBehaviorLookLeft basicBehaviorLookLeft; 
00264   GT2005BasicBehaviorLookRight basicBehaviorLookRight; 
00265   GT2005BasicBehaviorScanForObstacles basicBehaviorScanForObstacles; 
00266   GT2005BasicBehaviorSearchForLandmarks basicBehaviorSearchForLandmarks; 
00267   GT2005BasicBehaviorSearchForLandmarksHeadLow basicBehaviorSearchForLandmarksHeadLow; 
00268   GT2005BasicBehaviorLookAtBluePinkLandmark basicBehaviorLookAtBluePinkLandmark; 
00269   GT2005BasicBehaviorLookStraightAhead basicBehaviorLookStraightAhead; 
00270   GT2005BasicBehaviorLookTowardOpponentGoal basicBehaviorLookTowardOpponentGoal; 
00271   GT2005BasicBehaviorLookBetweenFeet basicBehaviorLookBetweenFeet; 
00272   GT2005BasicBehaviorLookToStars basicBehaviorLookToStars; 
00273   GT2005BasicBehaviorSnapAtFinger basicBehaviorSnapAtFinger; 
00274   GT2005BasicBehaviorLookParallelToGround basicBehaviorLookParallelToGround; 
00275   GT2005BasicBehaviorDirect basicBehaviorDirect; 
00276   GT2005BasicBehaviorStayAsForced basicBehaviorStayAsForced; 
00277   GT2005BasicBehaviorWatchOrigin basicBehaviorWatchOrigin; 
00278   GT2005BasicBehaviorCalibrateHeadSpeed basicBehaviorCalibrateHeadSpeed; 
00279 
00280   GT2005BasicBehaviorRealSlowScan basicBehaviorRealSlowScan; 
00281   GT2005BasicBehaviorLookBetweenFeetForCarriedBall basicBehaviorLookBetweenFeetForCarriedBall; 
00282   //!@}
00283 
00284 private:
00285   /** Is invoked when errors occurs */
00286   Xabsl2ErrorHandler& errorHandler;
00287   
00288   /** A reference to the GT2004HeadControl */
00289   GT2005HeadControl& headControl;
00290 };
00291 
00292 #endif // __GT2004HeadControlBasicBehaviors_h_
00293