Platform/Aperios1.3.2/GTCamTimeSyncHandler.cpp

Go to the documentation of this file.

/** 
* @file  Platform/Aperios1.3.2/GTCamTimeSyncHandler.cpp
* @author Matthias Meyer
* 
*/

#include "GTCamTimeSyncHandler.h"

/** 
* Constructor
*/
GTCamTimeSyncHandler::GTCamTimeSyncHandler() :
UDPEndpoint(2048,2048)
{
  robotTimeRequestWasSent = -1;
  robotTimeRequestWasReceived = -1;
  serverTimeInResponse = -1;
  received2Packages = false;
  timeServer = "127.0.0.1";
  //cout << "GTCAM: Generating TimeSyncHandler\n";
}

/**
* Init the handler
*/
void GTCamTimeSyncHandler::initGTCamTimeSyncHandler(GTCamWorldState* myState, Image* myImage)
{
  int camport = GTCAMPORT;//std port
  int syncport = GTCAMSYNCPORT;
  double version;

  //read port
  InConfigFile file("gtcam.cfg");
  // format: 
  // Version x.x
  // CAMPORT x
  // SYNCPORT x 
  // spaces and nl are ignored
  if(file.exists())
  {
    char buf[250];
    while(!file.eof())
    {
      file >> buf;
      if ( strcmp(buf,"Version")==0 )
      {
        file >> version;
      }else
      if ( strcmp(buf,"CAMPORT")==0 )
      {
        file >> camport;
      }
      if ( strcmp(buf,"SYNCPORT")==0 )
      {
        file >> syncport;
      }
    }
  }

  bind(syncport); // GTCamTimeSync data
  state = myState;    // The worldstate
  image = myImage;
  //cout << "GTCAM: Binding to Port " << GTCAMSYNCPORT << "\n";
}


/**
* Receive data
*/
void GTCamTimeSyncHandler::onReceive(void* data, int size)
{
  // Do we have 2 packages ?
  if (serverTimeInResponse != -1) {
    received2Packages = true;
  }

  // copy input to serverTimeResponse
  memcpy(&serverTimeInResponse,data,size);
  robotTimeRequestWasReceived = SystemCall::getCurrentSystemTime();
  frameNumberTimeRequestWasReceived = image->frameNumber;
  calculateOffset();
  //cout << "GTCAM: New TimeOffset" << offset << "\n";
}

/**
* calculate the offset
*/
void GTCamTimeSyncHandler::calculateOffset() {
  offset = serverTimeInResponse - (robotTimeRequestWasSent / 2) - (robotTimeRequestWasReceived / 2);
  buffer.add((double)offset);
  // cout << "GTCAM: New TimeOffset2 : " << offset << "\n";
  // cout << buffer.getSum() << "\n";
  offset = (long)(buffer.getSum() / buffer.getNumberOfEntries());


  
  frameOffset = serverTimeInResponse / 8 - 
                (frameNumberTimeRequestWasSent / 2) - (frameNumberTimeRequestWasReceived / 2);
  
 /* cout << "GTCAM: " << frameOffset << " = " << serverTimeInResponse / 8 << " - " <<
    frameNumberTimeRequestWasSent / 2 << " - " << frameNumberTimeRequestWasReceived / 2 << " ----- ";*/
  frameNumberBuffer.add(frameOffset);

  frameOffset = frameNumberBuffer.getSum() / frameNumberBuffer.getNumberOfEntries();
  //cout << frameOffset << " \n";

  
  
}
        
/** 
* Start the syncing
*/
void GTCamTimeSyncHandler::startTimeSync() {
  double id = (double)getPlayer().getPlayerNumber();
  send(&id,sizeof(id),timeServer,GTCAMSYNCPORT);
  robotTimeRequestWasSent = SystemCall::getCurrentSystemTime();
  frameNumberTimeRequestWasSent = image->frameNumber;
}

---