Tools/MessageQueue/LogPlayer.cpp

Go to the documentation of this file.

/**
* @file LogPlayer.cpp
*
* Implementation of class LogPlayer
*
* @author Martin Lötzsch
*/

#include "LogPlayer.h"
#include "Representations/Perception/Image.h"
#include "Representations/Perception/JPEGImage.h"
#include "Representations/Perception/CameraMatrix.h"
#include "Representations/Perception/SpecialPercept.h"
#include "Representations/Cognition/RobotPose.h"
#include "Representations/Cognition/BallModel.h"
#include "Representations/Cognition/PlayerPoseCollection.h"
#include "Representations/Cognition/ObstaclesModel.h"
#include "Representations/Cognition/RobotState.h"
#include "Platform/SystemCall.h"
#include "Platform/GTAssert.h"

LogPlayer::LogPlayer(MessageQueue& targetQueue)
: targetQueue(targetQueue),
smoothingEnabled(false),
playSpeed(1)
{
  _new();
}

void LogPlayer::_new()
{
  clear();
  stop();
  state = initial;
}

bool LogPlayer::open(const char* fileName)
{
  InBinaryFile file(fileName);
  if (file.exists())
  {
    clear();
    file >> *this;
    stop();
    return true;
  }
  return false;
}

void LogPlayer::play()
{
  pause();
  state = playing;
}

void LogPlayer::stop()
{
  pause();
  currentMessageNumber = -1;
}

void LogPlayer::pause()
{
  if (getNumberOfMessages() == 0) state = initial;
  else state = paused;

  timeOfFirstPlayedMessage = 0;
  timeWhenFirstMessageWasPlayed = 0;
}

void LogPlayer::stepBackward()
{
  pause();

  if (state == paused && currentMessageNumber > 0 )
    copyMessage(--currentMessageNumber,targetQueue);
}

void LogPlayer::stepForward()
{
  pause();

  if ((state == paused) && (currentMessageNumber < getNumberOfMessages()-1))
    copyMessage(++currentMessageNumber,targetQueue);
}

void LogPlayer::stepRepeat()
{
  pause();

  if (state == paused && currentMessageNumber >= 0 )
    copyMessage(currentMessageNumber,targetQueue);
}

void LogPlayer::jumpFrame(int frame)
{
  pause();

  if (state == paused && frame >= 1 && frame <= getNumberOfMessages() )
    copyMessage(currentMessageNumber = frame - 1,targetQueue);
}

bool LogPlayer::save(const char* fileName)
{
  if (getNumberOfMessages() == 0) return false;

  OutBinaryFile file(fileName);
  if (file.exists())
  {
    file << *this;
    return true;
  }
  return false;
}


void LogPlayer::convertIntString(char* str, int value)
{
  char temp[256]; 
  int i=0;
  while(value > 0)
  { 
    temp[i] = (char)(value % 10) + 48;
    value = value / 10; i++;
  }
  for (int j=i-1; j>=0; j--)
    str[j] = temp[i-j-1];
  str[i] = '\0';
}

bool LogPlayer::saveAMV(const char* fileName)
{
  if (getNumberOfMessages() == 0) return false;
  OutBinaryFile file(fileName);
  if (file.exists())
  {
    unsigned char* imgBuffer;
    CameraMatrix cameraMatrix;
    Image image;
    bool headerGenerated = false;
    for (currentMessageNumber=0; currentMessageNumber<getNumberOfMessages();currentMessageNumber++)
    {
      queue.setSelectedMessageForReading(currentMessageNumber);
      if (getCurrentMessageID()==idImage)
      {
        in.bin >> image >> cameraMatrix;
      }
      else if (getCurrentMessageID()==idJPEGImage)
      {
        JPEGImage jpegImage;
        in.bin >> jpegImage >> cameraMatrix;
        jpegImage.toImage(image);
      }
      else
        continue;
      if (!headerGenerated){
        char* magicNumber = "AIBOMOVIE";
        unsigned char padding = 0;
        unsigned char magicNumberLenUTF = 9;
        file << padding << magicNumberLenUTF;
        for (int k=0; k<magicNumberLenUTF; k++)
          file << magicNumber[k];
        char widthStr[10];
        char heightStr[10];
        unsigned char widthLenUTF = 3;
        unsigned char heightLenUTF = 3;
        convertIntString(widthStr, image.cameraInfo.resolutionWidth);
        convertIntString(heightStr, image.cameraInfo.resolutionHeight);
        file << padding << widthLenUTF;
        for (k=0; k<widthLenUTF; k++)
          file << widthStr[k];
        file << padding << heightLenUTF;
        for (k=0; k<heightLenUTF; k++)
          file << heightStr[k];
        char movType = '0';
        unsigned char movTypeLenUTF = 1;
        file << padding << movTypeLenUTF << movType;
        imgBuffer = new unsigned char[3*image.cameraInfo.resolutionWidth*image.cameraInfo.resolutionHeight];        
        headerGenerated = true;
      }
    int lineWidth = image.cameraInfo.resolutionWidth;
      for (int h=0; h<image.cameraInfo.resolutionHeight; h++)
        for (int w=0; w<image.cameraInfo.resolutionWidth; w++)
        {
          imgBuffer[3*(h*lineWidth + w) + 0] = image.image[h][0][w];
          imgBuffer[3*(h*lineWidth + w) + 2] = image.image[h][1][w];
          imgBuffer[3*(h*lineWidth + w) + 1] = image.image[h][2][w];
        }
      file.write(imgBuffer,3*image.cameraInfo.resolutionWidth*image.cameraInfo.resolutionHeight);
    }
    delete [] imgBuffer;
    stop();
    return true;
  }
  stop();
  return false;
}

void LogPlayer::saveCSVrow(OutTextRawFile& file, double* row, unsigned int rowLen)
{
  if (fabs(row[0])<=1e10)
  {
    file << row[0];
    for (unsigned int i=1;i<rowLen;i++)
    {
      if (fabs(row[i])>1e10)
      {
        file << ", -";
      }
      else
      {
        file << ", " << row[i];
      }
      row[i]=1e11;
    }
    file << "\n";
  }
}

bool LogPlayer::saveCSV(const char* fileName)
{
  //add more data to row if you like:
  static const unsigned int rowLen=12;
  if (getNumberOfMessages() == 0) return false;
  OutTextRawFile file(fileName);
  if (file.exists())
  {
    double row[rowLen];
    for (int i=1;i<rowLen;i++)
    {
      row[i]=1e11;
    }
    file << "frame, specialPercept.checkerPose.x, specialPercept.checkerPose.y, specialPercept.checkerPose.r, robotPose.x, robotPose.y, robotPose.r, rP.timestamp, rC.x, rC.y, rC.r, rC.timestamp\n";
    for (currentMessageNumber=0; currentMessageNumber<getNumberOfMessages();currentMessageNumber++)
    {
      queue.setSelectedMessageForReading(currentMessageNumber);
      switch (getCurrentMessageID())
      {
      case idSpecialPercept:
        {
          Player pl;
          CameraMatrix cm;
          SpecialPercept specialPercept;
          in.bin >> pl >> specialPercept >> cm;
          if (specialPercept.type==SpecialPercept::checkerboard)
          {
            if (specialPercept.frameNumber!=row[0])
            {
              saveCSVrow(file, row, rowLen);
              row[0]=specialPercept.frameNumber;
            }
            row[1]=specialPercept.checkerPose.translation.x;
            row[2]=specialPercept.checkerPose.translation.y;
            row[3]=specialPercept.checkerPose.rotation;
          }
        }
        break;
      case idWorldState:
        {
          RobotPose robotPose;
          BallModel ballModel;
          PlayerPoseCollection playerPoseCollection;
          ObstaclesModel obstaclesModel;
          RobotState robotState;
          CameraMatrix cameraMatrix;
          CameraInfo cameraInfo;
          in.bin >> RECEIVE_WORLDSTATE(robotPose,ballModel,playerPoseCollection,
            obstaclesModel,robotState,cameraMatrix,cameraInfo);
          if (robotPose.frameNumber!=row[0])
          {
            saveCSVrow(file, row, rowLen);
            row[0]=robotPose.frameNumber;
          }
          row[4]=robotPose.translation.x;
          row[5]=robotPose.translation.y;
          row[6]=robotPose.rotation;
          row[7]=robotPose.timestamp;
        }
        break;
      case idRemoteCamWorldState:
        {
          RobotPose robotPose;
          BallModel ballModel;
          PlayerPoseCollection playerPoseCollection;
          ObstaclesModel obstaclesModel;
          RobotState robotState;
          CameraMatrix cameraMatrix;
          CameraInfo cameraInfo;
          in.bin >> RECEIVE_WORLDSTATE(robotPose,ballModel,playerPoseCollection,
            obstaclesModel,robotState,cameraMatrix,cameraInfo);
          row[8]=robotPose.translation.x;
          row[9]=robotPose.translation.y;
          row[10]=robotPose.rotation;
          row[11]=robotPose.timestamp;
        }
        break;
      default:
        continue;
      }
    }
    saveCSVrow(file, row, rowLen);
    stop();
    return true;
  }
  stop();
  return false;
}

bool LogPlayer::saveSynchronized(const char* fileName)
{
  MessageQueue nirvana;
  LogPlayer remoteCamQueue(nirvana), robotQueue(nirvana);

  unsigned long syncTimestampRemoteCam, timestampOfSyncTimestampRemoteCam, syncTimestampRobot;
  bool foundSyncTimestampRemoteCam=false;
  bool foundSyncTimestampRobot=false;

  unsigned long robotSyncMessageTimestamp, remoteCamSyncMessageTimestamp;

  for (currentMessageNumber=0; currentMessageNumber<getNumberOfMessages();currentMessageNumber++)
  {
    queue.setSelectedMessageForReading(currentMessageNumber);
    switch (getCurrentMessageID())
    {
    
    case idRemoteCamWorldState:
      copyMessage(currentMessageNumber,remoteCamQueue);
      break;

    case idSyncTimestampRemoteCam:
      remoteCamSyncMessageTimestamp=queue.getTimeStamp();
      in.bin>>syncTimestampRemoteCam>>timestampOfSyncTimestampRemoteCam;
      remoteCamSyncMessageTimestamp -= timestampOfSyncTimestampRemoteCam - syncTimestampRemoteCam;
      foundSyncTimestampRemoteCam=true;
      break;
      
    case idSyncTimestampRobot:
      robotSyncMessageTimestamp=queue.getTimeStamp();
      in.bin>>syncTimestampRobot;
      foundSyncTimestampRobot=true;
      break;

    default:
      copyMessage(currentMessageNumber,robotQueue);
      break;
    }
  }

  if (!(foundSyncTimestampRobot&&foundSyncTimestampRemoteCam)) return false;

  

  long messageOffset= robotSyncMessageTimestamp - remoteCamSyncMessageTimestamp;
  long offset= syncTimestampRobot - syncTimestampRemoteCam;

  int robotQueueCurrentMessageNumber=0;
  int remoteCamQueueCurrentMessageNumber=0;

  
  LogPlayer saveQueue(nirvana);

  while (robotQueue.queue.getNumberOfMessages()>robotQueueCurrentMessageNumber&&remoteCamQueue.queue.getNumberOfMessages()>remoteCamQueueCurrentMessageNumber)
  {
    remoteCamQueue.queue.setSelectedMessageForReading(remoteCamQueueCurrentMessageNumber);
    robotQueue.queue.setSelectedMessageForReading(robotQueueCurrentMessageNumber);

    if (robotQueue.queue.getTimeStamp()<remoteCamQueue.queue.getTimeStamp()+messageOffset) 
    { 
      robotQueue.copyMessage(robotQueueCurrentMessageNumber,saveQueue);
      robotQueueCurrentMessageNumber++;
    } else {
      //timestamp anpassen und kopieren
      
      RobotPose robotPose;
      BallModel ballModel;
      PlayerPoseCollection playerPoseCollection;
      ObstaclesModel obstaclesModel;
      RobotState robotState;
      CameraMatrix cameraMatrix;
      CameraInfo cameraInfo;
      
      remoteCamQueue.in.bin >> RECEIVE_WORLDSTATE(robotPose,ballModel,playerPoseCollection,obstaclesModel,robotState, cameraMatrix, cameraInfo);

      if (((double)robotPose.timestamp + (double)offset >= 0.0) && (robotPose.getValidity() != 0.0))
      {
        unsigned long newTimestamp = robotPose.timestamp + offset;
        
        robotPose.setTimestamp(newTimestamp);
        robotPose.setFrameNumber(newTimestamp);
        ballModel.seen.timeWhenLastSeen = newTimestamp;
        ballModel.setFrameNumber(newTimestamp);
        
        PlayerPose playerPose;
        int i;
        for(i = 0; i < playerPoseCollection.numberOfOwnPlayers; ++i)
        {
          playerPose = playerPoseCollection.getOwnPlayerPose(i);
          playerPose.setFrameNumber(newTimestamp);
          playerPoseCollection.setOwnPlayerPose(i, playerPose);
        }
        for(i = 0; i < playerPoseCollection.numberOfOpponentPlayers; ++i)
        {
          playerPose = playerPoseCollection.getOpponentPlayerPose(i);
          playerPose.setFrameNumber(newTimestamp);
          playerPoseCollection.setOpponentPlayerPose(i, playerPose);
        }
        
        saveQueue.out.bin << SEND_WORLD_STATE(robotPose,ballModel,playerPoseCollection,obstaclesModel,robotState, cameraMatrix, cameraInfo);
        saveQueue.out.finishMessage(
          remoteCamQueue.queue.getMessageID(),
          remoteCamQueue.queue.getTimeStamp()+messageOffset,
          remoteCamQueue.queue.getTeamColor(),
          remoteCamQueue.queue.getPlayerNumber(),
          remoteCamQueue.queue.getMessageWasSentFromAPhysicalRobot());    
          /*      
          saveQueue.out.bin.write(remoteCamQueue.queue.getData(),remoteCamQueue.queue.getMessageSize());
          saveQueue.out.finishMessage(
          remoteCamQueue.queue.getMessageID(),
          remoteCamQueue.queue.getTimeStamp()+messageOffset,
          remoteCamQueue.queue.getTeamColor(),
          remoteCamQueue.queue.getPlayerNumber(),
          remoteCamQueue.queue.getMessageWasSentFromAPhysicalRobot());
        */
      }
      remoteCamQueueCurrentMessageNumber++;
    }
  }

  while (robotQueue.queue.getNumberOfMessages()>robotQueueCurrentMessageNumber)
  { 
    robotQueue.copyMessage(robotQueueCurrentMessageNumber,saveQueue);
    robotQueueCurrentMessageNumber++;
  }

  while (remoteCamQueue.queue.getNumberOfMessages()>remoteCamQueueCurrentMessageNumber)
  {
    //timestamp anpassen
    saveQueue.out.bin.write(remoteCamQueue.queue.getData(),remoteCamQueue.queue.getMessageSize());
    saveQueue.out.finishMessage(
      remoteCamQueue.queue.getMessageID(),
      remoteCamQueue.queue.getTimeStamp()+messageOffset,
      remoteCamQueue.queue.getTeamColor(),
      remoteCamQueue.queue.getPlayerNumber(),
      remoteCamQueue.queue.getMessageWasSentFromAPhysicalRobot());

    remoteCamQueueCurrentMessageNumber++;
  }
  
  return saveQueue.save(fileName);

  return false;
}

bool LogPlayer::saveImages(const char* fileName)
{
  struct BmpHeader{
    unsigned long k0,k1,k2;
    unsigned long ofs1,ofs2;
    unsigned long xsiz,ysiz;
    unsigned long modbit;
    unsigned long z1;
    unsigned long len;
    unsigned long z2,z3,z4,z5;
  } bmpHeader;
  
  char name[512];
  char fname[512];
  strcpy(name,fileName);
  if ((strlen(name)>4)&&((strncmp(name+strlen(name)-4,".bmp",4)==0)||(strncmp(name+strlen(name)-4,".jpg",4)==0)))
  {
    *(name+strlen(name)-4) = 0;
  }
  if ((strlen(name)>4)&&((strncmp(name+strlen(name)-4,"_000",4)==0)||(strncmp(name+strlen(name)-4,"_001",4)==0)))
  {
    *(name+strlen(name)-4) = 0;
  }
  int i=0;
  for (currentMessageNumber=0; currentMessageNumber<getNumberOfMessages();currentMessageNumber++)
  {
    CameraMatrix cameraMatrix;
    queue.setSelectedMessageForReading(currentMessageNumber);
    Image image;
    if (getCurrentMessageID()==idImage)
    {
      in.bin >> image >> cameraMatrix;
    }
    else if (getCurrentMessageID()==idJPEGImage)
    {
      JPEGImage jpegImage;
      in.bin >> jpegImage >> cameraMatrix;
      jpegImage.toImage(image);
    }
    else
      continue;

    Image rgbImage;
    rgbImage.convertFromYCbCrToRGB(image);

    sprintf(fname,"%s_%03i.bmp",name,i++);
    OutBinaryFile file(fname);
    if (file.exists())
    {
      long truelinelength=(3*rgbImage.cameraInfo.resolutionWidth+3) & 0xfffffc;
      char line[3*208+4];
      memset(line,0,truelinelength);
      bmpHeader.k0=0x4d420000;    //2 dummy bytes 4 alignment, then "BM"
      bmpHeader.k1=rgbImage.cameraInfo.resolutionHeight*truelinelength+0x36;
      bmpHeader.k2=0;
      bmpHeader.ofs1=0x36;
      bmpHeader.ofs2=0x28;
      bmpHeader.xsiz=rgbImage.cameraInfo.resolutionWidth;
      bmpHeader.ysiz=rgbImage.cameraInfo.resolutionHeight;
      bmpHeader.modbit=0x00180001;
      bmpHeader.z1=0;
      bmpHeader.len=truelinelength*rgbImage.cameraInfo.resolutionHeight;
      bmpHeader.z2=0;
      bmpHeader.z3=0;
      bmpHeader.z4=0;
      bmpHeader.z5=0;
      file.write(2+(char*)&bmpHeader,sizeof(bmpHeader)-2);
      for (int i=rgbImage.cameraInfo.resolutionHeight-1; i>=0; i--)
      {
        int ofs=0;
        for (int j=0;j<rgbImage.cameraInfo.resolutionWidth;j++)
        {
          line[ofs++]=rgbImage.image[i][2][j];
          line[ofs++]=rgbImage.image[i][1][j];
          line[ofs++]=rgbImage.image[i][0][j];
        }
        file.write(line,truelinelength);
      }
    }
    else
    {
      stop();
      return false;
    }
  }
  stop();
  return true;
}

void LogPlayer::record()
{
  switch(state)
  {
  case recording:
    pause();
    break;
  case paused:
  case initial:
  case playing:
    state = recording;
    break;
  }
}

void LogPlayer::smooth()
{
  if(smoothingEnabled)
    smoothingEnabled = false;
  else 
    smoothingEnabled = true;
}

void LogPlayer::setPlaySpeed(double speed)
{
  playSpeed = speed;
  timeWhenFirstMessageWasPlayed = 0;
}

void LogPlayer::handleMessage(InMessage& message)
{
  if (state == recording)
    message >> *this;
}

LogPlayer::LogPlayerState LogPlayer::getState()
{
  return state;
}

void LogPlayer::onIdle()
{
  if (state == playing)
  {
    while (1)
    {
      if (currentMessageNumber < getNumberOfMessages()-1)
      {
        if (timeWhenFirstMessageWasPlayed == 0 ||
            (double)(int(getTimeStamp(currentMessageNumber+1)) - int(timeOfFirstPlayedMessage))
            > (double)(SystemCall::getTimeSince(timeWhenFirstMessageWasPlayed)) * playSpeed + 4000)
        {
          copyMessage(++currentMessageNumber,targetQueue);
          timeOfFirstPlayedMessage = getTimeStamp(currentMessageNumber);
          timeWhenFirstMessageWasPlayed = SystemCall::getCurrentSystemTime();
        }
        else
        {
          if ((double)(int(getTimeStamp(currentMessageNumber+1)) - int(timeOfFirstPlayedMessage))
            < (double)(SystemCall::getTimeSince(timeWhenFirstMessageWasPlayed)) * playSpeed)
          {
            copyMessage(++currentMessageNumber,targetQueue);
          }
          else
          {
            targetQueue.removeRepetitions();
            break;
          }
        }
      }
      else 
      {  
        stop();
        break;
      }
    }
  }
}

unsigned long LogPlayer::getTimeStamp(int message)
{
  return queue.getTimeStamp(message);
}

int LogPlayer::getNumberOfMessages() const
{
  return queue.getNumberOfMessages();
}

int LogPlayer::getCurrentMessageNumber() const
{
  return currentMessageNumber;
}

MessageID LogPlayer::getCurrentMessageID() const
{
  if(currentMessageNumber < 0) return undefined;
  return queue.getMessageID();
}

void LogPlayer::keep(MessageID* messageIDs)
{
  LogPlayer temp((MessageQueue&) *this);
  moveAllMessages(temp);
  for(temp.currentMessageNumber = 0; temp.currentMessageNumber < temp.getNumberOfMessages(); ++temp.currentMessageNumber)
  {
    temp.queue.setSelectedMessageForReading(temp.currentMessageNumber);
    MessageID* m = messageIDs;
    while(*m)
    {
      if(*m == temp.getCurrentMessageID())
      {
        temp.stepRepeat();
        break;
      }
      ++m;
    }
  }
}

void LogPlayer::remove(MessageID* messageIDs)
{
  LogPlayer temp((MessageQueue&) *this);
  moveAllMessages(temp);
  for(temp.currentMessageNumber = 0; temp.currentMessageNumber < temp.getNumberOfMessages(); ++temp.currentMessageNumber)
  {
    temp.queue.setSelectedMessageForReading(temp.currentMessageNumber);
    MessageID* m = messageIDs;
    while(*m)
    {
      if(*m == temp.getCurrentMessageID())
        break;
      ++m;
    }
    if(!*m)
      temp.stepRepeat();
  }
}

void LogPlayer::statistics(int frequency[numOfMessageIDs])
{
  int i;
  for(i = 0; i < numOfMessageIDs; ++i)
    frequency[i] = 0;
  for(i = 0; i < getNumberOfMessages(); ++i)
  {
    queue.setSelectedMessageForReading(i);
    ASSERT(queue.getMessageID() < numOfMessageIDs);
    ++frequency[queue.getMessageID()];
  }
}

---