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"

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=7;
  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\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;
        }
        break;
      default:
        continue;
      }
    }
    saveCSVrow(file, row, rowLen);
    stop();
    return true;
  }
  stop();
  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.convertFromYUVToRGB(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
          {
            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();
}

/*
* Change Log:
*
* $Log: LogPlayer.cpp,v $
* Revision 1.1.1.1  2004/05/22 17:37:17  cvsadm
* created new repository GT2004_WM
*
* Revision 1.10  2004/05/19 07:58:14  dueffert
* saving to CSV implemented
*
* Revision 1.9  2004/04/20 13:18:08  roefer
* LogPlayer immediately switches to next message if it is older than 4 seconds (instead of one second).
*
* Revision 1.8  2004/03/26 16:33:57  thomas
* added field in logplayer to jump directly to a given frame-number
*
* Revision 1.7  2004/03/24 12:55:42  risler
* added logplayer repeat button
*
* Revision 1.6  2004/02/16 12:26:41  nistico
* Added noise reduction functionality for jpeg images in log file player
*
* Revision 1.5  2004/02/10 16:34:39  nistico
* Fixed bug with ERS-7 log files saved (converted) as a series of .bmp images
*
* Revision 1.4  2004/01/20 12:40:09  nistico
* - Added support for ColorTable32K (65K elements in packed format)
* - RobotControl can now convert GT *.log files into AIBOVision (external ColorTable32K calibration tool) *.amv file format
*
* Revision 1.3  2003/12/15 11:49:06  juengel
* Introduced CameraInfo
*
* Revision 1.2  2003/12/02 10:42:30  lohmann
* WorldstatePlayer Dialog changes
*
* Revision 1.1  2003/10/07 10:13:24  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.2  2003/09/26 15:28:10  juengel
* Renamed DataTypes to representations.
*
* Revision 1.1.1.1  2003/07/02 09:40:28  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.7  2003/05/03 21:57:47  roefer
* Skip large pauses
*
* Revision 1.6  2002/12/17 15:50:49  dueffert
* support for writing idJPEGImage to *.bmp added
*
* Revision 1.5  2002/12/15 23:34:13  dueffert
* saving images from logfiles added
*
* Revision 1.4  2002/12/07 16:40:45  roefer
* Blocking for theDebugReceiver changed
*
* Revision 1.3  2002/10/04 10:27:33  loetzsch
* Added functionality to adjust the speed of playing log files.
*
* Revision 1.2  2002/10/02 15:50:41  juengel
* Added getCurrentMessageID.
*
* Revision 1.1  2002/09/10 15:53:59  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.1  2002/08/08 16:40:30  loetzsch
* added class LogPlayer and redesigned RobotControl's Logplayer GUI
*
*/

---