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

# Platform/Aperios1.3.2/MessageQueueBase.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file Platform/Aperios1.3.2/MessageQueueBase.cpp
    00003 *
    00004 * Implementation of class MessageQueueBase for Aperios.
    00005 *
    00006 * @author Martin Lötzsch
    00007 */
    00008
    00009 #include "MessageQueueBase.h"
    00010 #include "GTAssert.h"
    00011 #include "SystemCall.h"
    00012 #include "Tools/Debugging/DebugDrawings.h"
    00013 #include "Tools/Debugging/Stopwatch.h"
    00014 #include <stdlib.h>
    00015 #include <string.h>
    00016
    00017 MessageQueueBase::MessageQueueBase()
    00018 : buf(0), queueSize(0)
    00019 {
    00020   clear();
    00021 }
    00022
    00023 MessageQueueBase::~MessageQueueBase()
    00024 {
    00025   if (buf != 0) delete[] buf;
    00026 }
    00027
    00028 void MessageQueueBase::setSize(int size)
    00029 {
    00030   ASSERT(buf == 0);
    00031   buf = (char*)malloc(size);
    00032   queueSize = size;
    00033   ASSERT(buf);
    00034 }
    00035
    00036 void MessageQueueBase::clear()
    00037 {
    00038   nextMessagePosition = 0;
    00039   numOfMessages = 0;
    00040   nextMessageSize = 0;
    00041   writingOfLastMessageFailed = false;
    00042   selectedMessageForReadingPosition = 0;
    00043   readPosition = 0;
    00044   lastMessage = 0;
    00045 }
    00046
    00047 int MessageQueueBase::getNumberOfMessages() const
    00048 {
    00049   return numOfMessages;
    00050 }
    00051
    00052 void MessageQueueBase::write(const void* p,int size)
    00053 {
    00054   if (nextMessagePosition + 12 + nextMessageSize + size >= queueSize)
    00055   {
    00056     writingOfLastMessageFailed = true;
    00057   }
    00058   else
    00059   {
    00060     memcpy(buf + nextMessagePosition + 12 + nextMessageSize, p, size);
    00061     nextMessageSize += size;
    00062   }
    00063 }
    00064
    00065 void MessageQueueBase::finishMessage(MessageID id, unsigned long timeStamp,
    00066     Player::teamColor teamColor,Player::playerNumber playerNumber,
    00067     bool messageWasSentFromAPhysicalRobot)
    00068 {
    00069   if (!writingOfLastMessageFailed)
    00070   {
    00071     ASSERT(nextMessageSize > 0);
    00072     memcpy(buf + nextMessagePosition, &nextMessageSize, 4); // write the size of the message
    00073     memcpy(buf + nextMessagePosition + 4, (char*)&id, 1); // write the id of the message
    00074     memcpy(buf + nextMessagePosition + 5, &timeStamp, 4); // write the timeStamp
    00075     memcpy(buf + nextMessagePosition + 9, (char*)&teamColor, 1); // write the team color
    00076     memcpy(buf + nextMessagePosition + 10,(char*)&playerNumber, 1); // write the player number
    00077     memcpy(buf + nextMessagePosition + 11,(char*)&messageWasSentFromAPhysicalRobot, 1);
    00078     numOfMessages++;
    00079     nextMessagePosition += nextMessageSize + 12;
    00080   }
    00081   nextMessageSize = 0;
    00082   writingOfLastMessageFailed = false;
    00083 }
    00084
    00085 void MessageQueueBase::removeRepetitions()
    00086 {
    00087
    00088   int messagesPerType[numOfMessageIDs];
    00089 #ifdef NEWDEBUGGING
    00090   unsigned long lastProcessFinishedFrameNumber = 0;
    00091 #else
    00092   int fieldDrawings[Drawings::numberOfFieldDrawings];
    00093   int imageDrawings[Drawings::numberOfImageDrawings];
    00094   memset(fieldDrawings,0,sizeof(fieldDrawings));
    00095   memset(imageDrawings,0,sizeof(imageDrawings));
    00096 #endif
    00097   int stopWatches[Stopwatch::numberOfStopwatchEventIDs];
    00098   memset(messagesPerType,0,sizeof(messagesPerType));
    00099   memset(stopWatches,0,sizeof(stopWatches));
    00100   selectedMessageForReadingPosition=0;
    00101   int i;
    00102   for (i=0;i<numOfMessages;i++)
    00103   {
    00104 #ifdef NEWDEBUGGING
    00105     if( getMessageID() == idProcessFinished)
    00106     {
    00107       lastProcessFinishedFrameNumber = getTimeStamp();
    00108       messagesPerType[getMessageID()]++;
    00109     }
    00110 #else
    00111     if(getMessageID() == idDebugDrawingFinished)
    00112     {
    00113       if(Drawings::TypeOfDrawing(getData()[1]) == Drawings::drawingOnField)
    00114         fieldDrawings[int(getData()[0])]++;
    00115       else
    00116         imageDrawings[int(getData()[0])]++;
    00117     }
    00118 #endif
    00119     else if(getMessageID() == idStopwatch)
    00120       stopWatches[int(getData()[0])]++;
    00121     else
    00122       messagesPerType[getMessageID()]++;
    00123     selectedMessageForReadingPosition += getMessageSize() + 12;
    00124   }
    00125   selectedMessageForReadingPosition=0;
    00126   nextMessagePosition = 0;
    00127   int numOfDeleted=0;
    00128   for (i=0; i < numOfMessages; i++)
    00129   {
    00130     int mlength = getMessageSize() + 12;
    00131     bool copy;
    00132     switch (getMessageID())
    00133     {
    00134      case idDebugDrawing2:
    00135 #ifdef NEWDEBUGGING
    00136       copy = ( getTimeStamp() == lastProcessFinishedFrameNumber);
    00137       break;
    00138 #else
    00139       if(Drawings::TypeOfDrawing(getData()[2]) == Drawings::drawingOnField)
    00140         copy = fieldDrawings[int(getData()[1])] <= 1;
    00141       else
    00142         copy = imageDrawings[int(getData()[1])] <= 1;
    00143       break;
    00144      case idDebugDrawingFinished:
    00145       if(Drawings::TypeOfDrawing(getData()[1]) == Drawings::drawingOnField)
    00146         copy = --fieldDrawings[int(getData()[0])] == 0;
    00147       else
    00148         copy = --imageDrawings[int(getData()[0])] == 0;
    00149       break;
    00150 #endif
    00151     case idStopwatch:
    00152       copy = --stopWatches[int(getData()[0])] == 0;
    00153       break;
    00154     case idText:
    00155       //allow up to 20 idTexts in realtime mode, because several of that kind may be send simultaneously:
    00156       copy = --messagesPerType[getMessageID()] <= 20;
    00157       break;
    00158     case idDebugRequest:
    00159     case idDebugResponse:
    00160     case idStreamSpecification:
    00161     case idDebugDataResponse:
    00162     case idDrawingManager:
    00163     case idModuleSolutionTable:
    00164       copy = true;
    00165       break;
    00166     default:
    00167       copy = --messagesPerType[getMessageID()] == 0;
    00168     }
    00169     if(copy)
    00170     {
    00171       //this message is important, it is the last of its type, it shall be copied
    00172       for (int l=0;l<mlength;l++)
    00173       {
    00174         buf[nextMessagePosition++] = buf[selectedMessageForReadingPosition++];
    00175       }
    00176     }
    00177     else
    00178     {
    00179       numOfDeleted++;
    00180       selectedMessageForReadingPosition += mlength;
    00181     }
    00182   }
    00183   readPosition=0;
    00184   numOfMessages -= numOfDeleted;
    00185   selectedMessageForReadingPosition = 0;
    00186   lastMessage = 0;
    00187 }
    00188
    00189 void MessageQueueBase::setSelectedMessageForReading(int message)
    00190 {
    00191   ASSERT(message >= 0);
    00192   ASSERT(message < numOfMessages);
    00193
    00194   int m = message;
    00195   if(m >= lastMessage)
    00196   {
    00197     ASSERT(lastMessage < numOfMessages);
    00198     m -= lastMessage;
    00199   }
    00200   else
    00201     selectedMessageForReadingPosition = 0;
    00202   for (int i=0; i < m; i++)
    00203   {
    00204     selectedMessageForReadingPosition += getMessageSize() + 12;
    00205   }
    00206   readPosition = 0;
    00207   lastMessage = message;
    00208 }
    00209
    00210 void MessageQueueBase::read(void* p,int size)
    00211 {
    00212   ASSERT(readPosition + size <= getMessageSize());
    00213   memcpy(p,buf + selectedMessageForReadingPosition + 12 + readPosition, size);
    00214   readPosition += size;
    00215 }
    00216
    00217 const char* MessageQueueBase::getData() const
    00218 {
    00219   return buf + selectedMessageForReadingPosition + 12;
    00220 }
    00221
    00222 int MessageQueueBase::getMessageSize() const
    00223 {
    00224   int size;
    00225   memcpy(&size, buf + selectedMessageForReadingPosition, 4);
    00226   return size;
    00227 }
    00228
    00229
    00230 void MessageQueueBase::resetReadPosition()
    00231 {
    00232   readPosition = 0;
    00233 }
    00234
    00235 unsigned long MessageQueueBase::getTimeStamp() const
    00236 {
    00237   unsigned long timeStamp;
    00238   memcpy(&timeStamp, buf + selectedMessageForReadingPosition + 5, 4);
    00239   return timeStamp;
    00240 }
    00241
    00242 unsigned long MessageQueueBase::getTimeStamp(int message)
    00243 {
    00244   setSelectedMessageForReading(message);
    00245   return getTimeStamp();
    00246 }
    00247
    00248 MessageID MessageQueueBase::getMessageID() const
    00249 {
    00250   char id;
    00251   memcpy(&id, buf + selectedMessageForReadingPosition + 4, 1);
    00252
    00253   int i = id;
    00254   i &= 0xff;
    00255   return (MessageID)i;
    00256 }
    00257
    00258 Player::teamColor MessageQueueBase::getTeamColor() const
    00259 {
    00260   char color;
    00261   memcpy(&color, buf + selectedMessageForReadingPosition + 9, 1);
    00262   return (Player::teamColor)color;
    00263 }
    00264
    00265 Player::playerNumber MessageQueueBase::getPlayerNumber() const
    00266 {
    00267   char number;
    00268   memcpy(&number, buf + selectedMessageForReadingPosition + 10, 1);
    00269   return (Player::playerNumber)number;
    00270 }
    00271
    00272 bool MessageQueueBase::getMessageWasSentFromAPhysicalRobot() const
    00273 {
    00274   char b;
    00275   memcpy(&b, buf + selectedMessageForReadingPosition + 11, 1);
    00276   return (bool)b;
    00277 }
    00278
    00279
    00280 bool MessageQueueBase::eof() const
    00281 {
    00282   return readPosition == getMessageSize();
    00283 }