Modules/HeadControl/GT2005HeadControl/GT2005HeadControl.h

Go to the documentation of this file.

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

#ifndef __GT2005HeadControl_h__
#define __GT2005HeadControl_h__

#include "../Xabsl2HeadControl.h"

#include "GT2005HeadPathPlanner.h"
#include "GT2005HeadControlSymbols.h"
#include "GT2005HeadControlBasicBehaviors.h"

/**
* @class GT2005HeadControl
*
* The GT2005 version of the HeadControl module.
*
* @author Marc Dassler
* @author Jan Hoffmann
* @author Uwe Düffert
* @author Martin Lötzsch
*/
class GT2005HeadControl : public Xabsl2HeadControl
{
public:
/** 
* Constructor.
* @param interfaces The paramters of the HeadControl module.
  */
  GT2005HeadControl(HeadControlInterfaces& interfaces);
  
  /** Destructor. */
  ~GT2005HeadControl(){}
  
  /** 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 */
  GT2005HeadPathPlanner 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 */
  GT2005HeadControlSymbols symbols;

  /** The basic behaviors used by the Xabsl2Engine */
  GT2005HeadControlBasicBehaviors 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.  
  * a pose can be specified; if it is not specified, the robot pose is used
  * @param disregard... is used to toggle whether landmarks or landmarks AND field markings are used
  */
  int calculateClosestLandmark(Pose2D pose, double direction, double nextLeftOrRight, bool disregardFieldMarkings = false);
  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:
  void showVisualizationQuality();
  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 //__GT2005HeadControl_h__

---