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 () 
{
  InBinaryFile fin(getLocation().getModelFilename("camera.cfg"));
  if (fin.exists())
  {
    fin >> cameraParameters;
  }
  else
  {
    cameraParameters.theWhiteBalance = CameraParameters::wb_indoor_mode;
    cameraParameters.theGain         = CameraParameters::gain_low;
    cameraParameters.theShutterSpeed = 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.theWhiteBalance + 1);
  OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_WHITE_BALANCE, &wb, sizeof(wb), 0, 0);
  OPrimitiveControl_CameraParam gain(cameraParameters.theGain + 1);
  OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_GAIN, &gain, sizeof(gain), 0, 0);
  OPrimitiveControl_CameraParam shutter(cameraParameters.theShutterSpeed + 1);
  OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_SHUTTER_SPEED, &shutter, sizeof(shutter), 0, 0);
}

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;
}

/*
 * Change log :
 * 
 * $Log: Sensors.cpp,v $
 * Revision 1.3  2004/05/27 17:13:37  jhoffman
 * - renaming: tilt1 -> neckTilt,  pan -> headPan,  tilt2 -> headTilt
 * - clipping included for setJoints
 * - removed some microrad/rad-bugs
 * - bodyPosture constructor and "=" operator fixed
 *
 * Revision 1.2  2004/05/27 10:08:08  thomas
 * added model-specific locations
 *
 * Revision 1.1.1.1  2004/05/22 17:23:35  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.9  2004/04/07 13:00:47  risler
 * ddd checkin after go04 - second part
 *
 * Revision 1.3  2004/04/07 11:44:06  risler
 * added sending low res images
 * added Image::setCameraInfo
 *
 * Revision 1.2  2004/03/29 15:19:04  Marc
 * Intruduced the Black and White Image
 * Normal Images (not Jpeg) images were now send as Color Image with BW
 *
 * Revision 1.8  2004/03/17 12:35:13  nistico
 * Fixed problem with loading of intrinsic camera parameters
 *
 * Revision 1.7  2004/01/13 11:57:38  dueffert
 * doxygen comment corrected
 *
 * Revision 1.6  2004/01/05 16:52:53  risler
 * ERS7 Sensor translation fixed
 *
 * Revision 1.5  2003/12/31 20:16:13  roefer
 * SensorData for ERS-7
 *
 * Revision 1.4  2003/12/30 20:12:03  roefer
 * Image size is now 208 x 160. Smaller images are placed in the upper left corner
 *
 * Revision 1.3  2003/12/15 11:49:09  juengel
 * Introduced CameraInfo
 *
 * Revision 1.2  2003/11/12 12:42:44  dueffert
 * image processing now works with sdk-1.1.3-r2 AND sdk-1.1.4-r1
 *
 * Revision 1.1  2003/10/07 10:06:59  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.3  2003/09/30 10:51:12  dueffert
 * typos fixed
 *
 * Revision 1.2  2003/09/26 15:30:28  juengel
 * Renamed DataTypes to representations.
 *
 * 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.11  2003/05/11 17:03:00  risler
 * added location.cfg
 *
 * Revision 1.10  2003/05/03 15:26:29  risler
 * added refValue to SensorData
 *
 * Revision 1.9  2003/05/02 18:26:18  risler
 * SensorDataBuffer added
 * replaced SensorData with SensorDataBuffer
 * full SensorData resolution now accessible
 *
 * Revision 1.8  2003/04/19 04:47:19  pruente
 * removed comment lines for not confusing the reader
 *
 * Revision 1.7  2003/04/19 04:46:15  pruente
 * Merged in changes by Andr\'{e} at GO2003:
 * - Made camera.cfg fail-proof
 *
 * - Fixed "first 16 pixels of last line" image bug
 *   Beautified fail-proof camera.cfg strategy
 *
 * Revision 1.6  2003/04/04 21:28:35  roefer
 * Communication protocol changed
 *
 * Revision 1.5  2003/02/17 10:41:00  dueffert
 * greenhills backport
 *
 * Revision 1.4  2003/02/10 13:59:26  dueffert
 * gcc32 adaption
 *
 * Revision 1.3  2002/12/05 23:17:23  roefer
 * Camera parameters corrected
 *
 * Revision 1.2  2002/09/16 15:55:16  dueffert
 * Martins changes to Win32 adapted to Aperios
 *
 * Revision 1.1  2002/09/10 15:40:04  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.6  2002/08/30 17:10:57  dueffert
 * removed unused includes
 *
 * Revision 1.5  2002/07/25 14:31:27  roefer
 * gcc compilation errors fixed
 *
 * Revision 1.4  2002/07/23 13:39:39  loetzsch
 * - new streaming classes
 * - removed many #include statements
 * - new design of debugging architecture
 * - exchanged StaticQueue with MessageQueue
 *
 * Revision 1.3  2002/07/13 10:54:58  roefer
 * New command and sound sender
 *
 * Revision 1.2  2002/05/14 21:06:58  hebbel
 * in operator for SoundDataIn included
 *
 * Revision 1.1.1.1  2002/05/10 12:40:18  cvsadm
 * Moved GT2002 Project from ute to tamara.
 *
 * Revision 1.7  2002/03/28 17:44:45  loetzsch
 * new streaming operator caused debug assertions
 *
 * Revision 1.6  2002/03/27 15:25:07  brunn
 * camera parameters werden ab jetzt nicht nur geladen sondern auch mit den geladenen werten initialisiert ;-)
 *
 * Revision 1.5  2002/03/26 18:59:23  loetzsch
 * enabled faster streaming operator
 *
 * Revision 1.4  2002/02/01 15:02:41  risler
 * Acceleration sensor adjusted to coordinate system
 *
 * Revision 1.3  2002/01/23 07:47:25  loetzsch
 * der In >>  - Streaming operator liest nun auch die frameNumber
 *
 * Revision 1.2  2001/12/19 18:37:32  bach
 * Camera.cfg added
 * Sensors: CameraParameters are set
 *
 * Revision 1.1  2001/12/19 16:03:55  bach
 * SystemDataTypes replaced by Sensors
 *
 *
 */

---