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

# Tools/LogfileAnalyzing/AnalyzerBase.cpp

Go to the documentation of this file.

    00001 //------------------------------------------------------------------------------
    00002 #include "AnalyzerBase.h"
    00003 #include "Representations/Cognition/BallModel.h"
    00004 #include "Representations/Cognition/PlayerPoseCollection.h"
    00005 #include "Representations/Cognition/ObstaclesModel.h"
    00006 #include "Representations/Cognition/RobotState.h"
    00007 #include "Representations/Perception/BallPercept.h"
    00008 #include "Representations/Perception/LandmarksPercept.h"
    00009 #include "Representations/Perception/LinesPercept.h"
    00010 #include "Representations/Perception/PlayersPercept.h"
    00011 #include "Representations/Perception/EdgesPercept.h"
    00012 #include "Representations/Perception/ObstaclesPercept.h"
    00013 #include "Representations/Perception/PSDPercept.h"
    00014 #include "Representations/Perception/CollisionPercept.h"
    00015 //------------------------------------------------------------------------------
    00016 CLogAnalyzerBase::CLogAnalyzerBase()
    00017 : logfile(nullQueue)
    00018 {
    00019   reset();
    00020 }
    00021 //------------------------------------------------------------------------------
    00022 CLogAnalyzerBase::~CLogAnalyzerBase()
    00023 {
    00024 }
    00025 //------------------------------------------------------------------------------
    00026 void CLogAnalyzerBase::openLogfile(const std::string& logfilename)
    00027 {
    00028   reset();
    00029
    00030   checkExtension(logfilename);
    00031
    00032   if (!logfile.open(logfilename.c_str()))
    00033     throw CLogAnalyzerException(CLogAnalyzerException::CouldNotOpenFile);
    00034
    00035   readLogfile();
    00036
    00037   synchronize();
    00038 }
    00039 //------------------------------------------------------------------------------
    00040 void CLogAnalyzerBase::checkExtension(const std::string& filename)
    00041 {
    00042   char szExt[_MAX_EXT];
    00043   _splitpath(filename.c_str(), NULL, NULL, NULL, szExt);
    00044   std::string sExt = szExt;
    00045   for (int i = 0; i < (int)sExt.length(); ++i)
    00046     sExt[i] = tolower(sExt[i]);
    00047   if (sExt == ".slg")
    00048   {
    00049     foundSyncTimestampRemoteCam = true;
    00050     foundSyncTimestampRobot = true;
    00051   }
    00052 }
    00053 //------------------------------------------------------------------------------
    00054 void CLogAnalyzerBase::reset()
    00055 {
    00056   nullQueue.clear();
    00057   logfile.clear();
    00058
    00059   foundSyncTimestampRemoteCam = false;
    00060   foundSyncTimestampRobot = false;
    00061   syncTimestampRemoteCam = 0;
    00062   syncTimestampRobot = 0;
    00063
    00064   framesSinceLastError = 0;
    00065   errors = 0;
    00066   drops = 0;
    00067
    00068   actualCamRobotPoses.reset();
    00069   actualRobRobotPoses.reset();
    00070   actualCamBallPositions.reset();
    00071   actualCamBallSpeeds.reset();
    00072
    00073   camRobotPoses.clear();
    00074   robRobotPoses.clear();
    00075   camBallPositions.clear();
    00076   camBallSpeeds.clear();
    00077 }
    00078 //------------------------------------------------------------------------------
    00079 void CLogAnalyzerBase::readLogfile()
    00080 {
    00081   for (logfile.currentMessageNumber = 0;
    00082        logfile.currentMessageNumber < logfile.getNumberOfMessages();
    00083        ++logfile.currentMessageNumber)
    00084   {
    00085     logfile.queue.setSelectedMessageForReading(logfile.currentMessageNumber);
    00086     switch (logfile.getCurrentMessageID())
    00087     {
    00088     case idSyncTimestampRemoteCam:
    00089       logfile.in.bin >> syncTimestampRemoteCam;
    00090       foundSyncTimestampRemoteCam = true;
    00091       break;
    00092
    00093     case idSyncTimestampRobot:
    00094       logfile.in.bin >> syncTimestampRobot;
    00095       foundSyncTimestampRobot = true;
    00096       break;
    00097
    00098     case idRemoteCamWorldState:
    00099       readRemoteCamWorldState();
    00100       break;
    00101
    00102     case idWorldState:
    00103       readWorldState();
    00104       break;
    00105
    00106     case idPercepts:
    00107       readPercepts();
    00108       break;
    00109     }
    00110   }
    00111
    00112   // Aktuelle Messreihen sichern
    00113   saveActualDataRows();
    00114
    00115 }
    00116 //------------------------------------------------------------------------------
    00117 void CLogAnalyzerBase::readRemoteCamWorldState()
    00118 {
    00119   RobotPose robotPose;
    00120   BallModel ballModel;
    00121   PlayerPoseCollection playerPoseCollection;
    00122   ObstaclesModel obstaclesModel;
    00123   RobotState robotState;
    00124   CameraMatrix cameraMatrix;
    00125   CameraInfo cameraInfo;
    00126
    00127   logfile.in.bin >> RECEIVE_WORLDSTATE(
    00128     robotPose, ballModel, playerPoseCollection, obstaclesModel, robotState,
    00129     cameraMatrix, cameraInfo);
    00130
    00131   if (robotPose.getValidity() == 0.0)
    00132     return;
    00133
    00134   // Sprung größer als 0.5m/s?
    00135   if ((lastCamRobotPose.getValidity() != 0.0) &&
    00136       ((robotPose.translation - lastCamRobotPose.translation).abs() /
    00137        (robotPose.timestamp - lastCamRobotPose.timestamp) > 0.5))
    00138   {
    00139     // Aktuelle Messreihen sichern
    00140     if (!actualCamRobotPoses.getCollection().empty())
    00141     {
    00142       camRobotPoses.push_back(actualCamRobotPoses);
    00143       actualCamRobotPoses.reset();
    00144     }
    00145     if (!actualCamBallPositions.getCollection().empty())
    00146     {
    00147       camBallPositions.push_back(actualCamBallPositions);
    00148       actualCamBallPositions.reset();
    00149     }
    00150     if (!actualCamBallSpeeds.getCollection().empty())
    00151     {
    00152       camBallSpeeds.push_back(actualCamBallSpeeds);
    00153       actualCamBallSpeeds.reset();
    00154     }
    00155
    00156     framesSinceLastError = 0;
    00157     errors++;
    00158     lastCamRobotPose = robotPose;
    00159     return;
    00160   }
    00161
    00162   framesSinceLastError++;
    00163   if ((framesSinceLastError < 4) ||
    00164       (abs(robotPose.translation.x) > 2150))
    00165   {
    00166     drops++;
    00167     lastCamRobotPose = robotPose;
    00168     return;
    00169   }
    00170
    00171   robotPose.rotation = normalize(robotPose.rotation);
    00172   actualCamRobotPoses.add(robotPose.timestamp, robotPose);
    00173
    00174   if (ballModel.ballWasSeen)
    00175   {
    00176     unsigned long time = ballModel.seen.timeWhenLastSeen;
    00177     BallModel bm;
    00178     bm.seen.setBallDataRelativeToField(
    00179       robotPose,
    00180       ballModel.seen.positionField.x,
    00181       ballModel.seen.positionField.y,
    00182       ballModel.seen.speedField.x,
    00183       ballModel.seen.speedField.y);
    00184     actualCamBallPositions.add(time, bm.seen.positionRobot);
    00185     actualCamBallPositions.add(time, bm.seen.speedRobot);
    00186   }
    00187   else
    00188   {
    00189     // Aktuelle Messreihen sichern
    00190     if (!actualCamBallPositions.getCollection().empty())
    00191     {
    00192       camBallPositions.push_back(actualCamBallPositions);
    00193       actualCamBallPositions.reset();
    00194     }
    00195     if (!actualCamBallSpeeds.getCollection().empty())
    00196     {
    00197       camBallSpeeds.push_back(actualCamBallSpeeds);
    00198       actualCamBallSpeeds.reset();
    00199     }
    00200   }
    00201
    00202   lastCamRobotPose = robotPose;
    00203 }
    00204 //------------------------------------------------------------------------------
    00205 void CLogAnalyzerBase::readWorldState()
    00206 {
    00207   RobotPose robotPose;
    00208   BallModel ballModel;
    00209   PlayerPoseCollection playerPoseCollection;
    00210   ObstaclesModel obstaclesModel;
    00211   RobotState robotState;
    00212   CameraMatrix cameraMatrix;
    00213   CameraInfo cameraInfo;
    00214
    00215   logfile.in.bin >> RECEIVE_WORLDSTATE(
    00216     robotPose, ballModel, playerPoseCollection, obstaclesModel, robotState,
    00217     cameraMatrix, cameraInfo);
    00218
    00219   actualRobRobotPoses.add(robotPose.timestamp, robotPose);
    00220 }
    00221 //------------------------------------------------------------------------------
    00222 void CLogAnalyzerBase::readPercepts()
    00223 {
    00224   CameraMatrix cameraMatrix;
    00225   CameraInfo cameraInfo;
    00226   BallPercept ballPercept;
    00227   LandmarksPercept landmarksPercept;
    00228   LinesPercept linesPercept;
    00229   PlayersPercept playersPercept;
    00230   EdgesPercept edgesPercept;
    00231   ObstaclesPercept obstaclesPercept;
    00232   PSDPercept psdPercept;
    00233   CollisionPercept collisionPercept;
    00234
    00235   logfile.in.bin >> RECEIVE_PERCEPTS(cameraMatrix, cameraInfo, ballPercept,
    00236     landmarksPercept, linesPercept, edgesPercept,
    00237     playersPercept, obstaclesPercept, psdPercept, collisionPercept);
    00238 }
    00239 //------------------------------------------------------------------------------
    00240 void CLogAnalyzerBase::saveActualDataRows()
    00241 {
    00242   if (!actualCamRobotPoses.getCollection().empty())
    00243   {
    00244     camRobotPoses.push_back(actualCamRobotPoses);
    00245     actualCamRobotPoses.reset();
    00246   }
    00247   if (!actualCamBallPositions.getCollection().empty())
    00248   {
    00249     camBallPositions.push_back(actualCamBallPositions);
    00250     actualCamBallPositions.reset();
    00251   }
    00252   if (!actualCamBallSpeeds.getCollection().empty())
    00253   {
    00254     camBallSpeeds.push_back(actualCamBallSpeeds);
    00255     actualCamBallSpeeds.reset();
    00256   }
    00257   if (!actualRobRobotPoses.getCollection().empty())
    00258   {
    00259     robRobotPoses.push_back(actualRobRobotPoses);
    00260     actualRobRobotPoses.reset();
    00261   }
    00262 }
    00263 //------------------------------------------------------------------------------
    00264 void CLogAnalyzerBase::synchronize()
    00265 {
    00266   if (!foundSyncTimestampRobot || !foundSyncTimestampRemoteCam)
    00267     throw CLogAnalyzerException(CLogAnalyzerException::LogfileNotSynchronized);
    00268
    00269   //unsigned long smallestTime = findSmallestTimestamp();
    00270   sortCollections();
    00271   long offset = syncTimestampRobot - syncTimestampRemoteCam;
    00272   offsetCamCollections(offset);
    00273 }
    00274 //------------------------------------------------------------------------------
    00275 unsigned long CLogAnalyzerBase::findSmallestTimestamp()
    00276 {
    00277   unsigned long smallest = (unsigned long)-1;
    00278
    00279   smallest = min(smallest, findSmallestTimestamp(camRobotPoses));
    00280   smallest = min(smallest, findSmallestTimestamp(robRobotPoses));
    00281   smallest = min(smallest, findSmallestTimestamp(camBallPositions));
    00282   smallest = min(smallest, findSmallestTimestamp(camBallSpeeds));
    00283
    00284   return smallest;
    00285 }
    00286 //------------------------------------------------------------------------------
    00287 void CLogAnalyzerBase::sortCollections()
    00288 {
    00289   sortCollection(camRobotPoses);
    00290   sortCollection(robRobotPoses);
    00291   sortCollection(camBallPositions);
    00292   sortCollection(camBallSpeeds);
    00293 }
    00294 //------------------------------------------------------------------------------
    00295 void CLogAnalyzerBase::offsetCamCollections(long offset)
    00296 {
    00297   offsetCollection(camRobotPoses, offset);
    00298   offsetCollection(camBallPositions, offset);
    00299   offsetCollection(camBallSpeeds, offset);
    00300 }
    00301 //------------------------------------------------------------------------------
    00302 RobotPose CLogAnalyzerBase::interpolate(
    00303   const CTimeStampedObject<RobotPose>& tso1,
    00304   const CTimeStampedObject<RobotPose>& tso2,
    00305   unsigned long timestamp)
    00306 {
    00307   const RobotPose& p1 = tso1.getObject();
    00308   const RobotPose& p2 = tso2.getObject();
    00309
    00310   double diff = tso2.getTime() - tso1.getTime();
    00311   double diff1 = timestamp - tso1.getTime();
    00312   double diff2 = tso2.getTime() - timestamp;
    00313
    00314   RobotPose interpolated;
    00315   interpolated.translation.x =
    00316     (diff2/diff)*p1.translation.x + (diff1/diff)*p2.translation.x;
    00317   interpolated.translation.y =
    00318     (diff2/diff)*p1.translation.y + (diff1/diff)*p2.translation.y;
    00319   interpolated.rotation =
    00320     (diff1/diff)*p2.rotation + (diff2/diff)*p1.rotation;
    00321   if (fabs(p1.rotation - p2.rotation) > pi)
    00322   {
    00323     if (interpolated.rotation > 0)
    00324       interpolated.rotation -= pi;
    00325     else
    00326       interpolated.rotation += pi;
    00327   }
    00328   interpolated.timestamp = timestamp;
    00329   return interpolated;
    00330 }
    00331 //------------------------------------------------------------------------------
    00332 Vector2<double> CLogAnalyzerBase::interpolate(
    00333   const CTimeStampedObject<Vector2<double> >& tso1,
    00334   const CTimeStampedObject<Vector2<double> >& tso2,
    00335   unsigned long timestamp)
    00336 {
    00337   const Vector2<double>& v1 = tso1.getObject();
    00338   const Vector2<double>& v2 = tso2.getObject();
    00339
    00340   double diff = tso2.getTime() - tso1.getTime();
    00341   double diff1 = timestamp - tso1.getTime();
    00342   double diff2 = tso2.getTime() - timestamp;
    00343
    00344   Vector2<double> interpolated;
    00345   interpolated.x = (diff2/diff)*v1.x + (diff1/diff)*v2.x;
    00346   interpolated.y = (diff2/diff)*v1.y + (diff1/diff)*v2.y;
    00347
    00348   return interpolated;
    00349 }
    00350 //------------------------------------------------------------------------------