Tools/LogfileAnalyzing/AnalyzerBase.h

Go to the documentation of this file.

//------------------------------------------------------------------------------
#ifndef ANALYZER_BASE_H_INCLUDED
#define ANALYZER_BASE_H_INCLUDED
//------------------------------------------------------------------------------
#include <string>
#include <vector>
#include <algorithm>
#include "Representations/Cognition/RobotPose.h"
#include "Tools/MessageQueue/LogPlayer.h"
//------------------------------------------------------------------------------
class CLogAnalyzerException
{
public:
  enum Reason
  {
    CouldNotOpenFile,
    LogfileNotSynchronized,
    ValueNotAvailable
  };

public:
  CLogAnalyzerException(Reason reason) : reason(reason) {}

public:
  Reason getReason() { return reason; }

private:
  Reason reason;
};
//------------------------------------------------------------------------------
template <class T>
class CTimeStampedObject
{
public:
  CTimeStampedObject(unsigned long time, const T& object)
    : time(time), object(object)
  {
  }

public:
  unsigned long getTime() const { return time; }
  void setTime(unsigned long time) { this->time = time; }
  const T& getObject() const { return object; }

private:
  unsigned long time;
  T object;

};
//------------------------------------------------------------------------------
template <class T>
bool operator<(const CTimeStampedObject<T>& obj1,
               const CTimeStampedObject<T>& obj2)
{
  return (obj1.getTime() < obj2.getTime());
}
//------------------------------------------------------------------------------
class CLogAnalyzerBase;
//------------------------------------------------------------------------------
template <class T>
class CTimeStampedObjectCollection
{
public:
  std::vector<CTimeStampedObject<T> >& getCollection()
  {
    return collection;
  }

  void add(unsigned long time, const T& object)
  {
    collection.push_back(CTimeStampedObject<T>(time, object));
  }

  unsigned long startTime()
  {
    return collection[0].getTime();
  }

  unsigned long endTime()
  {
    return collection[collection.size() - 1].getTime();
  }

  void sort() { std::sort(collection.begin(), collection.end()); }

  void reset() { collection.clear(); }

  void offestTime(long offset)
  {
    std::vector<CTimeStampedObject<T> >::iterator pos;
    for (pos = collection.begin(); pos != collection.end(); ++pos)
      pos->setTime(pos->getTime() + offset);
  }

private:
  std::vector<CTimeStampedObject<T> > collection;
};
//------------------------------------------------------------------------------
class CLogAnalyzerBase
{
public:
  CLogAnalyzerBase();
  ~CLogAnalyzerBase();

public:
  void openLogfile(const std::string& logfilename);

private:
  void checkExtension(const std::string& filename);
  void reset();
  void readLogfile();
  void readRemoteCamWorldState();
  void readWorldState();
  void readPercepts();
  void saveActualDataRows();
  void synchronize();

  unsigned long findSmallestTimestamp();
  template <class T>
  unsigned long findSmallestTimestamp(
    std::vector<CTimeStampedObjectCollection<T> >& col)
  {
    unsigned long smallest = (unsigned long)-1;
    std::vector<CTimeStampedObjectCollection<T> >::iterator pos;
    for (pos = col.begin(); pos != col.end(); ++pos)
    {
      pos->sort();
      if (pos->startTime() < smallest)
        smallest = pos->startTime();
    }
    return smallest;
  }

  void sortCollections();
  template <class T>
  void sortCollection(
    std::vector<CTimeStampedObjectCollection<T> >& col)
  {
    std::vector<CTimeStampedObjectCollection<T> >::iterator pos;
    for (pos = col.begin(); pos != col.end(); ++pos)
      pos->sort();
  }

  void offsetCamCollections(long offset);
  template <class T>
  void offsetCollection(
    std::vector<CTimeStampedObjectCollection<T> >& col,
    long offset)
  {
    std::vector<CTimeStampedObjectCollection<T> >::iterator pos;
    for (pos = col.begin(); pos != col.end(); ++pos)
      pos->offestTime(offset);
  }

  template <class T>
  bool available(
    const std::vector<CTimeStampedObjectCollection<T> >& col,
    unsigned long timestamp) const
  {
    std::vector<CTimeStampedObjectCollection<T> >::const_iterator pos;
    for (pos = col.begin(); pos != col.end(); ++pos)
    {
      if ((timestamp >= pos->startTime()) &&
          (timestamp <= pos->endTime()))
        return true;
    }
    return false;
  }

  template <class T>
  T getInterpolatedValue(
    const std::vector<CTimeStampedObjectCollection<T> >& col,
    unsigned long timestamp) const
  {
    std::vector<CTimeStampedObjectCollection<T> >::const_iterator pos;
    for (pos = col.begin(); pos != col.end(); ++pos)
    {
      if ((timestamp < pos->startTime()) ||
          (timestamp > pos->endTime()))
        continue;
      
      std::vector<CTimeStampedObject<T> >::const_iterator lower = lower_bound(
        pos->getCollection().begin(), pos->getCollection().end(), timestamp);
      if (timestamp == pos->endTime())
        return lower->getObject();

      const CTimeStampedObject<T>& tso1 = *lower;
      const CTimeStampedObject<T>& tso2 = *(lower + 1);
      return interpolate(tso1, tso2, timestamp);
    }
    throw CLogAnalyzerException(CLogAnalyzerException::ValueNotAvailable);
  }   

  static RobotPose interpolate(
    const CTimeStampedObject<RobotPose>& tso1,
    const CTimeStampedObject<RobotPose>& tso2,
    unsigned long timestamp);
  static Vector2<double> interpolate(
    const CTimeStampedObject<Vector2<double> >& tso1,
    const CTimeStampedObject<Vector2<double> >& tso2,
    unsigned long timestamp);

private:
  MessageQueue nullQueue;
  LogPlayer logfile;

  bool foundSyncTimestampRemoteCam;
  bool foundSyncTimestampRobot;
  unsigned long syncTimestampRemoteCam;
  unsigned long syncTimestampRobot;

  int framesSinceLastError;
  int errors;
  int drops;

  RobotPose lastCamRobotPose;
  CTimeStampedObjectCollection<RobotPose> actualCamRobotPoses;
  CTimeStampedObjectCollection<Vector2<double > > actualCamBallPositions;
  CTimeStampedObjectCollection<Vector2<double > > actualCamBallSpeeds;

  CTimeStampedObjectCollection<RobotPose> actualRobRobotPoses;
  
protected:
  std::vector<CTimeStampedObjectCollection<RobotPose> > camRobotPoses;
  std::vector<CTimeStampedObjectCollection<RobotPose> > robRobotPoses;
  std::vector<CTimeStampedObjectCollection<Vector2<double> > > camBallPositions;
  std::vector<CTimeStampedObjectCollection<Vector2<double> > > camBallSpeeds;
};
//------------------------------------------------------------------------------
#endif
//------------------------------------------------------------------------------

---