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

# Modules/SensorDataProcessor/GT2005SensorDataProcessor.h

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005SensorDataProcessor.h
    00003 *
    00004 * This file contains a class for Sensor Data Processing.
    00005 */
    00006
    00007 #ifndef __GT2005SensorDataProcessor_h_
    00008 #define __GT2005SensorDataProcessor_h_
    00009
    00010 #include "SensorDataProcessor.h"
    00011 #include "Tools/RingBuffer.h"
    00012 #include "Tools/Math/Common.h"
    00013
    00014 static const int gt2005SensorDataProcessorBufferSize = 20;
    00015
    00016 /** angles above which crash will be detected
    00017  * @attention adjust the angles to values in GetupEngine
    00018  */
    00019 const double gt2005SensorDataProcessorFalldownTiltAngle = fromDegrees(60);
    00020 const double gt2005SensorDataProcessorFalldownRollAngle = fromDegrees(60);
    00021 const double gt2005SensorDataProcessorRolledOnWallAngle = fromDegrees(19);
    00022
    00023 /**
    00024 * @class GT2004SensorDataProcessor
    00025 *
    00026 * The default SensorDataProcessor in the GT2004 project.
    00027 */
    00028 class GT2005SensorDataProcessor : public SensorDataProcessor
    00029 {
    00030 public:
    00031   /**
    00032   * Constructor
    00033   * @param interfaces The paramters of the HeadControl module.
    00034   */
    00035   GT2005SensorDataProcessor(const SensorDataProcessorInterfaces& interfaces);
    00036
    00037   /** Executes the module */
    00038   virtual void execute();
    00039
    00040   /** Returns the angle speed of the camera [degrees/second] */
    00041   double getCameraVelocity()
    00042   {
    00043     return getCameraVelocity(SensorData::headPan);
    00044   }
    00045   double getCameraVelocity(const SensorData::sensors joint);
    00046
    00047   /** Calculates the distance to objects in front of the bodyPSD */
    00048   double getDistanceToSIFOC();
    00049
    00050 protected:
    00051   /** calculates the offset and the rotation of the camera matrix */
    00052   virtual void buildCameraMatrix(const SensorData& sensorData, const BodyPosture& bP, CameraMatrix& cameraMatrix);
    00053
    00054   /** detects whether the robot fell down. */
    00055   virtual bool detectFallDown();
    00056
    00057   /** current averaged values from acceleration sensors, longer period for gravity*/
    00058   Vector3<double> gravity;
    00059
    00060 private:
    00061   unsigned long tempSysTime;
    00062
    00063
    00064   /** current averaged values from acceleration sensors, shorter period for acceleration*/
    00065   Vector3<double> accelerationWithGrav;
    00066
    00067   /** calculates a spot relative to the robot that was detected by the PSD sensor */
    00068   void buildPSDPercept(const SensorData& sensorData, const CameraMatrix& cameraMatrix, SinglePSDPercept& psdPercept);
    00069
    00070   int detectSwitches();
    00071   BodyPercept::MouthStates detectMouthState();
    00072   bool detectPickup(const Vector3<double>& gravity,
    00073     const Vector3<double>& acceleration);
    00074
    00075   /** The frame number of the last received image */
    00076   unsigned long lastImageFrameNumber;
    00077
    00078   /** The average neck tilt, head pan and tilt of the robot (used in this order) */
    00079   double previousAverage[3], actAverage[3];
    00080
    00081   /** Stores values of SensorData for averaging */
    00082   class SensorDataRingBuffer : public RingBuffer<SensorData,gt2005SensorDataProcessorBufferSize>
    00083   {
    00084   public:
    00085     /**
    00086      * calculates an average value by summing up last ticks values
    00087      * @warning not efficient, use updateAverage instead if possible
    00088      */
    00089     double getAverage (SensorData::sensors sensor, int ticks);
    00090
    00091     /**
    00092      * calculates an average value by subtracting oldest and adding newest value
    00093      */
    00094     void updateAverage (SensorData::sensors sensor, int ticks, double& average);
    00095
    00096     long interpolate (SensorData::sensors sensor, unsigned long frame);
    00097   };
    00098
    00099   void calculateBodyPostureFromLegSensors(const SensorData& sensorData, BodyPosture& bP);
    00100   void calculateBodyPostureFromAccelerationSensors(const SensorData& sensorData, BodyPosture& bP);
    00101
    00102   SensorDataRingBuffer sensorDataRingBuffer;
    00103   RingBuffer<CameraMatrix,gt2005SensorDataProcessorBufferSize> cameraMatrixRingBuffer;
    00104   RingBuffer<BodyPosture,gt2005SensorDataProcessorBufferSize> bodyPostureRingBuffer;
    00105   unsigned long leftRollStartTime;
    00106   unsigned long rightRollStartTime;
    00107 };
    00108
    00109 #endif// __GT2005SensorDataProcessor_h_