GT2004: Modules/HeadControl/GT2004HeadControl/GT2004HeadControlBasicBehaviors.h Source File

00001 /** 
00002 * @file GT2004HeadControlBasicBehaviors.h
00003 *
00004 * Declaration of basic behaviors defined in "basic-behaviors.xml".
00005 *
00006 * @author Martin Lötzsch
00007 */
00008 
00009 #ifndef __GT2004HeadControlBasicBehaviors_h_
00010 #define __GT2004HeadControlBasicBehaviors_h_
00011 
00012 #include "Tools/Xabsl2/Xabsl2Engine/Xabsl2Engine.h"
00013 
00014 
00015 /** 
00016 * @class GT2004HeadControlBasicBehavior
00017 *
00018 * Base class for all basic behaviors in the GT2004HeadControl
00019 *
00020 * @author Martin Lötzsch
00021 */
00022 class GT2004HeadControlBasicBehavior : 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   GT2004HeadControlBasicBehavior(const char* name, 
00036     Xabsl2ErrorHandler& errorHandler,
00037     HeadControlInterfaces interfaces,
00038     GT2004HeadControl& headControl,
00039     GT2004HeadPathPlanner& 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   GT2004HeadControl& headControl;
00056 
00057   /** A reference to the head path planner */
00058   GT2004HeadPathPlanner& 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 
00075 /** 
00076 * Declares a class for a basic behavior in the GT2004HeadControl 
00077 * @param className The name of the basic behavior class
00078 * @param xmlName The string that is used to register the basic behavior at the engine.
00079 */
00080 #define DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(className,xmlName) \
00081   class className : public GT2004HeadControlBasicBehavior \
00082 { \
00083 public:\
00084   className(Xabsl2ErrorHandler& errorHandler, HeadControlInterfaces& interfaces, GT2004HeadControl& headControl,GT2004HeadPathPlanner& headPathPlanner,bool& lastScanWasLeft,CameraInfo& cameraInfo) \
00085   : GT2004HeadControlBasicBehavior(xmlName, errorHandler, interfaces, headControl,headPathPlanner,lastScanWasLeft,cameraInfo) {} \
00086   virtual void execute();\
00087 };  
00088 
00089 /** classes for the basic behaviors which have neither parameters nor additional members */
00090 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorOtherHeadMovements,"other-head-movements");
00091 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorLookAtBall,"look-at-ball");
00092 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorLookAtBallAndClosestLandmark, "look-at-ball-and-closest-landmark");
00093 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorBeginBallSearchAtBallPositionSeen, "begin-ball-search-at-seen");
00094 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorBeginBallSearchAtBallPositionCommunicated, "begin-ball-search-at-communicated");
00095 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorBeginBallSearchAtBallPositionPropagated, "begin-ball-search-at-propagated");
00096 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorLookAroundAtSeenBall, "look-around-at-seen-ball");
00097 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorFindBall,"find-ball");
00098 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorReturnToBall,"return-to-ball");
00099 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorScanBackToBall,"scan-back-to-ball");
00100 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorScanAwayFromBall,"scan-away-from-ball");
00101 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorGrabBall,"grab-ball");
00102 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorReleaseBall,"release-ball");
00103 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorWaitForGrab,"wait-for-grab");
00104 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorLookAtCloseLandmark,"look-at-close-landmark");
00105 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorSearchForBallLeft,"search-for-ball-left");
00106 DECLARE_GT2004_HEADCONTROL_BASIC_BEHAVIOR(GT2004BasicBehaviorSearchForBallRight,"search-for-ball-right");
00107 
00108 
00109 /** The basic behavior "look-at-close-landmarks" */
00110 class GT2004BasicBehaviorDirectedScanForLandmarks : public GT2004HeadControlBasicBehavior \
00111 { 
00112 public:
00113   /** Constructor */
00114   GT2004BasicBehaviorDirectedScanForLandmarks(Xabsl2ErrorHandler& errorHandler, HeadControlInterfaces& interfaces, GT2004HeadControl& headControl,GT2004HeadPathPlanner& headPathPlanner,bool& lastScanWasLeft,CameraInfo& cameraInfo) 
00115   : GT2004HeadControlBasicBehavior("directed-scan-for-landmarks", errorHandler, interfaces, headControl,headPathPlanner,lastScanWasLeft,cameraInfo), leftOrRight(1) {} 
00116 
00117   /** Executes the basic behavior */
00118   virtual void execute();
00119 
00120   bool nextLandmarkIsWithinReach;
00121 
00122 private:
00123   double leftOrRight;
00124   int currentLandmark;
00125   int nextLandmark;
00126   int waitSomeBeforeLookingAtNextLandmark;
00127 };
00128 
00129 
00130 /** Initializes a GT2004HeadControl basic behavior with the right parameter list */
00131 #define INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehavior) \
00132 basicBehavior(errorHandler, interfaces, headControl, headPathPlanner, lastScanWasLeft, cameraInfo)
00133 
00134 
00135 /**
00136 * @class GT2004HeadControlBasicBehaviors
00137 *
00138 * Creates and registers the basic behaviors
00139 */
00140 class GT2004HeadControlBasicBehaviors
00141 {
00142 public:
00143   /** Constructor */
00144   GT2004HeadControlBasicBehaviors(Xabsl2ErrorHandler& errorHandler, HeadControlInterfaces& interfaces, GT2004HeadControl& headControl,GT2004HeadPathPlanner& headPathPlanner,bool& lastScanWasLeft,CameraInfo& cameraInfo)
00145     : errorHandler(errorHandler), headControl(headControl),
00146     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookAtBall),
00147     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookAtCloseLandmark),
00148     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookAtBallAndClosestLandmark),
00149     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorBeginBallSearchAtBallPositionSeen),
00150     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorBeginBallSearchAtBallPositionPropagated),
00151     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorBeginBallSearchAtBallPositionCommunicated),
00152     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorFindBall),
00153     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorReturnToBall),
00154     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorScanAwayFromBall),
00155     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorScanBackToBall),
00156     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorGrabBall),
00157     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorReleaseBall),
00158     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorWaitForGrab),
00159     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorOtherHeadMovements),
00160     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorDirectedScanForLandmarks),
00161     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorLookAroundAtSeenBall),
00162     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorSearchForBallLeft),
00163     INIT_GT2004_HEADCONTROL_BASIC_BEHAVIOR(basicBehaviorSearchForBallRight)
00164   {}
00165   
00166   /** Registers basic behaviors at the engine */
00167   void registerBasicBehaviors(Xabsl2Engine& engine);  
00168   
00169   //!@name Basic Behaviors
00170   //!@{
00171   GT2004BasicBehaviorLookAtBall basicBehaviorLookAtBall;
00172   GT2004BasicBehaviorFindBall basicBehaviorFindBall;
00173   GT2004BasicBehaviorReturnToBall basicBehaviorReturnToBall;
00174   GT2004BasicBehaviorScanAwayFromBall basicBehaviorScanAwayFromBall;
00175   GT2004BasicBehaviorDirectedScanForLandmarks basicBehaviorDirectedScanForLandmarks;
00176   GT2004BasicBehaviorScanBackToBall basicBehaviorScanBackToBall;
00177   GT2004BasicBehaviorGrabBall basicBehaviorGrabBall;
00178   GT2004BasicBehaviorReleaseBall basicBehaviorReleaseBall;
00179   GT2004BasicBehaviorWaitForGrab basicBehaviorWaitForGrab;
00180   GT2004BasicBehaviorBeginBallSearchAtBallPositionSeen basicBehaviorBeginBallSearchAtBallPositionSeen;
00181   GT2004BasicBehaviorBeginBallSearchAtBallPositionCommunicated basicBehaviorBeginBallSearchAtBallPositionCommunicated;
00182   GT2004BasicBehaviorBeginBallSearchAtBallPositionPropagated basicBehaviorBeginBallSearchAtBallPositionPropagated;
00183   GT2004BasicBehaviorOtherHeadMovements basicBehaviorOtherHeadMovements;
00184   GT2004BasicBehaviorLookAtCloseLandmark basicBehaviorLookAtCloseLandmark;
00185   GT2004BasicBehaviorLookAtBallAndClosestLandmark basicBehaviorLookAtBallAndClosestLandmark;
00186   GT2004BasicBehaviorLookAroundAtSeenBall basicBehaviorLookAroundAtSeenBall;
00187   GT2004BasicBehaviorSearchForBallLeft basicBehaviorSearchForBallLeft;
00188   GT2004BasicBehaviorSearchForBallRight basicBehaviorSearchForBallRight;
00189   //!@}
00190 
00191 private:
00192   /** Is invoked when errors occurs */
00193   Xabsl2ErrorHandler& errorHandler;
00194   
00195   /** A reference to the GT2004HeadControl */
00196   GT2004HeadControl& headControl;
00197 };
00198 
00199 #endif // __GT2004HeadControlBasicBehaviors_h_
00200 
00201 /*
00202 * Change Log
00203 * 
00204 * $Log: GT2004HeadControlBasicBehaviors.h,v $
00205 * Revision 1.18  2004/07/01 18:21:16  dassler
00206 * Added BasicBehavior and HeadControlMode:
00207 * search-for-ball-left
00208 * search-for-ball-right
00209 * Test is needed
00210 *
00211 * Revision 1.17  2004/06/29 17:47:40  schmitt
00212 * lookAtCloseLandmark updated, if no landmark is seen
00213 *
00214 * Revision 1.16  2004/06/28 14:00:04  jhoffman
00215 * - scan back to ball slower
00216 * - directed scan briefly stops at landmarks
00217 * - input symbol set-joints-is-close-to-destination implemented and added to behavior
00218 * - merged marcs changes into track-ball-strict
00219 *
00220 * Revision 1.15  2004/06/19 12:32:12  jhoffman
00221 * - directed scan briefly stops before looking at the next landmark
00222 * - comments added
00223 *
00224 * Revision 1.14  2004/06/18 18:28:39  dassler
00225 * introduced basic-behavior:
00226 * BeginBallSearchAtBallPositionSeen
00227 * BeginBallSearchAtBallPositionCommunicated
00228 * BeginBallSearchAtBallPositionPropagated
00229 *
00230 * track-ball modified and build in ball-just-lost
00231 *
00232 * Revision 1.13  2004/06/17 15:26:45  jhoffman
00233 * various improvements to headcontrol
00234 *
00235 * Revision 1.12  2004/06/16 18:14:00  jhoffman
00236 * - search-for-landmarks-bug removed (setjointsdirect now sets "headPathPlanner.last...")
00237 *
00238 * Revision 1.11  2004/06/14 20:12:10  jhoffman
00239 * - numerous changes and additions to headcontrol
00240 * - cameraInfo default constructor now creates ERS7 info
00241 * - debug drawing "headcontrolfield" added
00242 *
00243 * Revision 1.10  2004/06/11 16:28:46  juengel
00244 * Added basic behavior release-ball.
00245 *
00246 * Revision 1.9  2004/06/05 16:02:49  jhoffman
00247 * - added look-at-ball-and-closest-landmark (basicbahavior)
00248 * - found and removed bug in simple-look-at-point
00249 *
00250 * Revision 1.8  2004/05/29 18:18:56  dueffert
00251 * walk parameter evolution, measurement and calibration stuff ported to GT2004_WM
00252 *
00253 * Revision 1.7  2004/05/27 17:13:37  jhoffman
00254 * - renaming: tilt1 -> neckTilt,  pan -> headPan,  tilt2 -> headTilt
00255 * - clipping included for setJoints
00256 * - removed some microrad/rad-bugs
00257 * - bodyPosture constructor and "=" operator fixed
00258 *
00259 * Revision 1.6  2004/05/25 19:26:38  tim
00260 * renamed basic behavior
00261 *
00262 * Revision 1.5  2004/05/25 18:30:12  jhoffman
00263 * landmarks-array in field dimensions
00264 * headcontrol looks at close landmark
00265 *
00266 * Revision 1.4  2004/05/25 17:41:27  tim
00267 * added look-straight-ahead BasicBehavior
00268 *
00269 * Revision 1.3  2004/05/25 13:35:27  tim
00270 * added grab-ball BasicBehavior
00271 *
00272 * Revision 1.2  2004/05/23 22:49:14  loetzsch
00273 * simplified basic behaviors
00274 *
00275 * Revision 1.1.1.1  2004/05/22 17:19:24  cvsadm
00276 * created new repository GT2004_WM
00277 *
00278 * Revision 1.1  2004/05/18 13:40:00  loetzsch
00279 * registered symbols and basic behaviors for GT2004HeadControl,
00280 * renamed some states and basic behaviors
00281 *
00282 */
00283