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

# Platform/Aperios1.3.2/Sensors.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file Platform/Aperios1.3.2/Sensors.cpp
    00003 *
    00004 * Implementation of system dependend streaming operators for images and sensor data
    00005 *
    00006 * @author Martin Lötzsch
    00007 * @author Sebastian Petters
    00008 * @author Thomas Röfer
    00009 * @author Uwe Düffert
    00010 */
    00011
    00012 #include "Sensors.h"
    00013 #include <OPENR/OPENRAPI.h>
    00014 #include <iostream.h>
    00015 #include <OPENR/SharedMemoryHeader.h>
    00016 //#include <OPENR/OPrimitiveControl.h>
    00017 #include "Tools/Streams/InStreams.h"
    00018 #include "Tools/Streams/OutStreams.h"
    00019 #include "Tools/Location.h"
    00020 #include "Tools/Actorics/RobotDimensions.h"
    00021 #include "Representations/Perception/CameraParameters.h"
    00022 #include "SystemCall.h"
    00023
    00024 Sensors::Sensors ()
    00025 {
    00026   InTextFile fin(getLocation().getModelFilename("camera.cfg"));
    00027   if (fin.exists())
    00028   {
    00029     fin >> cameraParameters;
    00030   }
    00031   else
    00032   {
    00033     cameraParameters.setWhiteBalance(CameraParameters::wb_indoor_mode);
    00034     cameraParameters.setGain        (CameraParameters::gain_low);
    00035     cameraParameters.setShutterSpeed(CameraParameters::shutter_mid);
    00036   }
    00037
    00038   setCameraParameters(cameraParameters);  // it occurred to me that the parameters should not only be read from the file but also set into action, hope this fixes the problem :-) - ronnie brunn
    00039 }
    00040
    00041 void Sensors::setCameraParameters(CameraParameters &cameraParameters)
    00042 {
    00043   static OPrimitiveID fbkID = 0;
    00044   OPENR::OpenPrimitive("PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1", &fbkID);
    00045   OPrimitiveControl_CameraParam wb(cameraParameters.getWhiteBalance() + 1);
    00046   OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_WHITE_BALANCE, &wb, sizeof(wb), 0, 0);
    00047   OPrimitiveControl_CameraParam gain(cameraParameters.getGain() + 1);
    00048   OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_GAIN, &gain, sizeof(gain), 0, 0);
    00049   OPrimitiveControl_CameraParam shutter(cameraParameters.getShutterSpeed() + 1);
    00050   OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_SHUTTER_SPEED, &shutter, sizeof(shutter), 0, 0);
    00051 }
    00052
    00053 CameraParameters Sensors::getCameraParameters()
    00054 {
    00055   return cameraParameters;
    00056 }
    00057
    00058 In& operator>>(In& stream,Image& image)
    00059 {
    00060   //only read the header
    00061   unsigned char buffer[512];
    00062   stream.read(buffer,512);
    00063
    00064   OFbkImageVectorData* data = reinterpret_cast<OFbkImageVectorData*>(buffer);
    00065   image.cameraInfo.resolutionHeight = data->GetInfo(0)->height;
    00066   image.cameraInfo.resolutionWidth = data->GetInfo(0)->width;
    00067   image.frameNumber = data->GetInfo(0)->frameNumber;
    00068   //int numOfBands = data->GetInfo(0)->dataSize / image.cameraInfo.resolutionHeight / image.cameraInfo.resolutionWidth;
    00069
    00070   // this code even works if rows in "image" are longer than the data received
    00071   for(int y=0; y < image.cameraInfo.resolutionHeight; y++)
    00072   {
    00073     for(int c = 0; c < 6; ++c)
    00074       stream.read(&image.image[y][c][0], image.cameraInfo.resolutionWidth);
    00075     //stream.skip(image.cameraInfo.resolutionWidth * 3);
    00076   }
    00077   stream.skip(1000000); // skip anything else
    00078
    00079   image.setCameraInfo();
    00080
    00081   for (int i = 0; i < 16; i++)
    00082   {
    00083     image.image[image.cameraInfo.resolutionHeight-1][0][i] = image.image[image.cameraInfo.resolutionHeight-2][0][i];
    00084     image.image[image.cameraInfo.resolutionHeight-1][1][i] = image.image[image.cameraInfo.resolutionHeight-2][1][i];
    00085     image.image[image.cameraInfo.resolutionHeight-1][2][i] = image.image[image.cameraInfo.resolutionHeight-2][2][i];
    00086   }
    00087
    00088   return stream;
    00089 }
    00090 In& operator>>(In& stream,SensorDataBuffer& sensorDataBuffer)
    00091 {
    00092   static const int translateERS7[] =
    00093   {
    00094     SensorData::mouth,
    00095     SensorData::chin,
    00096     SensorData::headTilt,
    00097     SensorData::head,
    00098     SensorData::headPsdNear,
    00099     SensorData::headPsdFar,
    00100     SensorData::headPan,
    00101     SensorData::neckTilt,
    00102     SensorData::pawFL,
    00103     SensorData::legFL3,
    00104     SensorData::legFL2,
    00105     SensorData::legFL1,
    00106     SensorData::pawHL,
    00107     SensorData::legHL3,
    00108     SensorData::legHL2,
    00109     SensorData::legHL1,
    00110     SensorData::pawFR,
    00111     SensorData::legFR3,
    00112     SensorData::legFR2,
    00113     SensorData::legFR1,
    00114     SensorData::pawHR,
    00115     SensorData::legHR3,
    00116     SensorData::legHR2,
    00117     SensorData::legHR1,
    00118     SensorData::tailTilt,
    00119     SensorData::tailPan,
    00120     SensorData::accelerationX,
    00121     SensorData::accelerationY,
    00122     SensorData::accelerationZ,
    00123     SensorData::bodyPsd,
    00124     SensorData::wlan,
    00125     SensorData::backR,
    00126     SensorData::backM,
    00127     SensorData::backF
    00128   };
    00129
    00130 #ifdef __GNUC__
    00131   char buffer[12288];
    00132 #else // GreenHills
    00133   char buffer[9120]; // this should never be used with ERS-7
    00134 #endif
    00135   bool isERS210 = SystemCall::getRobotDesign() == RobotDesign::ERS210;
    00136   stream.read(buffer, isERS210 ? sizeof(buffer) : 9984);
    00137
    00138   OSensorFrameVectorData& data = *(OSensorFrameVectorData*) buffer;
    00139
    00140   sensorDataBuffer.numOfFrames = data.GetInfo(0)->numFrames;
    00141
    00142   for (int j = 0; j < sensorDataBuffer.numOfFrames; j++)
    00143   {
    00144     if(isERS210)
    00145       for (int i = 0; i < SensorData::numOfSensor_ERS210; ++i)
    00146       {
    00147         OSensorFrameData* pData = data.GetData(i);
    00148         sensorDataBuffer.frame[j].data[i] = pData->frame[j].value;
    00149
    00150         if (data.GetInfo(i)->type == odataJOINT_VALUE)
    00151           sensorDataBuffer.frame[j].refValue[i] =
    00152           ((OJointValue*)(&(pData->frame[j])))->refValue;
    00153       }
    00154     else
    00155       for (int i = 0; i < SensorData::numOfSensor_ERS7; ++i)
    00156       {
    00157         int index = translateERS7[i];
    00158         OSensorFrameData* pData = data.GetData(i);
    00159         sensorDataBuffer.frame[j].data[index] = pData->frame[j].value;
    00160
    00161         if (data.GetInfo(i)->type == odataJOINT_VALUE)
    00162           sensorDataBuffer.frame[j].refValue[index] =
    00163           ((OJointValue*)(&(pData->frame[j])))->refValue;
    00164       }
    00165
    00166     //open-r coordinates different than ours
    00167     sensorDataBuffer.frame[j].data[SensorData::accelerationY]=
    00168       -sensorDataBuffer.frame[j].data[SensorData::accelerationY];
    00169
    00170     sensorDataBuffer.frame[j].frameNumber = data.GetInfo(0)->frameNumber + j;
    00171   }
    00172
    00173   return stream;
    00174 }