Modules/BehaviorControl/GT2005BehaviorControl/GT2005BasicBehaviors/GT2005WalkCalibrationBasicBehaviors.h

Go to the documentation of this file.

#ifndef __GT2005WalkCalibrationBasicBehaviors_h_
#define __GT2005WalkCalibrationBasicBehaviors_h_

#include "../../BehaviorControl.h"
#include "Tools/Xabsl2/Xabsl2Engine/Xabsl2Engine.h"
#include "Tools/Math/PIDsmoothedValue.h"

class GT2005WalkCalibrationMainBehavior : public Xabsl2BasicBehavior, public BehaviorControlInterfaces
{
private:

  Pose2D calculateStartPose(const Pose2D& base);
  static double minDistanceToBorder(Vector2<double>& t);

  double initializeParameter;
  double rotationParameter;
  
  Pose2D startPose;  
  double startPoseAngle;
  bool startPoseCalculated;
  int startPoseBadCount;
  double initializationTime;
  int measureCount;
  char filename[20];

  int qCount;
  double qSum;

  double result;
  enum ResultCodes {
    resultUnready = 0,
    resultReady = 1,
    resultMeasure = 2,
    resultReposition = 3,
    resultInteraction = 4
  };

  void Initialize();
  void Measure();


  double measureStartTime, measureStopTime, tempTime;
  double startOffset, measureDuration, stopOffset;
  Vector2<double> translationStartPoint, translationStopPoint, test;
  double rotationStartAngle, rotationStopAngle, rotationTemp;

  double measureStartTime0, measureStopTime0;
  Vector2<double> translationStartPoint0, translationStopPoint0;
  double rotationStartAngle0, rotationStopAngle0;

  double walkingDirection;
  int currentTable, currentPoint, currentRotation;
  int robotColor, robotNumber;

  typedef struct
  {
    Pose2D controlled;
    Pose2D measured;
  } OdometryPoint;

  static const int measureSize = 195; //195 for normal plane callibartion 21 for rotation only callibration
  
  void removeExtreme(bool *pBadIndex, bool removeMaximum);

  
  OdometryPoint measurePoints[measureSize];
  int tempPoseCount;
  Pose2D tempPose[50];
  double tempTimeStart, tempTimeStop;

  Pose2D camRobotPose;
  double camTimestamp;
  int robotNotDetectedCount;


  /** center of the camera */
  Vector2<double> center;


  enum State
  { 
    stateNotInitialized,
    stateMeasureStart,
    stateMeasure,
    stateMeasureFinished,
    stateCalibrationFinished
  };

  State state;

  enum MeasureStatus
  {
    before,
    measuring,
    end
  };

  MeasureStatus measureStatus;





public:

  GT2005WalkCalibrationMainBehavior(BehaviorControlInterfaces& interfaces, Xabsl2ErrorHandler& errorHandler);
  
  void registerSymbols(Xabsl2Engine& engine);


  /**  Executes the basic behavior. */
  virtual void execute();

};



class GT2005WalkCalibrationBasicBehaviors : public BehaviorControlInterfaces
{
public:
/**
* Constructor
  */
  
  GT2005WalkCalibrationBasicBehaviors(BehaviorControlInterfaces& interfaces,
    Xabsl2ErrorHandler& errorHandler)
    : BehaviorControlInterfaces(interfaces),
    errorHandler(errorHandler),
    gt2005WalkCalibrationMainBehavior(interfaces, errorHandler)
   {}
  
  /** Registers basic behaviors at the engine */
  void registerBasicBehaviors(Xabsl2Engine& engine);
  
private:
  /** Is invoked when errors occurs */
  Xabsl2ErrorHandler& errorHandler;

  GT2005WalkCalibrationMainBehavior gt2005WalkCalibrationMainBehavior;

  //!@}
};



#endif // __GT2005BasicWalkCalibrationBasicBehaviors_h_

---