Tools/LogfileAnalyzing/AnalyzerBase.cpp

Go to the documentation of this file.

//------------------------------------------------------------------------------
#include "AnalyzerBase.h"
#include "Representations/Cognition/BallModel.h"
#include "Representations/Cognition/PlayerPoseCollection.h"
#include "Representations/Cognition/ObstaclesModel.h"
#include "Representations/Cognition/RobotState.h"
#include "Representations/Perception/BallPercept.h"
#include "Representations/Perception/LandmarksPercept.h"
#include "Representations/Perception/LinesPercept.h"
#include "Representations/Perception/PlayersPercept.h"
#include "Representations/Perception/EdgesPercept.h"
#include "Representations/Perception/ObstaclesPercept.h"
#include "Representations/Perception/PSDPercept.h"
#include "Representations/Perception/CollisionPercept.h"
//------------------------------------------------------------------------------
CLogAnalyzerBase::CLogAnalyzerBase()
: logfile(nullQueue)
{
  reset();
}
//------------------------------------------------------------------------------
CLogAnalyzerBase::~CLogAnalyzerBase()
{
}
//------------------------------------------------------------------------------
void CLogAnalyzerBase::openLogfile(const std::string& logfilename)
{
  reset();

  checkExtension(logfilename);
  
  if (!logfile.open(logfilename.c_str()))
    throw CLogAnalyzerException(CLogAnalyzerException::CouldNotOpenFile);
  
  readLogfile();

  synchronize();
}
//------------------------------------------------------------------------------
void CLogAnalyzerBase::checkExtension(const std::string& filename)
{
  char szExt[_MAX_EXT];
  _splitpath(filename.c_str(), NULL, NULL, NULL, szExt);
  std::string sExt = szExt;
  for (int i = 0; i < (int)sExt.length(); ++i)
    sExt[i] = tolower(sExt[i]);
  if (sExt == ".slg")
  {
    foundSyncTimestampRemoteCam = true;
    foundSyncTimestampRobot = true;
  }
}
//------------------------------------------------------------------------------
void CLogAnalyzerBase::reset()
{
  nullQueue.clear();
  logfile.clear();

  foundSyncTimestampRemoteCam = false;
  foundSyncTimestampRobot = false;
  syncTimestampRemoteCam = 0;
  syncTimestampRobot = 0;

  framesSinceLastError = 0;
  errors = 0;
  drops = 0;

  actualCamRobotPoses.reset();
  actualRobRobotPoses.reset();
  actualCamBallPositions.reset();
  actualCamBallSpeeds.reset();

  camRobotPoses.clear();
  robRobotPoses.clear();
  camBallPositions.clear();
  camBallSpeeds.clear();
}
//------------------------------------------------------------------------------
void CLogAnalyzerBase::readLogfile()
{
  for (logfile.currentMessageNumber = 0;
       logfile.currentMessageNumber < logfile.getNumberOfMessages();
       ++logfile.currentMessageNumber)
  {
    logfile.queue.setSelectedMessageForReading(logfile.currentMessageNumber);
    switch (logfile.getCurrentMessageID())
    {
    case idSyncTimestampRemoteCam:
      logfile.in.bin >> syncTimestampRemoteCam;
      foundSyncTimestampRemoteCam = true;
      break;

    case idSyncTimestampRobot:
      logfile.in.bin >> syncTimestampRobot;
      foundSyncTimestampRobot = true;
      break;

    case idRemoteCamWorldState:
      readRemoteCamWorldState();
      break;

    case idWorldState:
      readWorldState();
      break;

    case idPercepts:
      readPercepts();
      break;     
    }
  }

  // Aktuelle Messreihen sichern
  saveActualDataRows();

}
//------------------------------------------------------------------------------
void CLogAnalyzerBase::readRemoteCamWorldState()
{
  RobotPose robotPose;
  BallModel ballModel;
  PlayerPoseCollection playerPoseCollection;
  ObstaclesModel obstaclesModel;
  RobotState robotState;
  CameraMatrix cameraMatrix;
  CameraInfo cameraInfo;

  logfile.in.bin >> RECEIVE_WORLDSTATE(
    robotPose, ballModel, playerPoseCollection, obstaclesModel, robotState,
    cameraMatrix, cameraInfo);

  if (robotPose.getValidity() == 0.0)
    return;

  // Sprung größer als 0.5m/s?
  if ((lastCamRobotPose.getValidity() != 0.0) &&
      ((robotPose.translation - lastCamRobotPose.translation).abs() /
       (robotPose.timestamp - lastCamRobotPose.timestamp) > 0.5))
  {
    // Aktuelle Messreihen sichern
    if (!actualCamRobotPoses.getCollection().empty())
    {
      camRobotPoses.push_back(actualCamRobotPoses);
      actualCamRobotPoses.reset();
    }
    if (!actualCamBallPositions.getCollection().empty())
    {
      camBallPositions.push_back(actualCamBallPositions);
      actualCamBallPositions.reset();
    }
    if (!actualCamBallSpeeds.getCollection().empty())
    {
      camBallSpeeds.push_back(actualCamBallSpeeds);
      actualCamBallSpeeds.reset();
    }

    framesSinceLastError = 0;
    errors++;
    lastCamRobotPose = robotPose;
    return;
  }

  framesSinceLastError++;
  if ((framesSinceLastError < 4) || 
      (abs(robotPose.translation.x) > 2150))
  {
    drops++;
    lastCamRobotPose = robotPose;
    return;
  }

  robotPose.rotation = normalize(robotPose.rotation);
  actualCamRobotPoses.add(robotPose.timestamp, robotPose);

  if (ballModel.ballWasSeen)
  {
    unsigned long time = ballModel.seen.timeWhenLastSeen;
    BallModel bm;
    bm.seen.setBallDataRelativeToField(
      robotPose,
      ballModel.seen.positionField.x,
      ballModel.seen.positionField.y,
      ballModel.seen.speedField.x,
      ballModel.seen.speedField.y);
    actualCamBallPositions.add(time, bm.seen.positionRobot);
    actualCamBallPositions.add(time, bm.seen.speedRobot);
  }
  else
  {
    // Aktuelle Messreihen sichern
    if (!actualCamBallPositions.getCollection().empty())
    {
      camBallPositions.push_back(actualCamBallPositions);
      actualCamBallPositions.reset();
    }
    if (!actualCamBallSpeeds.getCollection().empty())
    {
      camBallSpeeds.push_back(actualCamBallSpeeds);
      actualCamBallSpeeds.reset();
    }
  }
  
  lastCamRobotPose = robotPose;
}
//------------------------------------------------------------------------------
void CLogAnalyzerBase::readWorldState()
{
  RobotPose robotPose;
  BallModel ballModel;
  PlayerPoseCollection playerPoseCollection;
  ObstaclesModel obstaclesModel;
  RobotState robotState;
  CameraMatrix cameraMatrix;
  CameraInfo cameraInfo;

  logfile.in.bin >> RECEIVE_WORLDSTATE(
    robotPose, ballModel, playerPoseCollection, obstaclesModel, robotState,
    cameraMatrix, cameraInfo);

  actualRobRobotPoses.add(robotPose.timestamp, robotPose);
}
//------------------------------------------------------------------------------
void CLogAnalyzerBase::readPercepts()
{
  CameraMatrix cameraMatrix;
  CameraInfo cameraInfo;
  BallPercept ballPercept;
  LandmarksPercept landmarksPercept;
  LinesPercept linesPercept;
  PlayersPercept playersPercept;
  EdgesPercept edgesPercept;
  ObstaclesPercept obstaclesPercept;
  PSDPercept psdPercept;
  CollisionPercept collisionPercept;

  logfile.in.bin >> RECEIVE_PERCEPTS(cameraMatrix, cameraInfo, ballPercept,
    landmarksPercept, linesPercept, edgesPercept,
    playersPercept, obstaclesPercept, psdPercept, collisionPercept);
}
//------------------------------------------------------------------------------
void CLogAnalyzerBase::saveActualDataRows()
{
  if (!actualCamRobotPoses.getCollection().empty())
  {
    camRobotPoses.push_back(actualCamRobotPoses);
    actualCamRobotPoses.reset();
  }
  if (!actualCamBallPositions.getCollection().empty())
  {
    camBallPositions.push_back(actualCamBallPositions);
    actualCamBallPositions.reset();
  }
  if (!actualCamBallSpeeds.getCollection().empty())
  {
    camBallSpeeds.push_back(actualCamBallSpeeds);
    actualCamBallSpeeds.reset();
  }
  if (!actualRobRobotPoses.getCollection().empty())
  {
    robRobotPoses.push_back(actualRobRobotPoses);
    actualRobRobotPoses.reset();
  }
}
//------------------------------------------------------------------------------
void CLogAnalyzerBase::synchronize()
{
  if (!foundSyncTimestampRobot || !foundSyncTimestampRemoteCam)
    throw CLogAnalyzerException(CLogAnalyzerException::LogfileNotSynchronized);

  //unsigned long smallestTime = findSmallestTimestamp();
  sortCollections();
  long offset = syncTimestampRobot - syncTimestampRemoteCam;
  offsetCamCollections(offset);
}
//------------------------------------------------------------------------------
unsigned long CLogAnalyzerBase::findSmallestTimestamp()
{
  unsigned long smallest = (unsigned long)-1;
  
  smallest = min(smallest, findSmallestTimestamp(camRobotPoses));
  smallest = min(smallest, findSmallestTimestamp(robRobotPoses));
  smallest = min(smallest, findSmallestTimestamp(camBallPositions));
  smallest = min(smallest, findSmallestTimestamp(camBallSpeeds));

  return smallest;  
}
//------------------------------------------------------------------------------
void CLogAnalyzerBase::sortCollections()
{
  sortCollection(camRobotPoses);
  sortCollection(robRobotPoses);
  sortCollection(camBallPositions);
  sortCollection(camBallSpeeds);
}
//------------------------------------------------------------------------------
void CLogAnalyzerBase::offsetCamCollections(long offset)
{
  offsetCollection(camRobotPoses, offset);
  offsetCollection(camBallPositions, offset);
  offsetCollection(camBallSpeeds, offset);
}
//------------------------------------------------------------------------------
RobotPose CLogAnalyzerBase::interpolate(
  const CTimeStampedObject<RobotPose>& tso1,
  const CTimeStampedObject<RobotPose>& tso2,
  unsigned long timestamp)
{
  const RobotPose& p1 = tso1.getObject();
  const RobotPose& p2 = tso2.getObject();

  double diff = tso2.getTime() - tso1.getTime();
  double diff1 = timestamp - tso1.getTime();
  double diff2 = tso2.getTime() - timestamp;

  RobotPose interpolated;
  interpolated.translation.x =
    (diff2/diff)*p1.translation.x + (diff1/diff)*p2.translation.x;
  interpolated.translation.y =
    (diff2/diff)*p1.translation.y + (diff1/diff)*p2.translation.y;
  interpolated.rotation =
    (diff1/diff)*p2.rotation + (diff2/diff)*p1.rotation;
  if (fabs(p1.rotation - p2.rotation) > pi)
  {
    if (interpolated.rotation > 0)
      interpolated.rotation -= pi;
    else
      interpolated.rotation += pi;
  }
  interpolated.timestamp = timestamp;
  return interpolated;
}
//------------------------------------------------------------------------------
Vector2<double> CLogAnalyzerBase::interpolate(
  const CTimeStampedObject<Vector2<double> >& tso1,
  const CTimeStampedObject<Vector2<double> >& tso2,
  unsigned long timestamp)
{
  const Vector2<double>& v1 = tso1.getObject();
  const Vector2<double>& v2 = tso2.getObject();

  double diff = tso2.getTime() - tso1.getTime();
  double diff1 = timestamp - tso1.getTime();
  double diff2 = tso2.getTime() - timestamp;

  Vector2<double> interpolated;
  interpolated.x = (diff2/diff)*v1.x + (diff1/diff)*v2.x;
  interpolated.y = (diff2/diff)*v1.y + (diff1/diff)*v2.y;

  return interpolated;
}
//------------------------------------------------------------------------------

---