Modules/SensorDataProcessor/GT2005SensorDataProcessor.cpp

Go to the documentation of this file.

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

#include "GT2005SensorDataProcessor.h"
#include "Tools/Actorics/RobotDimensions.h"
#include "Tools/Actorics/Kinematics.h"
#include "Tools/Math/Common.h"
#include "Platform/SystemCall.h"

#include "Tools/Debugging/Debugging.h"

GT2005SensorDataProcessor::GT2005SensorDataProcessor
(const SensorDataProcessorInterfaces& interfaces)
: SensorDataProcessor(interfaces),
lastImageFrameNumber(0),
gravity(0,0,0),
accelerationWithGrav(0,0,0),
leftRollStartTime(SystemCall::getCurrentSystemTime()),
tempSysTime(0),
rightRollStartTime(SystemCall::getCurrentSystemTime())
{
  sensorDataRingBuffer.init();
  cameraMatrixRingBuffer.init();
  bodyPostureRingBuffer.init();
  for (int i = 0; i<3; i++) {
    previousAverage[i] = 0.0;
    actAverage[i] = 0.0;
  }
}

void GT2005SensorDataProcessor::execute()
{
  int i;
  CameraMatrix frameCameraMatrix;
  BodyPosture frameBodyPosture;

  psdPercept.numOfPercepts = 0;

  for (i = 0; i < sensorDataBuffer.numOfFrames; i++)
  {
    sensorDataRingBuffer.add(sensorDataBuffer.frame[i]);
  
    sensorDataRingBuffer.updateAverage(SensorData::accelerationX,10,gravity.x);
    sensorDataRingBuffer.updateAverage(SensorData::accelerationY,10,gravity.y);
    sensorDataRingBuffer.updateAverage(SensorData::accelerationZ,10,gravity.z);
  
    sensorDataRingBuffer.updateAverage(SensorData::accelerationX,2,accelerationWithGrav.x);
    sensorDataRingBuffer.updateAverage(SensorData::accelerationY,2,accelerationWithGrav.y);
    sensorDataRingBuffer.updateAverage(SensorData::accelerationZ,2,accelerationWithGrav.z);


    frameBodyPosture.frameNumber = sensorDataBuffer.frame[i].frameNumber;
    calculateBodyPostureFromLegSensors(sensorDataBuffer.frame[i], frameBodyPosture);
    calculateBodyPostureFromAccelerationSensors(sensorDataBuffer.frame[i], frameBodyPosture);

    if (1) // if motion provides valid data...
    {
      frameBodyPosture.bodyTiltProvidedByMotionControl = motionInfo.bodyTilt;
      frameBodyPosture.neckHeightProvidedByMotionControl = motionInfo.neckHeight;
      frameBodyPosture.bodyRollProvidedByMotionControl = 0;
    }
    else
    {
      /** @todo copy data from "calculated from..."*/
    }

    buildCameraMatrix(sensorDataBuffer.frame[i], frameBodyPosture, frameCameraMatrix);

    cameraMatrixRingBuffer.add(frameCameraMatrix);
    bodyPostureRingBuffer.add(frameBodyPosture);

    buildPSDPercept(
      sensorDataBuffer.frame[i], 
      cameraMatrixRingBuffer.getEntry(9), // 9: hack
      psdPercept[psdPercept.numOfPercepts++]);
  }

  double bodyRollAngle = atan2((double)-gravity.y,(double)-gravity.z);
  if ((bodyRollAngle >= -gt2005SensorDataProcessorRolledOnWallAngle)||(bodyRollAngle <= -gt2005SensorDataProcessorFalldownRollAngle))
    leftRollStartTime = SystemCall::getCurrentSystemTime();
  if ((bodyRollAngle <= gt2005SensorDataProcessorRolledOnWallAngle)||(bodyRollAngle >= gt2005SensorDataProcessorFalldownRollAngle))
    rightRollStartTime = SystemCall::getCurrentSystemTime();
  
  if (lastImageFrameNumber == imageFrameNumber) 
  {
    // The image is not new, so the camera matrix should not be built
    return;
  }
  lastImageFrameNumber = imageFrameNumber;

  // search image frame in sensor data
  for (i = 0; i < sensorDataRingBuffer.getNumberOfEntries(); i++)
  {
    if (sensorDataRingBuffer.getEntry(i).frameNumber <= imageFrameNumber) break;
  }
  cameraMatrix = cameraMatrixRingBuffer.getEntry(i);
  bodyPosture = bodyPostureRingBuffer.getEntry(i);
  bodyPosture.bodyTiltProvidedByMotionControl = motionInfo.bodyTilt;
  bodyPosture.bodyRollProvidedByMotionControl = 0;
  bodyPosture.neckHeightProvidedByMotionControl = motionInfo.neckHeight;
 
  Vector3<double> acceleration = 
    accelerationWithGrav - gravity;
  bodyPercept.acceleration = acceleration;
  
  int switches = detectSwitches();

  bodyPercept.setFrameNumber(sensorDataBuffer.frame[0].frameNumber);
  
  bodyPercept.setMouthState(detectMouthState());

  bodyPercept.setBodyPSDHighValue(sensorDataBuffer.lastFrame().data[SensorData::bodyPsd]>150000); //BBWalk: 110000
  
  bodyPercept.setBodyPSDDistance(getDistanceToSIFOC());
  
    bodyPercept.setSwitches(switches);
  if (!motionInfo.motionIsStable) 
  {
    bodyPercept.setState(BodyPercept::standing);
  }
  else if (detectFallDown())
  {
    bodyPercept.setState(BodyPercept::crashed);
    //OUTPUT (idText, text, "GT2005SensorDataProcessor : crashed");
  }
  else if (SystemCall::getTimeSince(leftRollStartTime)>1500)
  {
    bodyPercept.setState(BodyPercept::rollLeft);
    //OUTPUT (idText, text, "GT2005SensorDataProcessor : roll left");
  }
  else if (SystemCall::getTimeSince(rightRollStartTime)>1500)
  {
    bodyPercept.setState(BodyPercept::rollRight);
    //OUTPUT (idText, text, "GT2005SensorDataProcessor : roll right");
  }
  else if (detectPickup(gravity,acceleration))
  {
    bodyPercept.setState(BodyPercept::pickedUp);
  }
  else
  {
    bodyPercept.setState(BodyPercept::standing);
  }
  
  //double camSpeed = getCameraVelocity();
}


void GT2005SensorDataProcessor::buildCameraMatrix(const SensorData& sensorData, const BodyPosture& bP, CameraMatrix& cameraMatrix)
{
  double tilt = fromMicroRad(sensorData.data[SensorData::neckTilt]);
  double pan  = fromMicroRad(sensorData.data[SensorData::headPan]);
  double tilt2 = fromMicroRad(sensorData.data[SensorData::headTilt]);
  Kinematics::calculateCameraMatrix(tilt, pan, tilt2, bP.bodyTiltCalculatedFromLegSensors, bP.neckHeightCalculatedFromLegSensors, cameraMatrix);
  cameraMatrix.setFrameNumber(sensorData.frameNumber);
  //~ cameraMatrix.setFrameNumber(imageFrameNumber);
  cameraMatrix.isValid=motionInfo.motionIsStable;
}

void GT2005SensorDataProcessor::buildPSDPercept(const SensorData& sensorData, const CameraMatrix& cameraMatrix, SinglePSDPercept& psdPercept)
{
  // Achtung !!!
  // Hier wird zunächst einmal die eventuell kritische Annahme getroffen, 
  // dass optische Achse und "PSD-Strahl"- Achse identisch sind. 
  // Das bleibt erst mal so, bis genaueres bekannt ist. (M.L.)

  psdPercept.isValid = cameraMatrix.isValid;

  // A vector in the camera coordinate system with the length of the measured psd distance
  Vector3<double> psd;
  psd.y = 0; psd.z = 0;
  
  if(sensorData.data[SensorData::psd]/1000 < 300)
    psd.x = sensorData.data[SensorData::psd]/1000 - 20; 
  else
    psd.x = sensorData.data[SensorData::headPsdFar]/1000 - 30; 
  // mark the percept to be out of scope 
  psdPercept.tooFarAway = (psd.x >= 1300 - 30);
  psdPercept.body = sensorData.data[SensorData::bodyPsd]/1000;

  //
  psdPercept.neckTilt = fromMicroRad(sensorData.data[SensorData::neckTilt]);

  // Transformation of the vector into the robot's coordinate system
  Vector3<double> objectRelativeToCamera = cameraMatrix.rotation * psd;

  //OUTPUT(idText,text,"x: " << objectRelativeToCamera.x << ", y: " << objectRelativeToCamera.y << ", z: " << objectRelativeToCamera.z);

  // calculates the spot detected by the PSD Sensor relative to the point on the
  // ground below the shoulder joints.
  (Vector3<double>&)psdPercept = objectRelativeToCamera + cameraMatrix.translation;

  // OUTPUT(idText,text,"x2 " << psdPercept.x << ", y2 " << psdPercept.y << ", z2 " << psdPercept.z);

  psdPercept.setFrameNumber(sensorData.frameNumber);
}

int GT2005SensorDataProcessor::detectSwitches()
{
  int backMiddle, backFront, backBack,
      head, mouth, chin;

    backMiddle= (int)( (int)(sensorDataBuffer.frame[0].data[SensorData::back]) > 10  ) << BodyPercept::backMiddle;
    backFront = (int)( (int)(sensorDataBuffer.frame[0].data[SensorData::backF]) > 10  ) << BodyPercept::backFront;
    backBack  = (int)( (int)(sensorDataBuffer.frame[0].data[SensorData::backR]) > 10  ) << BodyPercept::backBack;
    head      = (int)( (int)(sensorDataBuffer.frame[0].data[SensorData::head]) > 10  ) << BodyPercept::head;
    mouth     = (int)(0 != (int)(sensorDataRingBuffer.getAverage(SensorData::mouth,     5)/-80000)) << BodyPercept::mouth;
    chin      = (int)(0 != (int)(sensorDataRingBuffer.getAverage(SensorData::chin,      5)*10)) << BodyPercept::chin;

    //for debug purposes
    DEBUG_RESPONSE ("Buttons:press middle button",
      backMiddle = 1 << BodyPercept::backMiddle;
    );

    DEBUG_RESPONSE ("Buttons:press front button",
      backMiddle = 1 << BodyPercept::backFront;
    );

    DEBUG_RESPONSE ("Buttons:press back button",
      backMiddle = 1 << BodyPercept::backBack;
    );

    DEBUG_RESPONSE ("Buttons:press head button",
      backMiddle = 1 << BodyPercept::head;
    );

    DEBUG_RESPONSE ("Buttons:press mouth",
      backMiddle = 1 << BodyPercept::mouth;
    );

    DEBUG_RESPONSE ("Buttons:press chin",
      backMiddle = 1 << BodyPercept::chin;
    );

    DEBUG_RESPONSE ("Buttons:press power",
      SystemCall::shutdown ();
    );

  return 
    backMiddle | backFront | backBack |
    head | mouth | chin;
}

bool GT2005SensorDataProcessor::detectFallDown()
{
  double bodyTiltAngle = atan2((double)gravity.x,(double)-gravity.z);
  double bodyRollAngle = atan2((double)-gravity.y,(double)-gravity.z);

  return
    (bodyTiltAngle >  gt2005SensorDataProcessorFalldownTiltAngle  ||
    bodyTiltAngle < -gt2005SensorDataProcessorFalldownTiltAngle  ||
    bodyRollAngle >  gt2005SensorDataProcessorFalldownRollAngle  ||
    bodyRollAngle < -gt2005SensorDataProcessorFalldownRollAngle);
}

bool GT2005SensorDataProcessor::detectPickup(const Vector3<double>& gravity, const Vector3<double>& acceleration)
{
  double accZ = acceleration * gravity;
  
  return (accZ < -50000000000000.0);
}

BodyPercept::MouthStates 
GT2005SensorDataProcessor::detectMouthState()
{
  if(sensorDataBuffer.lastFrame().data[SensorData::mouth] < -370000) //mouth open
  {
    return BodyPercept::mouthOpen;
  }
  else
  {
    return BodyPercept::mouthClosed;
  }
}

/**************************/
/***  SensorDataBuffer  ***/

double GT2005SensorDataProcessor::SensorDataRingBuffer::getAverage (SensorData::sensors sensor, int ticks)
{
  if (ticks>getNumberOfEntries()) ticks=getNumberOfEntries();
  if (ticks<1) return 0.0;
  double sum=0.0;
  for (int i=0;i<ticks;i++)
    sum+=getEntry(i).data[sensor];
  return sum/(double)ticks;
}

void GT2005SensorDataProcessor::SensorDataRingBuffer::updateAverage(SensorData::sensors sensor, int ticks, double& average)
{
  if (ticks<1) return;
  if (ticks<getNumberOfEntries())
    average-=((double)getEntry(ticks).data[sensor])/(double)ticks;
  average+=((double)getEntry(0).data[sensor])/(double)ticks;
}

long GT2005SensorDataProcessor::SensorDataRingBuffer::interpolate (SensorData::sensors sensor, unsigned long imageFrame)
{
  long frame = (long) imageFrame,
    currentValue = getEntry(0).data[sensor],
    currentFrame = (long) getEntry(0).frameNumber;
  if(currentFrame == frame)
    return currentValue;
  else
  {
    long oldValue = getEntry(1).data[sensor],
      oldFrame = (long) getEntry(1).frameNumber;
    if(currentFrame == oldFrame)
      return currentValue;
    else
      return currentValue + (oldValue - currentValue) * (frame - currentFrame) / (oldFrame - currentFrame);
  }
}

void GT2005SensorDataProcessor::calculateBodyPostureFromLegSensors(const SensorData& sensorData, BodyPosture& bP)
{
  RobotVertices robotVertices;
  Kinematics::calcNeckAndLegPositions(sensorDataBuffer.lastFrame(), robotVertices);

  bP.bodyTiltCalculatedFromLegSensors = robotVertices.bodyTilt;
  bP.bodyRollCalculatedFromLegSensors = robotVertices.bodyRoll;
  bP.neckHeightCalculatedFromLegSensors = robotVertices.neckHeight;
}

void GT2005SensorDataProcessor::calculateBodyPostureFromAccelerationSensors(const SensorData& sensorData, BodyPosture& bP)
{
  //calculate body angles
  long accX = sensorDataBuffer.lastFrame().data[SensorData::accelerationX]/1000;
  long accY = sensorDataBuffer.lastFrame().data[SensorData::accelerationY]/1000;
  long accZ = sensorDataBuffer.lastFrame().data[SensorData::accelerationZ]/1000;
  bP.bodyTiltCalculatedFromAccelerationSensors = atan2((double)accX,(double)-accZ);
  bP.bodyRollCalculatedFromAccelerationSensors = atan2((double)-accY,(double)-accZ);
}

double GT2005SensorDataProcessor::getCameraVelocity(const SensorData::sensors joint=SensorData::headPan)
{
  const int numOfValues=4;
  int jointNum = 1;
  sensorDataRingBuffer.updateAverage(SensorData::neckTilt, numOfValues, actAverage[0]);
  sensorDataRingBuffer.updateAverage(SensorData::headPan, numOfValues, actAverage[1]);
  sensorDataRingBuffer.updateAverage(SensorData::headTilt, numOfValues, actAverage[2]);
  switch (joint)
  {
  case SensorData::headTilt: jointNum = 2;
    break;
  case SensorData::headPan: jointNum = 1;
    break;
  case SensorData::neckTilt: jointNum = 0;
    break;
  }
  double cameraSpeed = toDegrees(fromMicroRad((long) (actAverage[jointNum] - previousAverage[jointNum]))) / RobotDimensions::motionCycleTime;
  for (int index = 0; index < 3; index++)
    previousAverage[index] = actAverage[index];
  /** 
  if (abs ((int) cameraSpeed) > 50)
    if(SystemCall::getCurrentSystemTime()-tempSysTime > 250)
    {
      tempSysTime=SystemCall::getCurrentSystemTime();
      OUTPUT(idText, text, cameraSpeed);
    }
  */
  return cameraSpeed;
}

double GT2005SensorDataProcessor::getDistanceToSIFOC()
{
  double value = 50;
  double psdval = 0;
  for (int i = 0; i < psdPercept.numOfPercepts; ++i)
    psdval += psdPercept[i].body;
  psdval /= (double)(psdPercept.numOfPercepts);
  if (psdval >= 170)
    psdval = 170;

  // from 160 to 170 equals from 1cm to 0cm
  if (psdval >= 160)
    value = 10-(psdval-160);
  else
  // from 145 to 160 equals from 2cm to 1cm
  if (psdval >= 145)
    value = ((15-(psdval-145))/1.5)+10;
  else
  // from 130 to 145 equals from 4cm to 2cm
  if (psdval >= 130)
    value = ((15-(psdval-130))/0.75)+20;

  return value;
}

---