Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/HeadControl/GT2005HeadControl/GT2005HeadControl.h

Go to the documentation of this file.

    00001 /**
    00002 * @file Modules/HeadControl/GT2005HeadControl/GT2005HeadControl.h
    00003 *
    00004 * Definition of class GT2005HeadControl.
    00005 *
    00006 * @author Marc Dassler
    00007 * @author Jan Hoffmann
    00008 * @author Uwe Düffert
    00009 * @author Martin Lötzsch
    00010 */
    00011
    00012 #ifndef __GT2005HeadControl_h__
    00013 #define __GT2005HeadControl_h__
    00014
    00015 #include "../Xabsl2HeadControl.h"
    00016
    00017 #include "GT2005HeadPathPlanner.h"
    00018 #include "GT2005HeadControlSymbols.h"
    00019 #include "GT2005HeadControlBasicBehaviors.h"
    00020
    00021 /**
    00022 * @class GT2005HeadControl
    00023 *
    00024 * The GT2005 version of the HeadControl module.
    00025 *
    00026 * @author Marc Dassler
    00027 * @author Jan Hoffmann
    00028 * @author Uwe Düffert
    00029 * @author Martin Lötzsch
    00030 */
    00031 class GT2005HeadControl : public Xabsl2HeadControl
    00032 {
    00033 public:
    00034 /**
    00035 * Constructor.
    00036 * @param interfaces The paramters of the HeadControl module.
    00037   */
    00038   GT2005HeadControl(HeadControlInterfaces& interfaces);
    00039
    00040   /** Destructor. */
    00041   ~GT2005HeadControl(){}
    00042
    00043   /** Executes the module */
    00044   virtual void execute();
    00045
    00046   /**
    00047   * Is called for every incoming debug message.
    00048   * @param message An interface to read the message from the queue
    00049   * @return if the messag was read
    00050   */
    00051   virtual bool handleMessage(InMessage& message);
    00052
    00053   /** Registers symbols and basic behaviors at the engine */
    00054   virtual void registerSymbolsAndBasicBehaviors();
    00055
    00056   /** An instance of the head path planner */
    00057   GT2005HeadPathPlanner headPathPlanner;
    00058
    00059   /** If true, the last head movement was directed to the left side (of the ball) */
    00060   bool lastScanWasLeft;
    00061
    00062   /** Is true, if the head is on the left side */
    00063   bool headPanIsLeft();
    00064
    00065   /** put the sensor values in var pos */
    00066   void getSensorHeadAngles(Vector3<double>& pos);
    00067
    00068   /** returns a distance between actual position and comp. the small results are better */
    00069   double headPositionDistanceToActualPosition(Vector3<double> comp,bool leftSide);
    00070
    00071   /** The head control mode that was executed in the last frame */
    00072   HeadControlMode::HeadControlModes lastHeadControlMode;
    00073
    00074   /** Information about the used camera */
    00075   CameraInfo cameraInfo;
    00076
    00077   /** The symbols used by the Xabsl2Engine */
    00078   GT2005HeadControlSymbols symbols;
    00079
    00080   /** The basic behaviors used by the Xabsl2Engine */
    00081   GT2005HeadControlBasicBehaviors basicBehaviors;
    00082
    00083   /** The minimum head speed in urad per frame: 4000 = 28.6°/s */
    00084   enum {minHeadSpeed=4000};
    00085
    00086
    00087   /** deals with setting the head joints and performs
    00088   optimizations so that the head does not move too fast
    00089   angles are in RAD and NOT EVER AGAIN(!!) IN MICRORAD!
    00090   */
    00091   void setJoints(double tilt, double pan, double roll, double speed=0, double mouth=0);
    00092
    00093   /* Writes the joint angles to the HeadMotionRequest without optimization and smoothing */
    00094   void setJointsDirect(double tilt, double pan, double roll, double mouth=0);
    00095
    00096   void simpleLookAtPointRelativeToRobot(const Vector3<double> pos, Vector2<int> offset, double& neckTilt, double& headPan, double& headTilt);
    00097
    00098   /** Simplified "look at 3d-point" on field with offset point in camera image
    00099   this is straight-forward for the ERS210, but the ERS7 has two
    00100   tilt joints that can both be used to look at something. Lookatpoint
    00101   uses the two following methods to find a 'good' solution.  */
    00102   void simpleLookAtPointOnField(const Vector3<double> pos, Vector2<int> offset, double& neckTilt, double& headPan, double& headTilt);
    00103
    00104   /** Simplified "look at 3d-point" subroutine trying to find tilt1 solution for given tilt2/roll. this is useful on ERS210 and ERS7
    00105   *@return true when matching angles were found */
    00106   bool simpleLookAtPointFixNeckTilt(const Vector3<double> &aim, const double& tilt1, double& headPan, double& headTilt);
    00107
    00108   /** look at 3d-point on field with offset point in camera image
    00109   this is straight-forward for the ERS210, but the ERS7 has two
    00110   tilt joints that can both be used to look at something. Lookatpoint
    00111   uses the two following methods to find a 'good' solution.  */
    00112   void lookAtPoint(const Vector3<double> &pos,const Vector2<int> &offset,double& tilt,double& pan,double& roll);
    00113
    00114   /** look at 3d-point subroutine trying to find tilt1 solution for given tilt2/roll. this is useful on ERS210 and ERS7
    00115   *@return true when matching angles were found */
    00116   bool lookAtPointFixHeadTilt(const Vector3<double> &aim, const double& xtan, const double& ytan, double& tilt1, double& pan, const double& tilt2);
    00117
    00118   /** look at 3d-point subroutine trying to find tilt2 solution for given tilt1. this is only useful on ERS7
    00119   *@return true when matching angles were found */
    00120   bool lookAtPointFixNeckTilt(const Vector3<double> &aim, const double& xtan, const double& ytan, const double& tilt, double& pan, double& tilt2);
    00121
    00122   /** return the closest landmark w.r.t. a given direction; also,
    00123   * the next landmark to the left (or right) can be calculated.
    00124   * a pose can be specified; if it is not specified, the robot pose is used
    00125   * @param disregard... is used to toggle whether landmarks or landmarks AND field markings are used
    00126   */
    00127   int calculateClosestLandmark(Pose2D pose, double direction, double nextLeftOrRight, bool disregardFieldMarkings = false);
    00128   int calculateClosestLandmark(double direction = 0, double nextLeftOrRight = 0);
    00129
    00130   /** call look-at-point depending on whether the landmark has has a z-dimension
    00131   * the offset in the image is adjusted: landmarks on the ground are aimed at differently from goals and corner posts*/
    00132   void aimAtLandmark(int landmark, double& neckTilt, double& headPan, double& headTilt);
    00133
    00134   /** returns the angles that are good for looking at the ball */
    00135   void getLookAtBallAngles(const Vector2<double> ballOnField, double& neckTilt, double& headPan, double& headTilt);
    00136
    00137   /** stores the number of frames "setjoints" has been close to its destination */
    00138   int setJointsIsCloseToDestination;
    00139
    00140   /** true if the max pan of the head joint is reached */
    00141   bool setJointsMaxPanReached;
    00142
    00143   /** initial the main angles for the head movement */
    00144   void setupMainAngles();
    00145
    00146
    00147   /** basic headpositions for the gaze */
    00148   Vector3<double> headLeft;
    00149   Vector3<double> headRight;
    00150   Vector3<double> headMiddleLeft;
    00151   Vector3<double> headMiddleLeftDown;
    00152   Vector3<double> headMiddleRight;
    00153   Vector3<double> headMiddleRightDown;
    00154   Vector3<double> headRightDown;
    00155   Vector3<double> headLeftDown;
    00156   Vector3<double> headUp;
    00157   Vector3<double> headDown;
    00158
    00159   /** speed in rad/s for head movement. its used for headpathplanner and the calibration */
    00160   double speedNeckTilt;
    00161   double speedHeadPan;
    00162   double speedHeadTilt;
    00163
    00164   /* this calculates the speed of the head motors.
    00165    later the speeds where used for the headPathPlanner for calculating realistic headmotions */
    00166   void calibrateHeadSpeed();
    00167
    00168   /* if the ball get losts, he belives in the communicated ball */
    00169
    00170   bool useCommunicatedBall;
    00171
    00172   enum
    00173   {
    00174     calibrationStateStart,
    00175     calibrationStateLeft,
    00176     calibrationStateLeftWait,
    00177     calibrationStateRight,
    00178     calibrationStateRightWait,
    00179     calibrationStateDownTilt1,
    00180     calibrationStateDownTilt1Wait,
    00181     calibrationStateUpTilt1,
    00182     calibrationStateUpTilt1Wait,
    00183     calibrationStateDownTilt2,
    00184     calibrationStateDownTilt2Wait,
    00185     calibrationStateUpTilt2,
    00186     calibrationStateUpTilt2Wait,
    00187     calibrationStateUseResults,
    00188     calibrationStateReady
    00189   } calibrationState;
    00190
    00191   long calibrationTime;
    00192   int calibrationTimeOutsTilt1,calibrationTimeOutsPan,calibrationTimeOutsTilt2;
    00193   int calibrationRoundCount,calibrationSuccessfulRounds;
    00194
    00195   void calibrationReset()
    00196   {
    00197     calibrationTimeOutsTilt1 = 0;
    00198     calibrationTimeOutsPan   = 0;
    00199     calibrationTimeOutsTilt2 = 0;
    00200     calibrationState = calibrationStateStart;
    00201   }
    00202   /* return true, if the position is reached more or less */
    00203   bool headPositionReached(Vector3<double> pos)
    00204   {
    00205     return headPathPlanner.headPositionReached(pos);
    00206   }
    00207
    00208   /** return the index of the last seen beacon */
    00209   int getLastSeenBeaconIndex();
    00210
    00211   /** return the time of the last seen beacon */
    00212   long getTimeOfLastSeenBeacon(int index);
    00213
    00214   /** return the time between the two last seen beacons */
    00215   long getTimeBetweenSeen2LastBeacons(int index);
    00216
    00217   /* return true, if the calibration move is over the time to reach the given position */
    00218   bool isTimedOut()
    00219   {
    00220     return (SystemCall::getTimeSince(calibrationTime)>2500);
    00221   }
    00222   /* return true, if the calibration process is ready */
    00223   bool calibrateHeadSpeedIsReady()
    00224   {
    00225     return (calibrationState==calibrationStateReady);
    00226   }
    00227
    00228   /** looks to the left/right side an aearch for ball. Used for kicks */
    00229   void searchForBallLeft();
    00230   void searchForBallRight();
    00231
    00232   /** begin a ball search by the given start position */
    00233   void beginBallSearchAt(Vector2<double> ballPosition2d);
    00234
    00235 private:
    00236   void showVisualizationQuality();
    00237   Range<double> jointRangeNeckTilt, jointRangeHeadPan, jointRangeHeadTilt;
    00238
    00239   // the following are used to improve performance of set-joints when the robot is turning fast
    00240   OdometryData lastOdometryData;
    00241   RobotPose lastRobotPose;
    00242 };
    00243
    00244 #endif //__GT2005HeadControl_h__