DCData.cpp

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003, 2004    (see file CONTACTS for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#include "DCData.hh"
#include <Base/Debug/Error.hh>
#include <string>
#include <cmath>

// for gcc 4.7
#ifndef WIN32
#include <unistd.h>
#endif

#include <Base/Common/W32Compat.hh>

using namespace BIAS;
using namespace std;

//////////////////////////////////////////////////////////////////////////
//                    DC_ptu
/////////////////////////////////////////////////////////////////////////

DC_ptu::DC_ptu() : Debug()
{
  PanPos = TiltPos = 0.0;
  TimeStamp = StartReadTimeStamp = StopReadTimeStamp = 0;
}

DC_ptu::DC_ptu(const DC_ptu& ptu) : Debug()
{
  PanPos            = ptu.PanPos             ;
  TiltPos           = ptu.TiltPos            ;
  TimeStamp         = ptu.TimeStamp          ;
  StartReadTimeStamp= ptu.StartReadTimeStamp ;
  StopReadTimeStamp = ptu.StopReadTimeStamp  ;
}

DC_ptu::~DC_ptu()
{
}

void DC_ptu::Set(double panpos, double tiltpos, unsigned long int timestamp,
                 unsigned long startts, unsigned long stopts)
{
  PanPos = panpos;
  TiltPos = tiltpos;
  TimeStamp = timestamp;
  StartReadTimeStamp=startts;
  StopReadTimeStamp=stopts;
}

void DC_ptu::WriteBinary(char *data)
{
#ifdef BIAS_DEBUG  
  if (!data) {
    BIASERR("data is NULL");
    BIASASSERT(false);
  }
#endif
  /* char *d=data;
  (*((double *)d)++)=PanPos;
  (*((double *)d)++)=TiltPos;
  (*((unsigned long *)d)++)=TimeStamp;
  (*((unsigned long *)d)++)=StartReadTimeStamp;
  (*((unsigned long *)d)++)=StopReadTimeStamp;*/
  double *d = (double*)data;
  d[0] = PanPos;
  d[1] = TiltPos;
  unsigned long *l = (unsigned long *)(d+2);
  l[0] = TimeStamp;
  l[1] = StartReadTimeStamp;
  l[3] = StopReadTimeStamp;
}

void DC_ptu::ReadBinary(char *data)
{
  /*char *d=data;
  PanPos=(*((double *)d)++);
  TiltPos=(*((double *)d)++);
  TimeStamp=(*((unsigned long *)d)++);
  StartReadTimeStamp=(*((unsigned long *)d)++);
  StopReadTimeStamp=(*((unsigned long *)d)++);*/
  double *d = (double *)data;
  PanPos = d[0];
  TiltPos = d[1];
  unsigned long *l = (unsigned long *)(d+2);
  TimeStamp = l[0];
  StartReadTimeStamp = l[1];
  StopReadTimeStamp = l[2];
}

istream& BIAS::operator>>(istream& is, DC_ptu& ptu)
{
  string tmp;
  //  char header[512];
  //is.getline(header, 512, '\n'); 
  //if (strcmp(header, "#[PTU Data]")==0){
    is >> tmp >> ptu.PanPos >> tmp;
    is >> tmp;
    if (tmp=="#TiltPos="){
      is >> ptu.TiltPos >> tmp;
      is >> tmp >> ptu.TimeStamp;
      ptu.StartReadTimeStamp = ptu.StopReadTimeStamp = ptu.TimeStamp;
    } else if (tmp=="#ImageTimeStamp="){
      is >> ptu.TimeStamp;
      is >> tmp >> ptu.TiltPos >> tmp;
      is >> tmp >> ptu.StartReadTimeStamp;
      is >> tmp >> ptu.StopReadTimeStamp;
    }
    // cerr << ptu << endl;
    //} else {
    //  BIASERR("unable to read ptu data, invalid header " << tmp);
    //}
    if (!is) {
      BIASERR("error reading ptu meta data from ascii stream");
      biassleep(1);
    }
  return is;
}

ostream& BIAS::operator<<(ostream& os, const DC_ptu& ptu)
{
  //#ifdef BIAS_DEBUG
  streambuf *OutBuf_=cout.rdbuf(), *OsBuf_= os.rdbuf(), 
    *ErrBuf_=cerr.rdbuf();

  if (OutBuf_==OsBuf_ || ErrBuf_==OsBuf_){ // cout or cerr
    //if (OutBuf_==OsBuf_) cerr << "os == cout\n";
    //if (ErrBuf_==OsBuf_) cerr << "os == cerr\n";
      
    os << "PTU:\tpan: "<<ptu.PanPos<<"\ttilt: "<<ptu.TiltPos
       << "\ttime: "<<ptu.TimeStamp<<"\tStartReadTime: "
       <<ptu.StartReadTimeStamp<<"\tStopReadTime: "<<ptu.StopReadTimeStamp;
  } else {
    //#endif
    os << "#PanPos= "<<ptu.PanPos<<"  #[rad]"<<endl
       << "#ImageTimeStamp= "<<ptu.TimeStamp<<endl
       << "#TiltPos= "<<ptu.TiltPos<<"  #[rad]"<<endl
       << "#PTUStartReadTimeStamp= "<<ptu.StartReadTimeStamp<<endl
       << "#PTUStopReadTimeStamp= "<<ptu.StopReadTimeStamp;
    //#ifdef BIAS_DEBUG
  }
  //#endif
  return os;
}

//////////////////////////////////////////////////////////////////////////
//                    DC_inertial
/////////////////////////////////////////////////////////////////////////

DC_inertial::DC_inertial() : Debug()
{
  Yaw = YawRateRaw = YawRateOffset = 0.0;
  SteeringAngle = Pitch = Speed = 0.0;
  LongAcceleration = LatAcceleration = 0.0;
  TempomatLever = BlinkLever = SteeringWheelLever = 0;
  ArtLeadVehicleDistance = ArtLeadVehicleVelocity = ArtSollVelocity = 0.0;
  ArtFlags = 0;
  AcceleratorPedal = EngineSpeed = 0.0;
  Gear = VehicleFlags = 0;
}

DC_inertial::DC_inertial(const DC_inertial& inert) : Debug()
{
  Yaw                      = inert.Yaw;                     
  YawRateRaw               = inert.YawRateRaw;              
  YawRateOffset            = inert.YawRateOffset;           
  SteeringAngle            = inert.SteeringAngle;           
  Pitch                    = inert.Pitch;                   
  Speed                    = inert.Speed;                   
  LongAcceleration         = inert.LongAcceleration;        
  LatAcceleration          = inert.LatAcceleration;         
  TempomatLever            = inert.TempomatLever;           
  BlinkLever               = inert.BlinkLever;              
  SteeringWheelLever       = inert.SteeringWheelLever;      
  ArtLeadVehicleDistance   = inert.ArtLeadVehicleDistance;  
  ArtLeadVehicleVelocity   = inert.ArtLeadVehicleVelocity;  
  ArtSollVelocity          = inert.ArtSollVelocity;         
  ArtFlags                 = inert.ArtFlags;                
  AcceleratorPedal         = inert.AcceleratorPedal;        
  EngineSpeed              = inert.EngineSpeed;             
  Gear                     = inert.Gear;                    
  VehicleFlags             = inert.VehicleFlags;            
}

DC_inertial::~DC_inertial()
{
}

void DC_inertial::Set(double yaw, double yawrateraw, double yawrateoffset, 
                      double speed, double steeringangle)
{
  Yaw = yaw;
  YawRateRaw = yawrateraw;
  YawRateOffset = yawrateoffset;
  Speed = speed;
  SteeringAngle = steeringangle;
}

istream& BIAS::operator>>(istream& is, DC_inertial& inert)
{
  string tmp;
  //  char header[512];
  //is.getline(header, 512, '\n'); 
  // cerr << header << endl;
  //if (strcmp(header, "#[Inertial Sensor]")==0){
    is >> tmp >> inert.Yaw;  
    is >> tmp >> inert.YawRateRaw;              
    is >> tmp >> inert.YawRateOffset;           
    is >> tmp >> inert.SteeringAngle;           
    is >> tmp >> inert.Pitch;                   
    is >> tmp >> inert.Speed;                   
    is >> tmp >> inert.LongAcceleration;        
    is >> tmp >> inert.LatAcceleration;         
    is >> tmp >> inert.TempomatLever;           
    is >> tmp >> inert.BlinkLever;              
    is >> tmp >> inert.SteeringWheelLever;      
    is >> tmp >> inert.ArtLeadVehicleDistance;  
    is >> tmp >> inert.ArtLeadVehicleVelocity;  
    is >> tmp >> inert.ArtSollVelocity;         
    is >> tmp >> inert.ArtFlags;                
    is >> tmp >> inert.AcceleratorPedal;        
    is >> tmp >> inert.EngineSpeed;             
    is >> tmp >> inert.Gear;                    
    is >> tmp >> inert.VehicleFlags;            
    
    //cerr << inert << endl;
    //} else {
    //  BIASERR("unable to read inertial sensor data, invalid header "
    //   <<tmp);
    //}
    if (!is) {
      BIASERR("error reading inertial meta data from ascii stream");
      biassleep(1);
    }
  return is;
}

ostream& BIAS::operator<<(ostream& os, const DC_inertial& inert)
{
  //#ifdef BIAS_DEBUG
  streambuf *OutBuf_=cout.rdbuf(), *OsBuf_= os.rdbuf(), 
    *ErrBuf_=cerr.rdbuf();

  if (OutBuf_==OsBuf_ || ErrBuf_==OsBuf_){ // cout or  cerr
    os << "INERT: "
       <<"   Speed: "<<inert.Speed
       <<"   StAng: "<<inert.SteeringAngle
       <<"   Yaw: "<<inert.Yaw;
      //<<"   YawRateRaw: "<<inert.YawRateRaw
      //<<"   YawRateOffset: "<<inert.YawRateOffset
      //<<"   LongAccel: "<<inert.LongAcceleration;
  } else {
    //#endif
    os //<< "#[Inertial Sensor]"<<endl
      << "#Yaw= "<<inert.Yaw<<endl
      << "#YawRateRaw= "<<inert.YawRateRaw<<endl
      << "#YawRateOffset= "<<inert.YawRateOffset<<endl
      << "#SteeringAngle= "<<inert.SteeringAngle<<endl
      << "#Pitch= "<<inert.Pitch<<endl
      << "#Speed= "<<inert.Speed<<endl
      << "#LongAcceleration= "<<inert.LongAcceleration<<endl
      << "#LatAcceleration= "<<inert.LatAcceleration<<endl
      << "#TempomatLever= "<<inert.TempomatLever<<endl
      << "#BlinkLever= "<<inert.BlinkLever<<endl
      << "#SteeringWheelLever= "<<inert.SteeringWheelLever<<endl
      << "#ArtLeadVehicleDistance= "<<inert.ArtLeadVehicleDistance<<endl
      << "#ArtLeadVehicleVelocity= "<<inert.ArtLeadVehicleVelocity<<endl
      << "#ArtSollVelocity= "<<inert.ArtSollVelocity<<endl
      << "#ArtFlags= "<<inert.ArtFlags<<endl
      << "#AcceleratorPedal= "<<inert.AcceleratorPedal<<endl
      << "#EngineSpeed= "<<inert.EngineSpeed<<endl
      << "#Gear= "<<inert.Gear<<endl
      << "#VehicleFlags= "<<inert.VehicleFlags;
    //#ifdef BIAS_DEBUG
  }
  //#endif
  return os;
}

void DC_inertial::WriteBinary(char *data)
{
#ifdef BIAS_DEBUG  
  if (!data) {
    BIASERR("data is NULL");
    BIASASSERT(false);
  }
#endif
  /*char *d = data;
  (*((double *)d)++)=Yaw; 
  (*((double *)d)++)=YawRateRaw;
  (*((double *)d)++)=YawRateOffset;
  (*((double *)d)++)=SteeringAngle;
  (*((double *)d)++)=Pitch;
  (*((double *)d)++)=Speed;
  (*((double *)d)++)=LongAcceleration;
  (*((double *)d)++)=LatAcceleration;
  (*((int *)d)++)=TempomatLever;
  (*((int *)d)++)=BlinkLever;
  (*((int *)d)++)=SteeringWheelLever;
  (*((double *)d)++)=ArtLeadVehicleDistance;
  (*((double *)d)++)=ArtLeadVehicleVelocity;
  (*((double *)d)++)=ArtSollVelocity;
  (*((int *)d)++)=ArtFlags;
  (*((double *)d)++)=AcceleratorPedal;
  (*((double *)d)++)=EngineSpeed;
  (*((int *)d)++)=Gear;
  (*((int *)d)++)=VehicleFlags;*/
  double *d = (double *)data;
  d[0] = Yaw; 
  d[1] = YawRateRaw;
  d[2] = YawRateOffset;
  d[3] = SteeringAngle;
  d[4] = Pitch;
  d[5] = Speed;
  d[6] = LongAcceleration;
  d[7] = LatAcceleration;
  int *i = (int *)(d+8);
  i[0] = TempomatLever;
  i[1] = BlinkLever;
  i[2] = SteeringWheelLever;
  d = (double *)(i+3);
  d[0] = ArtLeadVehicleDistance;
  d[1] = ArtLeadVehicleVelocity;
  d[2] = ArtSollVelocity;
  i = (int *)(d+3);
  i[0] = ArtFlags;
  d = (double *)(i+1);
  d[0] = AcceleratorPedal;
  d[1] = EngineSpeed;
  i = (int *)(d+2);
  i[0] = Gear;
  i[1] = VehicleFlags;
}

void DC_inertial::ReadBinary(char *data)
{
  /*char *d=data;
  Yaw=(*((double *)d)++); 
  YawRateRaw=(*((double *)d)++);
  YawRateOffset=(*((double *)d)++);
  SteeringAngle=(*((double *)d)++);
  Pitch=(*((double *)d)++);
  Speed=(*((double *)d)++);
  LongAcceleration=(*((double *)d)++);
  LatAcceleration=(*((double *)d)++);
  TempomatLever=(*((int *)d)++);
  BlinkLever=(*((int *)d)++);
  SteeringWheelLever=(*((int *)d)++);
  ArtLeadVehicleDistance=(*((double *)d)++);
  ArtLeadVehicleVelocity=(*((double *)d)++);
  ArtSollVelocity=(*((double *)d)++);
  ArtFlags=(*((int *)d)++);
  AcceleratorPedal=(*((double *)d)++);
  EngineSpeed=(*((double *)d)++);
  Gear=(*((int *)d)++);
  VehicleFlags=(*((int *)d)++);*/
  double *d = (double *)data;
  Yaw= d[0]; 
  YawRateRaw= d[1];
  YawRateOffset= d[2];
  SteeringAngle= d[3];
  Pitch= d[4];
  Speed= d[5];
  LongAcceleration= d[6];
  LatAcceleration= d[7];
  int *i = (int *)(d+8);
  TempomatLever = i[0];
  BlinkLever = i[1];
  SteeringWheelLever = i[2];
  d = (double *)(i+3);
  ArtLeadVehicleDistance= d[0];
  ArtLeadVehicleVelocity= d[1];
  ArtSollVelocity= d[2];
  i = (int *)(d+3);
  ArtFlags = i[0];
  d = (double *)(i+1);
  AcceleratorPedal= d[0];
  EngineSpeed= d[1];
  i = (int *)(d+2);
  Gear = i[0];
  VehicleFlags = i[1];
}

//////////////////////////////////////////////////////////////////////////
//                    DC_GPS
/////////////////////////////////////////////////////////////////////////

DC_GPS::DC_GPS() : Debug()
{
  framenum=0;
  latPos=longPos=azimuth=offset=speed=gpsTime=curvature=clothoidParameter=0.0;
  mapIndex=scaleFactorSpeed=yawRateOffset=distanceToMap=0.0;
}

DC_GPS::DC_GPS(const DC_GPS& gps) : Debug()
{
  framenum          = gps.framenum           ;
  latPos            = gps.latPos             ;
  longPos               = gps.longPos                ;
  azimuth               = gps.azimuth                ; 
  offset                = gps.offset                 ; 
  speed               = gps.speed                  ; 
  gpsTime               = gps.gpsTime                ; 
  curvature           = gps.curvature              ; 
  clothoidParameter = gps.clothoidParameter  ; 
  mapIndex            = gps.mapIndex               ; 
  scaleFactorSpeed  = gps.scaleFactorSpeed   ; 
  yawRateOffset       = gps.yawRateOffset          ; 
  distanceToMap       = gps.distanceToMap          ;      
}

DC_GPS::~DC_GPS()
{
}

void DC_GPS::Set(int fnum, double lPos, double loPos, double az, 
                 double sp, double distMap)
{
  framenum=fnum;
  latPos=lPos;
  longPos=loPos;
  azimuth=az;
  speed=sp;
  distanceToMap=distMap;
}

istream& BIAS::operator>>(istream& is, DC_GPS& gps)
{
  string tmp;
  is >> tmp >> gps.framenum;
  for (int i=0; i<12; i++) is >>tmp;
  is >> gps.latPos          ;
  is >> gps.longPos          ;
  is >> gps.azimuth          ;
  is >> gps.offset             ;
  is >> gps.speed               ;
  is >> gps.gpsTime           ;
  is >> gps.curvature           ;
  is >> gps.clothoidParameter;
  is >> gps.mapIndex             ;
  is >> gps.scaleFactorSpeed ;
  is >> gps.yawRateOffset        ;
  is >> gps.distanceToMap        ;
  for (int i=0; i<9; i++) is >>tmp;
  if (!is) {
    BIASERR("error reading gps meta data from ascii stream");
    biassleep(1);
  }
  return is;
}

ostream& BIAS::operator<<(ostream& os, const DC_GPS& gps)
{
  streambuf *OutBuf_=cout.rdbuf(), *OsBuf_= os.rdbuf(), 
    *ErrBuf_=cerr.rdbuf();

  if (OutBuf_==OsBuf_ || ErrBuf_==OsBuf_){ // cout or  cerr
    os << "GPS: framenum: "<<gps.framenum
       <<"   latPos: "<<setprecision(10)<<gps.latPos
       <<"   longPos: "<<setprecision(10)<<gps.longPos
       <<"   azimuth: "<<gps.azimuth 
       <<"   speed: "<<setprecision(10)<<gps.speed
       <<"   distanceToMap: "<<setprecision(10)<<gps.distanceToMap;
  } else {
    os //<< "#[GPS Data]"<<endl
      << "#framenum= "<<gps.framenum<<endl
      << "#latPos longPos azimuth offset speed gpsTime curvature clothoidParameter mapIndex scaleFactorSpeed yawRateOffset distanceToMap   "
      << gps.framenum         << "\t"
      << gps.latPos           << "\t"
      << gps.longPos              << "\t" 
      << gps.azimuth              << "\t" 
      << gps.offset             << "\t"
      << gps.speed              << "\t" 
      << gps.gpsTime              << "\t" 
      << gps.curvature          << "\t" 
      << gps.clothoidParameter<< "\t"
      << gps.mapIndex             << "\t"
      << gps.scaleFactorSpeed << "\t"
      << gps.yawRateOffset      << "\t" 
      << gps.distanceToMap      <<endl
      << "#Traffic Point Map Size: 0"<<endl
      << "#LanePointNo posX posZ curvature";
  }
  return os;
}

void DC_GPS::WriteBinary(char *data)
{
#ifdef BIAS_DEBUG  
  if (!data) {
    BIASERR("data is NULL");
    BIASASSERT(false);
  }
#endif
  /*char *d=data;
  (*((int*)d)++)   =framenum         ;  
  (*((double*)d)++)=latPos           ;
  (*((double*)d)++)=longPos            ;
  (*((double*)d)++)=azimuth            ;
  (*((double*)d)++)=offset             ;
  (*((double*)d)++)=speed                ;
  (*((double*)d)++)=gpsTime            ;
  (*((double*)d)++)=curvature            ;
  (*((double*)d)++)=clothoidParameter;
  (*((double*)d)++)=mapIndex             ;
  (*((double*)d)++)=scaleFactorSpeed ;
  (*((double*)d)++)=yawRateOffset        ;
  (*((double*)d)++)=distanceToMap        ;*/
  int *i = (int *)data;
  i[0]   =framenum         ;  
  double *d = (double *)(i+1);
  d[0] = latPos           ;
  d[1] = longPos               ;
  d[2] = azimuth               ;
  d[3] = offset            ;
  d[4] = speed               ;
  d[5] = gpsTime               ;
  d[6] = curvature           ;
  d[7] = clothoidParameter;
  d[8] = mapIndex            ;
  d[9] = scaleFactorSpeed ;
  d[10] = yawRateOffset      ;
  d[11] = distanceToMap      ;
}

void DC_GPS::ReadBinary(char *data)
{
  int *i = (int *)data;
  framenum           = i[0];
  double *d = (double *)(i+1);
  latPos             = d[0];
  longPos                = d[1];
  azimuth                = d[2];
  offset                 = d[3];
  speed                = d[4];
  gpsTime                = d[5];
  curvature            = d[6];
  clothoidParameter  = d[7];
  mapIndex             = d[8];
  scaleFactorSpeed   = d[9];
  yawRateOffset        = d[10];
  distanceToMap        = d[11];
}