Platform/Aperios1.3.2/MessageQueueBase.cpp

Go to the documentation of this file.

/**
* @file Platform/Aperios1.3.2/MessageQueueBase.cpp
* 
* Implementation of class MessageQueueBase for Aperios.
*
* @author Martin Lötzsch
*/

#include "MessageQueueBase.h"
#include "GTAssert.h"
#include "SystemCall.h"
#include "Tools/Debugging/DebugDrawings.h"
#include "Tools/Debugging/Stopwatch.h"
#include <stdlib.h>
#include <string.h>

MessageQueueBase::MessageQueueBase()
: buf(0), queueSize(0)
{
  clear();
}

MessageQueueBase::~MessageQueueBase()
{
  if (buf != 0) delete[] buf;
}

void MessageQueueBase::setSize(int size)
{
  ASSERT(buf == 0);
  buf = (char*)malloc(size);
  queueSize = size;
  ASSERT(buf);
}

void MessageQueueBase::clear()
{
  nextMessagePosition = 0;
  numOfMessages = 0;
  nextMessageSize = 0;
  writingOfLastMessageFailed = false;
  selectedMessageForReadingPosition = 0;
  readPosition = 0;
}

int MessageQueueBase::getNumberOfMessages() const
{
  return numOfMessages;
}

void MessageQueueBase::write(const void* p,int size)
{
  if (nextMessagePosition + 12 + nextMessageSize + size >= queueSize)
  {
    writingOfLastMessageFailed = true;
  }
  else
  {
    memcpy(buf + nextMessagePosition + 12 + nextMessageSize, p, size);
    nextMessageSize += size;
  }
}

void MessageQueueBase::finishMessage(MessageID id, unsigned long timeStamp,
    Player::teamColor teamColor,Player::playerNumber playerNumber,
    bool messageWasSentFromAPhysicalRobot)
{
  if (!writingOfLastMessageFailed)
  {
    ASSERT(nextMessageSize > 0);
    memcpy(buf + nextMessagePosition, &nextMessageSize, 4); // write the size of the message
    memcpy(buf + nextMessagePosition + 4, (char*)&id, 1); // write the id of the message
    memcpy(buf + nextMessagePosition + 5, &timeStamp, 4); // write the timeStamp
    memcpy(buf + nextMessagePosition + 9, (char*)&teamColor, 1); // write the team color 
    memcpy(buf + nextMessagePosition + 10,(char*)&playerNumber, 1); // write the player number
    memcpy(buf + nextMessagePosition + 11,(char*)&messageWasSentFromAPhysicalRobot, 1); 
    numOfMessages++;
    nextMessagePosition += nextMessageSize + 12;
  }
  nextMessageSize = 0;
  writingOfLastMessageFailed = false;
}

void MessageQueueBase::removeRepetitions()
{
  int messagesPerType[numOfMessageIDs],
      fieldDrawings[Drawings::numberOfFieldDrawings],
      imageDrawings[Drawings::numberOfImageDrawings],
      stopWatches[Stopwatch::numberOfStopwatchEventIDs];
  memset(messagesPerType,0,sizeof(messagesPerType));
  memset(fieldDrawings,0,sizeof(fieldDrawings));
  memset(imageDrawings,0,sizeof(imageDrawings));
  memset(stopWatches,0,sizeof(stopWatches));
  selectedMessageForReadingPosition=0;
  int i;
  for (i=0;i<numOfMessages;i++)
  {
    if(getMessageID() == idDebugDrawingFinished)
    {
      if(Drawings::TypeOfDrawing(getData()[1]) == Drawings::drawingOnField)
        fieldDrawings[int(getData()[0])]++;
      else
        imageDrawings[int(getData()[0])]++;
    }
    else if(getMessageID() == idStopwatch)
      stopWatches[int(getData()[0])]++;
    else
      messagesPerType[getMessageID()]++;
    selectedMessageForReadingPosition += getMessageSize() + 12;
  }
  selectedMessageForReadingPosition=0;
  nextMessagePosition = 0;
  int numOfDeleted=0;
  for (i=0; i < numOfMessages; i++)
  {
    int mlength = getMessageSize() + 12;
    bool copy;
    switch (getMessageID())
    {
    case idDebugDrawingFinished:
      if(Drawings::TypeOfDrawing(getData()[1]) == Drawings::drawingOnField)
        copy = --fieldDrawings[int(getData()[0])] == 0;
      else 
        copy = --imageDrawings[int(getData()[0])] == 0;
      break;
    case idDebugDrawing2:
      if(Drawings::TypeOfDrawing(getData()[2]) == Drawings::drawingOnField)
        copy = fieldDrawings[int(getData()[1])] <= 1;
      else
        copy = imageDrawings[int(getData()[1])] <= 1;
      break;
    case idStopwatch:
      copy = --stopWatches[int(getData()[0])] == 0;
      break;
    case idText:
      //allow up to 20 idTexts in realtime mode, because several of that kind may be send simultaneously:
      copy = --messagesPerType[getMessageID()] <= 20;
      break;
    default:
      copy = --messagesPerType[getMessageID()] == 0;
    }
    if(copy)
    {
      //this message is important, it is the last of its type, it shall be copied
      for (int l=0;l<mlength;l++)
      {
        buf[nextMessagePosition++] = buf[selectedMessageForReadingPosition++];
      }
    }
    else
    {
      numOfDeleted++;
      selectedMessageForReadingPosition += mlength;
    }
  }
  readPosition=0;
  numOfMessages -= numOfDeleted;
}

void MessageQueueBase::setSelectedMessageForReading(int message)
{
  ASSERT(message >= 0);
  ASSERT(message < numOfMessages);

  selectedMessageForReadingPosition = 0;
  for (int i=0; i < message; i++)
  {
    selectedMessageForReadingPosition += getMessageSize() + 12;
  }
  readPosition = 0;
}

void MessageQueueBase::read(void* p,int size)
{
  ASSERT(readPosition + size <= getMessageSize());
  memcpy(p,buf + selectedMessageForReadingPosition + 12 + readPosition, size);
  readPosition += size;
} 

const char* MessageQueueBase::getData() const
{
  return buf + selectedMessageForReadingPosition + 12;
}

int MessageQueueBase::getMessageSize() const
{
  int size;
  memcpy(&size, buf + selectedMessageForReadingPosition, 4);
  return size;
}


void MessageQueueBase::resetReadPosition()
{
  readPosition = 0;
}

unsigned long MessageQueueBase::getTimeStamp() const
{
  unsigned long timeStamp;
  memcpy(&timeStamp, buf + selectedMessageForReadingPosition + 5, 4);
  return timeStamp;
}

unsigned long MessageQueueBase::getTimeStamp(int message)
{
  setSelectedMessageForReading(message);
  return getTimeStamp();
}

MessageID MessageQueueBase::getMessageID() const
{
  char id;
  memcpy(&id, buf + selectedMessageForReadingPosition + 4, 1);
  return (MessageID)id;
}

Player::teamColor MessageQueueBase::getTeamColor() const
{
  char color;
  memcpy(&color, buf + selectedMessageForReadingPosition + 9, 1);
  return (Player::teamColor)color;
}

Player::playerNumber MessageQueueBase::getPlayerNumber() const
{
  char number;
  memcpy(&number, buf + selectedMessageForReadingPosition + 10, 1);
  return (Player::playerNumber)number;
}

bool MessageQueueBase::getMessageWasSentFromAPhysicalRobot() const
{
  char b;
  memcpy(&b, buf + selectedMessageForReadingPosition + 11, 1);
  return (bool)b;
}


bool MessageQueueBase::eof() const
{
  return readPosition == getMessageSize();
}


/*
* Change log :
* 
* $Log: MessageQueueBase.cpp,v $
* Revision 1.1.1.1  2004/05/22 17:23:26  cvsadm
* created new repository GT2004_WM
*
* Revision 1.4  2004/04/14 14:48:56  roefer
* Fixing replaceRepetitions for idStopwatch, 3rd try
*
* Revision 1.3  2004/03/05 15:49:47  dueffert
* 20 idTexts in realtime mode
*
* Revision 1.2  2003/12/06 23:23:55  loetzsch
* messages in a MessageQueue now contain
* - the team color of the robot which sent the message
* - the player number of the robot which sent the message
* - if the message was sent from a physical robot or not
*
* Revision 1.1  2003/10/07 10:06:59  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.2  2003/09/26 11:36:26  juengel
* - sorted tools
* - clean-up in DataTypes
*
* Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.8  2003/06/22 12:55:24  roefer
* Stopwatch RT compatible (2nd try)
*
* Revision 1.7  2003/06/13 21:46:47  roefer
* Real time mode now supports debug drawings
*
* Revision 1.6  2003/03/06 11:56:25  dueffert
* re-order warning removed
*
* Revision 1.5  2002/12/19 11:48:34  roefer
* ASSERT moved
*
* Revision 1.4  2002/12/16 23:49:44  roefer
* removeRepetitions sets nextMessagePosition
*
* Revision 1.3  2002/12/06 16:28:45  dueffert
* bug fixed: real time mode works now
*
* Revision 1.2  2002/12/05 16:13:07  dueffert
* started implementing realtime sending
*
* Revision 1.1  2002/09/10 15:40:04  cvsadm
* Created new project GT2003 (M.L.)
* - Cleaned up the /Src/DataTypes directory
* - Removed challenge related source code
* - Removed processing of incoming audio data
* - Renamed AcousticMessage to SoundRequest
*
* Revision 1.4  2002/09/03 14:43:06  dueffert
* bug fixed
*
* Revision 1.3  2002/08/30 17:10:57  dueffert
* removed unused includes
*
* Revision 1.2  2002/08/10 17:09:24  roefer
* ASSERT bug fixed
*
* Revision 1.1  2002/07/23 13:36:39  loetzsch
* - exchanged StaticQueue by MessageQueue with platform dependend
*   base classes
*
*/

---