Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/LogfileAnalyzing/AnalyzerBase.h

Go to the documentation of this file.

    00001 //------------------------------------------------------------------------------
    00002 #ifndef ANALYZER_BASE_H_INCLUDED
    00003 #define ANALYZER_BASE_H_INCLUDED
    00004 //------------------------------------------------------------------------------
    00005 #include <string>
    00006 #include <vector>
    00007 #include <algorithm>
    00008 #include "Representations/Cognition/RobotPose.h"
    00009 #include "Tools/MessageQueue/LogPlayer.h"
    00010 //------------------------------------------------------------------------------
    00011 class CLogAnalyzerException
    00012 {
    00013 public:
    00014   enum Reason
    00015   {
    00016     CouldNotOpenFile,
    00017     LogfileNotSynchronized,
    00018     ValueNotAvailable
    00019   };
    00020
    00021 public:
    00022   CLogAnalyzerException(Reason reason) : reason(reason) {}
    00023
    00024 public:
    00025   Reason getReason() { return reason; }
    00026
    00027 private:
    00028   Reason reason;
    00029 };
    00030 //------------------------------------------------------------------------------
    00031 template <class T>
    00032 class CTimeStampedObject
    00033 {
    00034 public:
    00035   CTimeStampedObject(unsigned long time, const T& object)
    00036     : time(time), object(object)
    00037   {
    00038   }
    00039
    00040 public:
    00041   unsigned long getTime() const { return time; }
    00042   void setTime(unsigned long time) { this->time = time; }
    00043   const T& getObject() const { return object; }
    00044
    00045 private:
    00046   unsigned long time;
    00047   T object;
    00048
    00049 };
    00050 //------------------------------------------------------------------------------
    00051 template <class T>
    00052 bool operator<(const CTimeStampedObject<T>& obj1,
    00053                const CTimeStampedObject<T>& obj2)
    00054 {
    00055   return (obj1.getTime() < obj2.getTime());
    00056 }
    00057 //------------------------------------------------------------------------------
    00058 class CLogAnalyzerBase;
    00059 //------------------------------------------------------------------------------
    00060 template <class T>
    00061 class CTimeStampedObjectCollection
    00062 {
    00063 public:
    00064   std::vector<CTimeStampedObject<T> >& getCollection()
    00065   {
    00066     return collection;
    00067   }
    00068
    00069   void add(unsigned long time, const T& object)
    00070   {
    00071     collection.push_back(CTimeStampedObject<T>(time, object));
    00072   }
    00073
    00074   unsigned long startTime()
    00075   {
    00076     return collection[0].getTime();
    00077   }
    00078
    00079   unsigned long endTime()
    00080   {
    00081     return collection[collection.size() - 1].getTime();
    00082   }
    00083
    00084   void sort() { std::sort(collection.begin(), collection.end()); }
    00085
    00086   void reset() { collection.clear(); }
    00087
    00088   void offestTime(long offset)
    00089   {
    00090     std::vector<CTimeStampedObject<T> >::iterator pos;
    00091     for (pos = collection.begin(); pos != collection.end(); ++pos)
    00092       pos->setTime(pos->getTime() + offset);
    00093   }
    00094
    00095 private:
    00096   std::vector<CTimeStampedObject<T> > collection;
    00097 };
    00098 //------------------------------------------------------------------------------
    00099 class CLogAnalyzerBase
    00100 {
    00101 public:
    00102   CLogAnalyzerBase();
    00103   ~CLogAnalyzerBase();
    00104
    00105 public:
    00106   void openLogfile(const std::string& logfilename);
    00107
    00108 private:
    00109   void checkExtension(const std::string& filename);
    00110   void reset();
    00111   void readLogfile();
    00112   void readRemoteCamWorldState();
    00113   void readWorldState();
    00114   void readPercepts();
    00115   void saveActualDataRows();
    00116   void synchronize();
    00117
    00118   unsigned long findSmallestTimestamp();
    00119   template <class T>
    00120   unsigned long findSmallestTimestamp(
    00121     std::vector<CTimeStampedObjectCollection<T> >& col)
    00122   {
    00123     unsigned long smallest = (unsigned long)-1;
    00124     std::vector<CTimeStampedObjectCollection<T> >::iterator pos;
    00125     for (pos = col.begin(); pos != col.end(); ++pos)
    00126     {
    00127       pos->sort();
    00128       if (pos->startTime() < smallest)
    00129         smallest = pos->startTime();
    00130     }
    00131     return smallest;
    00132   }
    00133
    00134   void sortCollections();
    00135   template <class T>
    00136   void sortCollection(
    00137     std::vector<CTimeStampedObjectCollection<T> >& col)
    00138   {
    00139     std::vector<CTimeStampedObjectCollection<T> >::iterator pos;
    00140     for (pos = col.begin(); pos != col.end(); ++pos)
    00141       pos->sort();
    00142   }
    00143
    00144   void offsetCamCollections(long offset);
    00145   template <class T>
    00146   void offsetCollection(
    00147     std::vector<CTimeStampedObjectCollection<T> >& col,
    00148     long offset)
    00149   {
    00150     std::vector<CTimeStampedObjectCollection<T> >::iterator pos;
    00151     for (pos = col.begin(); pos != col.end(); ++pos)
    00152       pos->offestTime(offset);
    00153   }
    00154
    00155   template <class T>
    00156   bool available(
    00157     const std::vector<CTimeStampedObjectCollection<T> >& col,
    00158     unsigned long timestamp) const
    00159   {
    00160     std::vector<CTimeStampedObjectCollection<T> >::const_iterator pos;
    00161     for (pos = col.begin(); pos != col.end(); ++pos)
    00162     {
    00163       if ((timestamp >= pos->startTime()) &&
    00164           (timestamp <= pos->endTime()))
    00165         return true;
    00166     }
    00167     return false;
    00168   }
    00169
    00170   template <class T>
    00171   T getInterpolatedValue(
    00172     const std::vector<CTimeStampedObjectCollection<T> >& col,
    00173     unsigned long timestamp) const
    00174   {
    00175     std::vector<CTimeStampedObjectCollection<T> >::const_iterator pos;
    00176     for (pos = col.begin(); pos != col.end(); ++pos)
    00177     {
    00178       if ((timestamp < pos->startTime()) ||
    00179           (timestamp > pos->endTime()))
    00180         continue;
    00181
    00182       std::vector<CTimeStampedObject<T> >::const_iterator lower = lower_bound(
    00183         pos->getCollection().begin(), pos->getCollection().end(), timestamp);
    00184       if (timestamp == pos->endTime())
    00185         return lower->getObject();
    00186
    00187       const CTimeStampedObject<T>& tso1 = *lower;
    00188       const CTimeStampedObject<T>& tso2 = *(lower + 1);
    00189       return interpolate(tso1, tso2, timestamp);
    00190     }
    00191     throw CLogAnalyzerException(CLogAnalyzerException::ValueNotAvailable);
    00192   }
    00193
    00194   static RobotPose interpolate(
    00195     const CTimeStampedObject<RobotPose>& tso1,
    00196     const CTimeStampedObject<RobotPose>& tso2,
    00197     unsigned long timestamp);
    00198   static Vector2<double> interpolate(
    00199     const CTimeStampedObject<Vector2<double> >& tso1,
    00200     const CTimeStampedObject<Vector2<double> >& tso2,
    00201     unsigned long timestamp);
    00202
    00203 private:
    00204   MessageQueue nullQueue;
    00205   LogPlayer logfile;
    00206
    00207   bool foundSyncTimestampRemoteCam;
    00208   bool foundSyncTimestampRobot;
    00209   unsigned long syncTimestampRemoteCam;
    00210   unsigned long syncTimestampRobot;
    00211
    00212   int framesSinceLastError;
    00213   int errors;
    00214   int drops;
    00215
    00216   RobotPose lastCamRobotPose;
    00217   CTimeStampedObjectCollection<RobotPose> actualCamRobotPoses;
    00218   CTimeStampedObjectCollection<Vector2<double > > actualCamBallPositions;
    00219   CTimeStampedObjectCollection<Vector2<double > > actualCamBallSpeeds;
    00220
    00221   CTimeStampedObjectCollection<RobotPose> actualRobRobotPoses;
    00222
    00223 protected:
    00224   std::vector<CTimeStampedObjectCollection<RobotPose> > camRobotPoses;
    00225   std::vector<CTimeStampedObjectCollection<RobotPose> > robRobotPoses;
    00226   std::vector<CTimeStampedObjectCollection<Vector2<double> > > camBallPositions;
    00227   std::vector<CTimeStampedObjectCollection<Vector2<double> > > camBallSpeeds;
    00228 };
    00229 //------------------------------------------------------------------------------
    00230 #endif
    00231 //------------------------------------------------------------------------------