Platform/Aperios1.3.2/Sensors.cpp

Go to the documentation of this file.

/** 
* @file Platform/Aperios1.3.2/Sensors.cpp
*
* Implementation of system dependend streaming operators for images and sensor data
*
* @author Martin Lötzsch
* @author Sebastian Petters
* @author Thomas Röfer
* @author Uwe Düffert
*/ 

#include "Sensors.h"
#include <OPENR/OPENRAPI.h>
#include <iostream.h>
#include <OPENR/SharedMemoryHeader.h>
//#include <OPENR/OPrimitiveControl.h>
#include "Tools/Streams/InStreams.h"
#include "Tools/Streams/OutStreams.h"
#include "Tools/Location.h"
#include "Tools/Actorics/RobotDimensions.h"
#include "Representations/Perception/CameraParameters.h"
#include "SystemCall.h"

Sensors::Sensors () 
{
  InTextFile fin(getLocation().getModelFilename("camera.cfg"));
  if (fin.exists())
  {
    fin >> cameraParameters;
  }
  else
  {
    cameraParameters.setWhiteBalance(CameraParameters::wb_indoor_mode);
    cameraParameters.setGain        (CameraParameters::gain_low);
    cameraParameters.setShutterSpeed(CameraParameters::shutter_mid);
  }
  
  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
}

void Sensors::setCameraParameters(CameraParameters &cameraParameters)
{
  static OPrimitiveID fbkID = 0;
  OPENR::OpenPrimitive("PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1", &fbkID);
  OPrimitiveControl_CameraParam wb(cameraParameters.getWhiteBalance() + 1);
  OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_WHITE_BALANCE, &wb, sizeof(wb), 0, 0);
  OPrimitiveControl_CameraParam gain(cameraParameters.getGain() + 1);
  OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_GAIN, &gain, sizeof(gain), 0, 0);
  OPrimitiveControl_CameraParam shutter(cameraParameters.getShutterSpeed() + 1);
  OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_SHUTTER_SPEED, &shutter, sizeof(shutter), 0, 0);
}

CameraParameters Sensors::getCameraParameters()
{
  return cameraParameters;
}

In& operator>>(In& stream,Image& image)
{  
  //only read the header
  unsigned char buffer[512];
  stream.read(buffer,512);
  
  OFbkImageVectorData* data = reinterpret_cast<OFbkImageVectorData*>(buffer);
  image.cameraInfo.resolutionHeight = data->GetInfo(0)->height;
  image.cameraInfo.resolutionWidth = data->GetInfo(0)->width;
  image.frameNumber = data->GetInfo(0)->frameNumber;
  //int numOfBands = data->GetInfo(0)->dataSize / image.cameraInfo.resolutionHeight / image.cameraInfo.resolutionWidth;
  
  // this code even works if rows in "image" are longer than the data received
  for(int y=0; y < image.cameraInfo.resolutionHeight; y++)
  {
    for(int c = 0; c < 6; ++c)
      stream.read(&image.image[y][c][0], image.cameraInfo.resolutionWidth);
    //stream.skip(image.cameraInfo.resolutionWidth * 3);
  }
  stream.skip(1000000); // skip anything else

  image.setCameraInfo();

  for (int i = 0; i < 16; i++)
  {
    image.image[image.cameraInfo.resolutionHeight-1][0][i] = image.image[image.cameraInfo.resolutionHeight-2][0][i];
    image.image[image.cameraInfo.resolutionHeight-1][1][i] = image.image[image.cameraInfo.resolutionHeight-2][1][i];
    image.image[image.cameraInfo.resolutionHeight-1][2][i] = image.image[image.cameraInfo.resolutionHeight-2][2][i];
  }
  
  return stream;
}
In& operator>>(In& stream,SensorDataBuffer& sensorDataBuffer)
{
  static const int translateERS7[] =
  {
    SensorData::mouth,
    SensorData::chin,
    SensorData::headTilt,
    SensorData::head,
    SensorData::headPsdNear, 
    SensorData::headPsdFar,
    SensorData::headPan, 
    SensorData::neckTilt,
    SensorData::pawFL, 
    SensorData::legFL3, 
    SensorData::legFL2, 
    SensorData::legFL1,
    SensorData::pawHL, 
    SensorData::legHL3, 
    SensorData::legHL2, 
    SensorData::legHL1,
    SensorData::pawFR, 
    SensorData::legFR3, 
    SensorData::legFR2, 
    SensorData::legFR1,
    SensorData::pawHR, 
    SensorData::legHR3, 
    SensorData::legHR2, 
    SensorData::legHR1,
    SensorData::tailTilt,
    SensorData::tailPan, 
    SensorData::accelerationX, 
    SensorData::accelerationY, 
    SensorData::accelerationZ,
    SensorData::bodyPsd,
    SensorData::wlan,
    SensorData::backR,
    SensorData::backM, 
    SensorData::backF
  };

#ifdef __GNUC__
  char buffer[12288];
#else // GreenHills
  char buffer[9120]; // this should never be used with ERS-7
#endif
  bool isERS210 = SystemCall::getRobotDesign() == RobotDesign::ERS210;
  stream.read(buffer, isERS210 ? sizeof(buffer) : 9984);
  
  OSensorFrameVectorData& data = *(OSensorFrameVectorData*) buffer;
  
  sensorDataBuffer.numOfFrames = data.GetInfo(0)->numFrames;
  
  for (int j = 0; j < sensorDataBuffer.numOfFrames; j++)
  {
    if(isERS210)
      for (int i = 0; i < SensorData::numOfSensor_ERS210; ++i)
      {
        OSensorFrameData* pData = data.GetData(i);
        sensorDataBuffer.frame[j].data[i] = pData->frame[j].value;
        
        if (data.GetInfo(i)->type == odataJOINT_VALUE)
          sensorDataBuffer.frame[j].refValue[i] = 
          ((OJointValue*)(&(pData->frame[j])))->refValue;
      }
    else
      for (int i = 0; i < SensorData::numOfSensor_ERS7; ++i)
      {
        int index = translateERS7[i];
        OSensorFrameData* pData = data.GetData(i);
        sensorDataBuffer.frame[j].data[index] = pData->frame[j].value;
        
        if (data.GetInfo(i)->type == odataJOINT_VALUE)
          sensorDataBuffer.frame[j].refValue[index] = 
          ((OJointValue*)(&(pData->frame[j])))->refValue;
      }

    //open-r coordinates different than ours
    sensorDataBuffer.frame[j].data[SensorData::accelerationY]=
      -sensorDataBuffer.frame[j].data[SensorData::accelerationY];
    
    sensorDataBuffer.frame[j].frameNumber = data.GetInfo(0)->frameNumber + j;
  }
  
  return stream;
}

---