Modules/SensorDataProcessor/GT2005SensorDataProcessor.h

Go to the documentation of this file.

/**
* @file GT2005SensorDataProcessor.h
* 
* This file contains a class for Sensor Data Processing.
*/

#ifndef __GT2005SensorDataProcessor_h_
#define __GT2005SensorDataProcessor_h_

#include "SensorDataProcessor.h"
#include "Tools/RingBuffer.h"
#include "Tools/Math/Common.h"

static const int gt2005SensorDataProcessorBufferSize = 20;

/** angles above which crash will be detected
 * @attention adjust the angles to values in GetupEngine
 */
const double gt2005SensorDataProcessorFalldownTiltAngle = fromDegrees(60);
const double gt2005SensorDataProcessorFalldownRollAngle = fromDegrees(60);
const double gt2005SensorDataProcessorRolledOnWallAngle = fromDegrees(19);

/**
* @class GT2004SensorDataProcessor
* 
* The default SensorDataProcessor in the GT2004 project.
*/
class GT2005SensorDataProcessor : public SensorDataProcessor
{
public:
  /** 
  * Constructor
  * @param interfaces The paramters of the HeadControl module.
  */
  GT2005SensorDataProcessor(const SensorDataProcessorInterfaces& interfaces);
  
  /** Executes the module */
  virtual void execute();

  /** Returns the angle speed of the camera [degrees/second] */
  double getCameraVelocity()
  {
    return getCameraVelocity(SensorData::headPan);
  }
  double getCameraVelocity(const SensorData::sensors joint);

  /** Calculates the distance to objects in front of the bodyPSD */
  double getDistanceToSIFOC();

protected:
  /** calculates the offset and the rotation of the camera matrix */
  virtual void buildCameraMatrix(const SensorData& sensorData, const BodyPosture& bP, CameraMatrix& cameraMatrix);

  /** detects whether the robot fell down. */
  virtual bool detectFallDown();

  /** current averaged values from acceleration sensors, longer period for gravity*/
  Vector3<double> gravity;

private:
  unsigned long tempSysTime;


  /** current averaged values from acceleration sensors, shorter period for acceleration*/
  Vector3<double> accelerationWithGrav;

  /** calculates a spot relative to the robot that was detected by the PSD sensor */
  void buildPSDPercept(const SensorData& sensorData, const CameraMatrix& cameraMatrix, SinglePSDPercept& psdPercept);

  int detectSwitches();
  BodyPercept::MouthStates detectMouthState();
  bool detectPickup(const Vector3<double>& gravity, 
    const Vector3<double>& acceleration);

  /** The frame number of the last received image */
  unsigned long lastImageFrameNumber;

  /** The average neck tilt, head pan and tilt of the robot (used in this order) */
  double previousAverage[3], actAverage[3];
    
  /** Stores values of SensorData for averaging */
  class SensorDataRingBuffer : public RingBuffer<SensorData,gt2005SensorDataProcessorBufferSize>
  {
  public:
    /** 
     * calculates an average value by summing up last ticks values
     * @warning not efficient, use updateAverage instead if possible
     */
    double getAverage (SensorData::sensors sensor, int ticks);

    /**
     * calculates an average value by subtracting oldest and adding newest value
     */
    void updateAverage (SensorData::sensors sensor, int ticks, double& average);

    long interpolate (SensorData::sensors sensor, unsigned long frame);
  };

  void calculateBodyPostureFromLegSensors(const SensorData& sensorData, BodyPosture& bP);
  void calculateBodyPostureFromAccelerationSensors(const SensorData& sensorData, BodyPosture& bP);

  SensorDataRingBuffer sensorDataRingBuffer;
  RingBuffer<CameraMatrix,gt2005SensorDataProcessorBufferSize> cameraMatrixRingBuffer;
  RingBuffer<BodyPosture,gt2005SensorDataProcessorBufferSize> bodyPostureRingBuffer;
  unsigned long leftRollStartTime;
  unsigned long rightRollStartTime;
};

#endif// __GT2005SensorDataProcessor_h_

---