Modules/SensorDataProcessor/GT2004SensorDataProcessor.h

Go to the documentation of this file.

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

#ifndef __GT2004SensorDataProcessor_h_
#define __GT2004SensorDataProcessor_h_

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

static const int gt2004SensorDataProcessorBufferSize = 20;

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

/**
* @class GT2004SensorDataProcessor
* 
* The default SensorDataProcessor in the GT2004 project.
*/
class GT2004SensorDataProcessor : public SensorDataProcessor
{
public:
  /** 
  * Constructor
  * @param interfaces The paramters of the HeadControl module.
  */
  GT2004SensorDataProcessor(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,gt2004SensorDataProcessorBufferSize>
  {
  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,gt2004SensorDataProcessorBufferSize> cameraMatrixRingBuffer;
  RingBuffer<BodyPosture,gt2004SensorDataProcessorBufferSize> bodyPostureRingBuffer;
  unsigned long leftRollStartTime;
  unsigned long rightRollStartTime;
};

#endif// __GT2004SensorDataProcessor_h_

/*
 * Change log :
 * 
 * $Log: GT2004SensorDataProcessor.h,v $
 * Revision 1.1  2004/07/10 00:18:31  spranger
 * renamed (readded) for coderelease
 *
 * Revision 1.6  2004/06/17 12:17:15  schmitt
 * Modified getCameraVelocity()
 * Selection of the speed each head joint is now possible
 * Default is headPan
 *
 * Revision 1.5  2004/06/16 17:07:33  cesarz
 * Moved body PSD calculations
 *
 * Revision 1.4  2004/06/16 12:40:02  cesarz
 * implemented getCameraVelocity()
 *
 * Revision 1.3  2004/05/26 20:18:05  juengel
 * Calculation of BodyPosture.
 *
 * Revision 1.2  2004/05/24 14:14:26  juengel
 * New button evaluation.
 *
 * Revision 1.1.1.1  2004/05/22 17:21:30  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.5  2004/04/09 11:35:53  roefer
 * Bremen Byters German Open check-in
 *
 * Revision 1.4  2004/04/07 12:28:59  risler
 * ddd checkin after go04 - first part
 *
 * Revision 1.2  2004/04/02 19:10:48  risler
 * robot no longer is crashed when using dash
 *
 * Revision 1.1.1.1  2004/03/29 08:28:47  Administrator
 * initial transfer from tamara
 *
 * Revision 1.3  2004/03/08 02:11:51  roefer
 * Interfaces should be const
 *
 * Revision 1.2  2004/02/04 13:41:33  roefer
 * Some place holders for BB2004 modules added
 *
 * Revision 1.1  2003/10/06 14:10:14  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.2  2003/07/06 12:05:31  schumann
 * added foreleg opening angle for ball challenge
 *
 * Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.14  2003/06/20 15:32:32  dueffert
 * getting from down from wall by rolling added
 *
 * Revision 1.13  2003/05/14 19:54:42  risler
 * PSDPercept contains all points from one frame
 *
 * Revision 1.12  2003/05/02 18:26:18  risler
 * SensorDataBuffer added
 * replaced SensorData with SensorDataBuffer
 * full SensorData resolution now accessible
 *
 * Revision 1.11  2003/04/04 17:51:23  hebbel
 * Removed not needed OUTPUTs
 *
 * Revision 1.10  2003/04/04 16:08:59  cesarz
 * mouth values adepted
 *
 * Revision 1.9  2003/04/01 22:40:44  cesarz
 * added mouth states
 *
 * Revision 1.8  2003/01/30 11:26:47  juengel
 * Added tailPosition to bodyPercept
 *
 * Revision 1.7  2002/10/10 13:09:49  loetzsch
 * First experiments with the PSD Sensor
 * - SensorDataProcessor now calculates PSDPercept
 * - Added the PerceptBehaviorControl solution PSDTest
 * - Added the RadarViewer3D to RobotControl, which can display the Points3D structure
 *
 * Revision 1.6  2002/09/22 18:40:53  risler
 * added new math functions, removed GTMath library
 *
 * Revision 1.5  2002/09/17 23:55:22  loetzsch
 * - unraveled several datatypes
 * - changed the WATCH macro
 * - completed the process restructuring
 *
 * Revision 1.4  2002/09/12 12:24:09  juengel
 * continued change of module/solution mechanisms
 *
 * Revision 1.3  2002/09/10 21:07:30  loetzsch
 * continued change of module/solution mechanisms
 *
 * Revision 1.2  2002/09/10 17:53:40  loetzsch
 * First draft of new Module/Solution Mechanisms
 *
 * Revision 1.1  2002/09/10 15:36:16  cvsadm
 * Created new project GT2003 (M.L.)
 * - Cleaned up the /Src/DataTypes directory
 * - Removed challenge related source code
 * - Removed processing of incoming audio data
 * - Renamed AcousticMessage to SoundRequest
 *
 * Revision 1.1.1.1  2002/05/10 12:40:15  cvsadm
 * Moved GT2002 Project from ute to tamara.
 *
 * Revision 1.18  2002/04/23 10:38:30  risler
 * renamed headOdometry to headState
 *
 * Revision 1.17  2002/04/17 17:04:40  risler
 * Darmstadt GO
 *
 * Revision 1.16  2002/04/08 18:35:07  risler
 * decreased buffer size
 *
 * Revision 1.15  2002/04/08 17:48:28  risler
 * added tilt angle calculation, pickedUp detection
 * some cleaning up
 *
 * Revision 1.14  2002/04/02 13:10:20  dueffert
 * big change: odometryData and cameraMatrix in image now, old logfiles may be obsolete
 *
 * Revision 1.13  2002/03/05 17:58:43  risler
 * added detectPickup
 *
 * Revision 1.12  2002/02/14 16:45:00  petters
 * no message
 *
 * Revision 1.11  2002/02/05 20:26:46  petters
 * ... uncommented
 *
 * Revision 1.10  2002/02/05 17:34:18  petters
 * interpolation functions in sensor data buffer added (not tested -> commented out)
 *
 * Revision 1.9  2002/02/03 09:32:54  risler
 * RingBuffer from DefaultSensorDataProcessor now is seperate class
 *
 * Revision 1.8  2002/02/01 15:03:23  risler
 * Removed pickup detection, as not working with paw sensors
 *
 * Revision 1.7  2002/01/20 23:35:06  loetzsch
 * Added parameter imageFrameNumber to execute(...)
 *
 * Revision 1.6  2002/01/18 12:24:17  mkunz
 * build camera matrix added
 *
 * Revision 1.5  2002/01/18 11:23:17  petters
 * return bodypercept added
 *
 * Revision 1.4  2001/12/23 23:45:28  petters
 * sensordatabuffer implemented, state detector implemented, switch detector implemented
 *
 * Revision 1.3  2001/12/12 20:21:12  petters
 * Streaming for SensorData / Image implemented; Conflict solved
 *
 * Revision 1.2  2001/12/10 17:47:07  risler
 * change log added
 *
 */

---