Modules/HeadControl/GT2004HeadControl/GT2004HeadControl.h

Go to the documentation of this file.

/**
* @file GT2004HeadControl.h
* 
* Definition of class GT2004HeadControl.
*
* @author Marc Dassler
* @author Jan Hoffmann
* @author Uwe Düffert
* @author Martin Lötzsch
*/

#ifndef __GT2004HeadControl_h__
#define __GT2004HeadControl_h__

#include "../Xabsl2HeadControl.h"

#include "GT2004HeadPathPlanner.h"
#include "GT2004HeadControlSymbols.h"
#include "GT2004HeadControlBasicBehaviors.h"

/**
* @class GT2004HeadControl
*
* The GT2004 version of the HeadControl module.
*
* @author Marc Dassler
* @author Jan Hoffmann
* @author Uwe Düffert
* @author Martin Lötzsch
*/
class GT2004HeadControl : public Xabsl2HeadControl
{
public:
/** 
* Constructor.
* @param interfaces The paramters of the HeadControl module.
  */
  GT2004HeadControl(HeadControlInterfaces& interfaces);
  
  /** Destructor. */
  ~GT2004HeadControl(){}
  
  /** Executes the module */
  virtual void execute();
  
  /** 
  * Is called for every incoming debug message.
  * @param message An interface to read the message from the queue
  * @return if the messag was read
  */
  virtual bool handleMessage(InMessage& message);

  /** Registers symbols and basic behaviors at the engine */
  virtual void registerSymbolsAndBasicBehaviors();
  
  /** An instance of the head path planner */
  GT2004HeadPathPlanner headPathPlanner;

  /** If true, the last head movement was directed to the left side (of the ball) */
  bool lastScanWasLeft;

  /** Is true, if the head is on the left side */
  bool headPanIsLeft();

  /** put the sensor values in var pos */
  void getSensorHeadAngles(Vector3<double>& pos);

  /** returns a distance between actual position and comp. the small results are better */
  double headPositionDistanceToActualPosition(Vector3<double> comp,bool leftSide);

  /** The head control mode that was executed in the last frame */
  HeadControlMode::HeadControlModes lastHeadControlMode;

  /** Information about the used camera */
  CameraInfo cameraInfo;

  /** The symbols used by the Xabsl2Engine */
  GT2004HeadControlSymbols symbols;

  /** The basic behaviors used by the Xabsl2Engine */
  GT2004HeadControlBasicBehaviors basicBehaviors;

  /** The minimum head speed in urad per frame: 4000 = 28.6°/s */
  enum {minHeadSpeed=4000};


  /** deals with setting the head joints and performs 
  optimizations so that the head does not move too fast 
  angles are in RAD and NOT EVER AGAIN(!!) IN MICRORAD!
  */ 
  void setJoints(double tilt, double pan, double roll, double speed=0, double mouth=0);

  /* Writes the joint angles to the HeadMotionRequest without optimization and smoothing */ 
  void setJointsDirect(double tilt, double pan, double roll, double mouth=0);

  void simpleLookAtPointRelativeToRobot(const Vector3<double> pos, Vector2<int> offset, double& neckTilt, double& headPan, double& headTilt);
  
  /** Simplified "look at 3d-point" on field with offset point in camera image 
  this is straight-forward for the ERS210, but the ERS7 has two 
  tilt joints that can both be used to look at something. Lookatpoint
  uses the two following methods to find a 'good' solution.  */
  void simpleLookAtPointOnField(const Vector3<double> pos, Vector2<int> offset, double& neckTilt, double& headPan, double& headTilt);

  /** Simplified "look at 3d-point" subroutine trying to find tilt1 solution for given tilt2/roll. this is useful on ERS210 and ERS7
  *@return true when matching angles were found */
  bool simpleLookAtPointFixNeckTilt(const Vector3<double> &aim, const double& tilt1, double& headPan, double& headTilt);

  /** look at 3d-point on field with offset point in camera image 
  this is straight-forward for the ERS210, but the ERS7 has two 
  tilt joints that can both be used to look at something. Lookatpoint
  uses the two following methods to find a 'good' solution.  */
  void lookAtPoint(const Vector3<double> &pos,const Vector2<int> &offset,double& tilt,double& pan,double& roll);

  /** look at 3d-point subroutine trying to find tilt1 solution for given tilt2/roll. this is useful on ERS210 and ERS7
  *@return true when matching angles were found */
  bool lookAtPointFixHeadTilt(const Vector3<double> &aim, const double& xtan, const double& ytan, double& tilt1, double& pan, const double& tilt2);

  /** look at 3d-point subroutine trying to find tilt2 solution for given tilt1. this is only useful on ERS7
  *@return true when matching angles were found */
  bool lookAtPointFixNeckTilt(const Vector3<double> &aim, const double& xtan, const double& ytan, const double& tilt, double& pan, double& tilt2);

  /** return the closest landmark w.r.t. a given direction; also,
  * the next landmark to the left (or right) can be calculated.  */
  int calculateClosestLandmark(double direction = 0, double nextLeftOrRight = 0);

  /** call look-at-point depending on whether the landmark has has a z-dimension 
  * the offset in the image is adjusted: landmarks on the ground are aimed at differently from goals and corner posts*/
  void aimAtLandmark(int landmark, double& neckTilt, double& headPan, double& headTilt);

  /** returns the angles that are good for looking at the ball */
  void getLookAtBallAngles(const Vector2<double> ballOnField, double& neckTilt, double& headPan, double& headTilt);
   
  /** stores the number of frames "setjoints" has been close to its destination */
  int setJointsIsCloseToDestination;
  
  /** true if the max pan of the head joint is reached */
  bool setJointsMaxPanReached;

  /** initial the main angles for the head movement */ 
  void setupMainAngles();

  
  /** basic headpositions for the gaze */
  Vector3<double> headLeft;
  Vector3<double> headRight;
  Vector3<double> headMiddleLeft;
  Vector3<double> headMiddleLeftDown;
  Vector3<double> headMiddleRight;
  Vector3<double> headMiddleRightDown;
  Vector3<double> headRightDown;
  Vector3<double> headLeftDown;
  Vector3<double> headUp;
  Vector3<double> headDown;

  /** speed in rad/s for head movement. its used for headpathplanner and the calibration */
  double speedNeckTilt;
  double speedHeadPan;
  double speedHeadTilt;

  /* this calculates the speed of the head motors.
   later the speeds where used for the headPathPlanner for calculating realistic headmotions */
  void calibrateHeadSpeed();

  /* if the ball get losts, he belives in the communicated ball */

  bool useCommunicatedBall;

  enum
  {
    calibrationStateStart,
    calibrationStateLeft,
    calibrationStateLeftWait,
    calibrationStateRight,
    calibrationStateRightWait,
    calibrationStateDownTilt1,
    calibrationStateDownTilt1Wait,
    calibrationStateUpTilt1,
    calibrationStateUpTilt1Wait,
    calibrationStateDownTilt2,
    calibrationStateDownTilt2Wait,
    calibrationStateUpTilt2,
    calibrationStateUpTilt2Wait,
    calibrationStateUseResults,
    calibrationStateReady
  } calibrationState;

  long calibrationTime;
  int calibrationTimeOutsTilt1,calibrationTimeOutsPan,calibrationTimeOutsTilt2;
  int calibrationRoundCount,calibrationSuccessfulRounds;

  void calibrationReset()
  {
    calibrationTimeOutsTilt1 = 0;
    calibrationTimeOutsPan   = 0;
    calibrationTimeOutsTilt2 = 0;
    calibrationState = calibrationStateStart;
  }
  /* return true, if the position is reached more or less */
  bool headPositionReached(Vector3<double> pos)
  {
    return headPathPlanner.headPositionReached(pos);
  }

  /** return the index of the last seen beacon */
  int getLastSeenBeaconIndex();

  /** return the time of the last seen beacon */
  long getTimeOfLastSeenBeacon(int index);

  /** return the time between the two last seen beacons */
  long getTimeBetweenSeen2LastBeacons(int index);

  /* return true, if the calibration move is over the time to reach the given position */
  bool isTimedOut()
  {
    return (SystemCall::getTimeSince(calibrationTime)>2500);
  }
  /* return true, if the calibration process is ready */
  bool calibrateHeadSpeedIsReady()
  {
    return (calibrationState==calibrationStateReady);
  }

  /** looks to the left/right side an aearch for ball. Used for kicks */
  void searchForBallLeft();
  void searchForBallRight();

  /** begin a ball search by the given start position */
  void beginBallSearchAt(Vector2<double> ballPosition2d);

private:
  Range<double> jointRangeNeckTilt, jointRangeHeadPan, jointRangeHeadTilt;

  // the following are used to improve performance of set-joints when the robot is turning fast
  OdometryData lastOdometryData;
  RobotPose lastRobotPose;
};

#endif //__GT2004HeadControl_h__

/*
* Change log :
* 
* $Log: GT2004HeadControl.h,v $
* Revision 1.22  2004/07/02 10:27:03  jhoffman
* - preparation for compensating robot motions by appropriate head motion (no actual changes)
* - headControl-Mode added for testing
* - look-at-ball-and-closest-landmark uses quicker head movement
*
* Revision 1.21  2004/07/01 18:21:16  dassler
* Added BasicBehavior and HeadControlMode:
* search-for-ball-left
* search-for-ball-right
* Test is needed
*
* Revision 1.20  2004/06/29 17:06:53  dassler
* find ball scans first to the side where the head pan angle is smaller
*
* Revision 1.19  2004/06/29 10:17:00  dassler
* find ball choose side to scan dependent of headPan
*
* Revision 1.18  2004/06/28 14:00:04  jhoffman
* - scan back to ball slower
* - directed scan briefly stops at landmarks
* - input symbol set-joints-is-close-to-destination implemented and added to behavior
* - merged marcs changes into track-ball-strict
*
* Revision 1.17  2004/06/28 09:46:57  dassler
* introduced some more headcontrol symbols
* time-since-last-seen-beacon
* time-between-last-beacons
*
* Revision 1.16  2004/06/18 18:28:39  dassler
* introduced basic-behavior:
* BeginBallSearchAtBallPositionSeen
* BeginBallSearchAtBallPositionCommunicated
* BeginBallSearchAtBallPositionPropagated
*
* track-ball modified and build in ball-just-lost
*
* Revision 1.15  2004/06/17 22:02:13  dassler
* LookLeft/Right set to the the berlin gaze
* searchForBall headPath optimzied.
*
* Revision 1.14  2004/06/17 15:39:33  dassler
* LandmarkState added
*
* Revision 1.12  2004/06/15 16:29:56  jhoffman
* check in for practice match
*
* Revision 1.11  2004/06/14 20:12:10  jhoffman
* - numerous changes and additions to headcontrol
* - cameraInfo default constructor now creates ERS7 info
* - debug drawing "headcontrolfield" added
*
* Revision 1.10  2004/06/08 18:37:11  juengel
* Added  setJointsDirect().
*
* Revision 1.9  2004/06/05 16:02:49  jhoffman
* - added look-at-ball-and-closest-landmark (basicbahavior)
* - found and removed bug in simple-look-at-point
*
* Revision 1.8  2004/06/04 08:44:12  juengel
* Renamed simpleLookAtPoint to simpleLookAtPointOnField.
* Added simpleLookAtPointRelativeToRobot.
*
* Revision 1.7  2004/06/01 15:05:41  jhoffman
* added "simple-look-at-ball" to gt2004headcontrol
*
* Revision 1.6  2004/05/27 17:13:37  jhoffman
* - renaming: tilt1 -> neckTilt,  pan -> headPan,  tilt2 -> headTilt
* - clipping included for setJoints
* - removed some microrad/rad-bugs
* - bodyPosture constructor and "=" operator fixed
*
* Revision 1.5  2004/05/25 18:30:12  jhoffman
* landmarks-array in field dimensions
* headcontrol looks at close landmark
*
* Revision 1.4  2004/05/24 18:19:43  jhoffman
* microrad --> rad
*
* Revision 1.3  2004/05/23 22:49:14  loetzsch
* simplified basic behaviors
*
* Revision 1.2  2004/05/23 20:27:57  loetzsch
* some improvements with the head control
*
* Revision 1.1.1.1  2004/05/22 17:19:24  cvsadm
* created new repository GT2004_WM
*
* Revision 1.2  2004/05/18 13:40:00  loetzsch
* registered symbols and basic behaviors for GT2004HeadControl,
* renamed some states and basic behaviors
*
* Revision 1.1  2004/05/14 11:37:08  loetzsch
* support for multiple xabsl2engines in different modules
* preliminary GT2004HeadControl (does not work at all)
*
*/

---