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

# Modules/SensorDataProcessor/GT2005SensorDataProcessor.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005SensorDataProcessor.cpp
    00003 *
    00004 * This file contains a class for Sensor Data Processing.
    00005 */
    00006
    00007 #include "GT2005SensorDataProcessor.h"
    00008 #include "Tools/Actorics/RobotDimensions.h"
    00009 #include "Tools/Actorics/Kinematics.h"
    00010 #include "Tools/Math/Common.h"
    00011 #include "Platform/SystemCall.h"
    00012
    00013 #include "Tools/Debugging/Debugging.h"
    00014
    00015 GT2005SensorDataProcessor::GT2005SensorDataProcessor
    00016 (const SensorDataProcessorInterfaces& interfaces)
    00017 : SensorDataProcessor(interfaces),
    00018 lastImageFrameNumber(0),
    00019 gravity(0,0,0),
    00020 accelerationWithGrav(0,0,0),
    00021 leftRollStartTime(SystemCall::getCurrentSystemTime()),
    00022 tempSysTime(0),
    00023 rightRollStartTime(SystemCall::getCurrentSystemTime())
    00024 {
    00025   sensorDataRingBuffer.init();
    00026   cameraMatrixRingBuffer.init();
    00027   bodyPostureRingBuffer.init();
    00028   for (int i = 0; i<3; i++) {
    00029     previousAverage[i] = 0.0;
    00030     actAverage[i] = 0.0;
    00031   }
    00032 }
    00033
    00034 void GT2005SensorDataProcessor::execute()
    00035 {
    00036   int i;
    00037   CameraMatrix frameCameraMatrix;
    00038   BodyPosture frameBodyPosture;
    00039
    00040   psdPercept.numOfPercepts = 0;
    00041
    00042   for (i = 0; i < sensorDataBuffer.numOfFrames; i++)
    00043   {
    00044     sensorDataRingBuffer.add(sensorDataBuffer.frame[i]);
    00045
    00046     sensorDataRingBuffer.updateAverage(SensorData::accelerationX,10,gravity.x);
    00047     sensorDataRingBuffer.updateAverage(SensorData::accelerationY,10,gravity.y);
    00048     sensorDataRingBuffer.updateAverage(SensorData::accelerationZ,10,gravity.z);
    00049
    00050     sensorDataRingBuffer.updateAverage(SensorData::accelerationX,2,accelerationWithGrav.x);
    00051     sensorDataRingBuffer.updateAverage(SensorData::accelerationY,2,accelerationWithGrav.y);
    00052     sensorDataRingBuffer.updateAverage(SensorData::accelerationZ,2,accelerationWithGrav.z);
    00053
    00054
    00055     frameBodyPosture.frameNumber = sensorDataBuffer.frame[i].frameNumber;
    00056     calculateBodyPostureFromLegSensors(sensorDataBuffer.frame[i], frameBodyPosture);
    00057     calculateBodyPostureFromAccelerationSensors(sensorDataBuffer.frame[i], frameBodyPosture);
    00058
    00059     if (1) // if motion provides valid data...
    00060     {
    00061       frameBodyPosture.bodyTiltProvidedByMotionControl = motionInfo.bodyTilt;
    00062       frameBodyPosture.neckHeightProvidedByMotionControl = motionInfo.neckHeight;
    00063       frameBodyPosture.bodyRollProvidedByMotionControl = 0;
    00064     }
    00065     else
    00066     {
    00067       /** @todo copy data from "calculated from..."*/
    00068     }
    00069
    00070     buildCameraMatrix(sensorDataBuffer.frame[i], frameBodyPosture, frameCameraMatrix);
    00071
    00072     cameraMatrixRingBuffer.add(frameCameraMatrix);
    00073     bodyPostureRingBuffer.add(frameBodyPosture);
    00074
    00075     buildPSDPercept(
    00076       sensorDataBuffer.frame[i],
    00077       cameraMatrixRingBuffer.getEntry(9), // 9: hack
    00078       psdPercept[psdPercept.numOfPercepts++]);
    00079   }
    00080
    00081   double bodyRollAngle = atan2((double)-gravity.y,(double)-gravity.z);
    00082   if ((bodyRollAngle >= -gt2005SensorDataProcessorRolledOnWallAngle)||(bodyRollAngle <= -gt2005SensorDataProcessorFalldownRollAngle))
    00083     leftRollStartTime = SystemCall::getCurrentSystemTime();
    00084   if ((bodyRollAngle <= gt2005SensorDataProcessorRolledOnWallAngle)||(bodyRollAngle >= gt2005SensorDataProcessorFalldownRollAngle))
    00085     rightRollStartTime = SystemCall::getCurrentSystemTime();
    00086
    00087   if (lastImageFrameNumber == imageFrameNumber)
    00088   {
    00089     // The image is not new, so the camera matrix should not be built
    00090     return;
    00091   }
    00092   lastImageFrameNumber = imageFrameNumber;
    00093
    00094   // search image frame in sensor data
    00095   for (i = 0; i < sensorDataRingBuffer.getNumberOfEntries(); i++)
    00096   {
    00097     if (sensorDataRingBuffer.getEntry(i).frameNumber <= imageFrameNumber) break;
    00098   }
    00099   cameraMatrix = cameraMatrixRingBuffer.getEntry(i);
    00100   bodyPosture = bodyPostureRingBuffer.getEntry(i);
    00101   bodyPosture.bodyTiltProvidedByMotionControl = motionInfo.bodyTilt;
    00102   bodyPosture.bodyRollProvidedByMotionControl = 0;
    00103   bodyPosture.neckHeightProvidedByMotionControl = motionInfo.neckHeight;
    00104
    00105   Vector3<double> acceleration =
    00106     accelerationWithGrav - gravity;
    00107   bodyPercept.acceleration = acceleration;
    00108
    00109   int switches = detectSwitches();
    00110
    00111   bodyPercept.setFrameNumber(sensorDataBuffer.frame[0].frameNumber);
    00112
    00113   bodyPercept.setMouthState(detectMouthState());
    00114
    00115   bodyPercept.setBodyPSDHighValue(sensorDataBuffer.lastFrame().data[SensorData::bodyPsd]>150000); //BBWalk: 110000
    00116
    00117   bodyPercept.setBodyPSDDistance(getDistanceToSIFOC());
    00118
    00119     bodyPercept.setSwitches(switches);
    00120   if (!motionInfo.motionIsStable)
    00121   {
    00122     bodyPercept.setState(BodyPercept::standing);
    00123   }
    00124   else if (detectFallDown())
    00125   {
    00126     bodyPercept.setState(BodyPercept::crashed);
    00127     //OUTPUT (idText, text, "GT2005SensorDataProcessor : crashed");
    00128   }
    00129   else if (SystemCall::getTimeSince(leftRollStartTime)>1500)
    00130   {
    00131     bodyPercept.setState(BodyPercept::rollLeft);
    00132     //OUTPUT (idText, text, "GT2005SensorDataProcessor : roll left");
    00133   }
    00134   else if (SystemCall::getTimeSince(rightRollStartTime)>1500)
    00135   {
    00136     bodyPercept.setState(BodyPercept::rollRight);
    00137     //OUTPUT (idText, text, "GT2005SensorDataProcessor : roll right");
    00138   }
    00139   else if (detectPickup(gravity,acceleration))
    00140   {
    00141     bodyPercept.setState(BodyPercept::pickedUp);
    00142   }
    00143   else
    00144   {
    00145     bodyPercept.setState(BodyPercept::standing);
    00146   }
    00147
    00148   //double camSpeed = getCameraVelocity();
    00149 }
    00150
    00151
    00152 void GT2005SensorDataProcessor::buildCameraMatrix(const SensorData& sensorData, const BodyPosture& bP, CameraMatrix& cameraMatrix)
    00153 {
    00154   double tilt = fromMicroRad(sensorData.data[SensorData::neckTilt]);
    00155   double pan  = fromMicroRad(sensorData.data[SensorData::headPan]);
    00156   double tilt2 = fromMicroRad(sensorData.data[SensorData::headTilt]);
    00157   Kinematics::calculateCameraMatrix(tilt, pan, tilt2, bP.bodyTiltCalculatedFromLegSensors, bP.neckHeightCalculatedFromLegSensors, cameraMatrix);
    00158   cameraMatrix.setFrameNumber(sensorData.frameNumber);
    00159   //~ cameraMatrix.setFrameNumber(imageFrameNumber);
    00160   cameraMatrix.isValid=motionInfo.motionIsStable;
    00161 }
    00162
    00163 void GT2005SensorDataProcessor::buildPSDPercept(const SensorData& sensorData, const CameraMatrix& cameraMatrix, SinglePSDPercept& psdPercept)
    00164 {
    00165   // Achtung !!!
    00166   // Hier wird zunächst einmal die eventuell kritische Annahme getroffen,
    00167   // dass optische Achse und "PSD-Strahl"- Achse identisch sind.
    00168   // Das bleibt erst mal so, bis genaueres bekannt ist. (M.L.)
    00169
    00170   psdPercept.isValid = cameraMatrix.isValid;
    00171
    00172   // A vector in the camera coordinate system with the length of the measured psd distance
    00173   Vector3<double> psd;
    00174   psd.y = 0; psd.z = 0;
    00175
    00176   if(sensorData.data[SensorData::psd]/1000 < 300)
    00177     psd.x = sensorData.data[SensorData::psd]/1000 - 20;
    00178   else
    00179     psd.x = sensorData.data[SensorData::headPsdFar]/1000 - 30;
    00180   // mark the percept to be out of scope
    00181   psdPercept.tooFarAway = (psd.x >= 1300 - 30);
    00182   psdPercept.body = sensorData.data[SensorData::bodyPsd]/1000;
    00183
    00184   //
    00185   psdPercept.neckTilt = fromMicroRad(sensorData.data[SensorData::neckTilt]);
    00186
    00187   // Transformation of the vector into the robot's coordinate system
    00188   Vector3<double> objectRelativeToCamera = cameraMatrix.rotation * psd;
    00189
    00190   //OUTPUT(idText,text,"x: " << objectRelativeToCamera.x << ", y: " << objectRelativeToCamera.y << ", z: " << objectRelativeToCamera.z);
    00191
    00192   // calculates the spot detected by the PSD Sensor relative to the point on the
    00193   // ground below the shoulder joints.
    00194   (Vector3<double>&)psdPercept = objectRelativeToCamera + cameraMatrix.translation;
    00195
    00196   // OUTPUT(idText,text,"x2 " << psdPercept.x << ", y2 " << psdPercept.y << ", z2 " << psdPercept.z);
    00197
    00198   psdPercept.setFrameNumber(sensorData.frameNumber);
    00199 }
    00200
    00201 int GT2005SensorDataProcessor::detectSwitches()
    00202 {
    00203   int backMiddle, backFront, backBack,
    00204       head, mouth, chin;
    00205
    00206     backMiddle= (int)( (int)(sensorDataBuffer.frame[0].data[SensorData::back]) > 10  ) << BodyPercept::backMiddle;
    00207     backFront = (int)( (int)(sensorDataBuffer.frame[0].data[SensorData::backF]) > 10  ) << BodyPercept::backFront;
    00208     backBack  = (int)( (int)(sensorDataBuffer.frame[0].data[SensorData::backR]) > 10  ) << BodyPercept::backBack;
    00209     head      = (int)( (int)(sensorDataBuffer.frame[0].data[SensorData::head]) > 10  ) << BodyPercept::head;
    00210     mouth     = (int)(0 != (int)(sensorDataRingBuffer.getAverage(SensorData::mouth,     5)/-80000)) << BodyPercept::mouth;
    00211     chin      = (int)(0 != (int)(sensorDataRingBuffer.getAverage(SensorData::chin,      5)*10)) << BodyPercept::chin;
    00212
    00213     //for debug purposes
    00214     DEBUG_RESPONSE ("Buttons:press middle button",
    00215       backMiddle = 1 << BodyPercept::backMiddle;
    00216     );
    00217
    00218     DEBUG_RESPONSE ("Buttons:press front button",
    00219       backMiddle = 1 << BodyPercept::backFront;
    00220     );
    00221
    00222     DEBUG_RESPONSE ("Buttons:press back button",
    00223       backMiddle = 1 << BodyPercept::backBack;
    00224     );
    00225
    00226     DEBUG_RESPONSE ("Buttons:press head button",
    00227       backMiddle = 1 << BodyPercept::head;
    00228     );
    00229
    00230     DEBUG_RESPONSE ("Buttons:press mouth",
    00231       backMiddle = 1 << BodyPercept::mouth;
    00232     );
    00233
    00234     DEBUG_RESPONSE ("Buttons:press chin",
    00235       backMiddle = 1 << BodyPercept::chin;
    00236     );
    00237
    00238     DEBUG_RESPONSE ("Buttons:press power",
    00239       SystemCall::shutdown ();
    00240     );
    00241
    00242   return
    00243     backMiddle | backFront | backBack |
    00244     head | mouth | chin;
    00245 }
    00246
    00247 bool GT2005SensorDataProcessor::detectFallDown()
    00248 {
    00249   double bodyTiltAngle = atan2((double)gravity.x,(double)-gravity.z);
    00250   double bodyRollAngle = atan2((double)-gravity.y,(double)-gravity.z);
    00251
    00252   return
    00253     (bodyTiltAngle >  gt2005SensorDataProcessorFalldownTiltAngle  ||
    00254     bodyTiltAngle < -gt2005SensorDataProcessorFalldownTiltAngle  ||
    00255     bodyRollAngle >  gt2005SensorDataProcessorFalldownRollAngle  ||
    00256     bodyRollAngle < -gt2005SensorDataProcessorFalldownRollAngle);
    00257 }
    00258
    00259 bool GT2005SensorDataProcessor::detectPickup(const Vector3<double>& gravity, const Vector3<double>& acceleration)
    00260 {
    00261   double accZ = acceleration * gravity;
    00262
    00263   return (accZ < -50000000000000.0);
    00264 }
    00265
    00266 BodyPercept::MouthStates
    00267 GT2005SensorDataProcessor::detectMouthState()
    00268 {
    00269   if(sensorDataBuffer.lastFrame().data[SensorData::mouth] < -370000) //mouth open
    00270   {
    00271     return BodyPercept::mouthOpen;
    00272   }
    00273   else
    00274   {
    00275     return BodyPercept::mouthClosed;
    00276   }
    00277 }
    00278
    00279 /**************************/
    00280 /***  SensorDataBuffer  ***/
    00281
    00282 double GT2005SensorDataProcessor::SensorDataRingBuffer::getAverage (SensorData::sensors sensor, int ticks)
    00283 {
    00284   if (ticks>getNumberOfEntries()) ticks=getNumberOfEntries();
    00285   if (ticks<1) return 0.0;
    00286   double sum=0.0;
    00287   for (int i=0;i<ticks;i++)
    00288     sum+=getEntry(i).data[sensor];
    00289   return sum/(double)ticks;
    00290 }
    00291
    00292 void GT2005SensorDataProcessor::SensorDataRingBuffer::updateAverage(SensorData::sensors sensor, int ticks, double& average)
    00293 {
    00294   if (ticks<1) return;
    00295   if (ticks<getNumberOfEntries())
    00296     average-=((double)getEntry(ticks).data[sensor])/(double)ticks;
    00297   average+=((double)getEntry(0).data[sensor])/(double)ticks;
    00298 }
    00299
    00300 long GT2005SensorDataProcessor::SensorDataRingBuffer::interpolate (SensorData::sensors sensor, unsigned long imageFrame)
    00301 {
    00302   long frame = (long) imageFrame,
    00303     currentValue = getEntry(0).data[sensor],
    00304     currentFrame = (long) getEntry(0).frameNumber;
    00305   if(currentFrame == frame)
    00306     return currentValue;
    00307   else
    00308   {
    00309     long oldValue = getEntry(1).data[sensor],
    00310       oldFrame = (long) getEntry(1).frameNumber;
    00311     if(currentFrame == oldFrame)
    00312       return currentValue;
    00313     else
    00314       return currentValue + (oldValue - currentValue) * (frame - currentFrame) / (oldFrame - currentFrame);
    00315   }
    00316 }
    00317
    00318 void GT2005SensorDataProcessor::calculateBodyPostureFromLegSensors(const SensorData& sensorData, BodyPosture& bP)
    00319 {
    00320   RobotVertices robotVertices;
    00321   Kinematics::calcNeckAndLegPositions(sensorDataBuffer.lastFrame(), robotVertices);
    00322
    00323   bP.bodyTiltCalculatedFromLegSensors = robotVertices.bodyTilt;
    00324   bP.bodyRollCalculatedFromLegSensors = robotVertices.bodyRoll;
    00325   bP.neckHeightCalculatedFromLegSensors = robotVertices.neckHeight;
    00326 }
    00327
    00328 void GT2005SensorDataProcessor::calculateBodyPostureFromAccelerationSensors(const SensorData& sensorData, BodyPosture& bP)
    00329 {
    00330   //calculate body angles
    00331   long accX = sensorDataBuffer.lastFrame().data[SensorData::accelerationX]/1000;
    00332   long accY = sensorDataBuffer.lastFrame().data[SensorData::accelerationY]/1000;
    00333   long accZ = sensorDataBuffer.lastFrame().data[SensorData::accelerationZ]/1000;
    00334   bP.bodyTiltCalculatedFromAccelerationSensors = atan2((double)accX,(double)-accZ);
    00335   bP.bodyRollCalculatedFromAccelerationSensors = atan2((double)-accY,(double)-accZ);
    00336 }
    00337
    00338 double GT2005SensorDataProcessor::getCameraVelocity(const SensorData::sensors joint=SensorData::headPan)
    00339 {
    00340   const int numOfValues=4;
    00341   int jointNum = 1;
    00342   sensorDataRingBuffer.updateAverage(SensorData::neckTilt, numOfValues, actAverage[0]);
    00343   sensorDataRingBuffer.updateAverage(SensorData::headPan, numOfValues, actAverage[1]);
    00344   sensorDataRingBuffer.updateAverage(SensorData::headTilt, numOfValues, actAverage[2]);
    00345   switch (joint)
    00346   {
    00347   case SensorData::headTilt: jointNum = 2;
    00348     break;
    00349   case SensorData::headPan: jointNum = 1;
    00350     break;
    00351   case SensorData::neckTilt: jointNum = 0;
    00352     break;
    00353   }
    00354   double cameraSpeed = toDegrees(fromMicroRad((long) (actAverage[jointNum] - previousAverage[jointNum]))) / RobotDimensions::motionCycleTime;
    00355   for (int index = 0; index < 3; index++)
    00356     previousAverage[index] = actAverage[index];
    00357   /**
    00358   if (abs ((int) cameraSpeed) > 50)
    00359     if(SystemCall::getCurrentSystemTime()-tempSysTime > 250)
    00360     {
    00361       tempSysTime=SystemCall::getCurrentSystemTime();
    00362       OUTPUT(idText, text, cameraSpeed);
    00363     }
    00364   */
    00365   return cameraSpeed;
    00366 }
    00367
    00368 double GT2005SensorDataProcessor::getDistanceToSIFOC()
    00369 {
    00370   double value = 50;
    00371   double psdval = 0;
    00372   for (int i = 0; i < psdPercept.numOfPercepts; ++i)
    00373     psdval += psdPercept[i].body;
    00374   psdval /= (double)(psdPercept.numOfPercepts);
    00375   if (psdval >= 170)
    00376     psdval = 170;
    00377
    00378   // from 160 to 170 equals from 1cm to 0cm
    00379   if (psdval >= 160)
    00380     value = 10-(psdval-160);
    00381   else
    00382   // from 145 to 160 equals from 2cm to 1cm
    00383   if (psdval >= 145)
    00384     value = ((15-(psdval-145))/1.5)+10;
    00385   else
    00386   // from 130 to 145 equals from 4cm to 2cm
    00387   if (psdval >= 130)
    00388     value = ((15-(psdval-130))/0.75)+20;
    00389
    00390   return value;
    00391 }