Representations/WLan/TeamMessageCollection.cpp

Go to the documentation of this file.

/**
* @file Representations/WLan/TeamMessageCollection.cpp
*
* Implementation of class TeamMessageCollection.
*
* @author <A href=mailto:sebastian.schmidt@udo.edu>Sebastian Schmidt</A>
* @author <A href=mailto:m_wachter@gmx.de>Michael Wachter</A>
*/

#include "TeamMessageCollection.h"
#include "Platform/SystemCall.h"
#include "Tools/Player.h"
#include "Tools/Debugging/Debugging.h"

TeamMessageCollection::TeamMessageCollection()
{
  lastSendingTimeStamp = 0;
  for (int a=0; a< maxNumberOfTeamMessages; a++)
  {
    incomingTimeStamp[a] = teamMessageTimeStamp[a] = 0;
  }
  
  int i;
   
  // all ringbuffer elements are set to zero. 
  for(int j=0;j< maxNumberOfTeamMessages ;j++) 
  {
    for(int i=0;i<100;i++) offsets[j].add(0);
    offsets[j].init();
    timeOffsetSum[j]=0;
    offsets[j].add(0);
  }
  
  messagesSent = false;
  for(i=0;i< maxNumberOfTeamMessages ;i++) 
  {
    teamMessages[i] = NULL;
  }

  for(i=0;i< maxNumberOfTeamMessages ;i++) 
  {
    teamMessagesForActualTeamMessages[i] = 0;
  }

  numberOfInTeamMessages = 0;
  numberOfOutTeamMessages = 0;
  lastSendTime=0;
  sendNow = false;
  delay = 100;
 }

void TeamMessageCollection::processTimeStamps()
{
  signed long actualOffset;
  for (int i=0; i < numberOfInTeamMessages; i++)
  {
  /*  Offset = (1/2) * ( T2 - T1 + T3 - T4 )  =  T5 - T4
  *  
  *  T1 = LastReceivedTimeStamp
  *  T2 = IncomimgTimeStamp
  *  T3 = TimeStamp
  *  T4 = System-Time
  *  T5 = System-Time of the other Robot
  *  
    */
    
    actualOffset= ( (signed long)teamMessages[i]->incomingTimeStamp[teamMessages[i]->playerNumberOfSender] -
      (signed long)teamMessages[i]->lastReceivedTimeStamp[(int)getPlayer().getPlayerNumber()] +
      (signed long)teamMessages[i]->timeStamp -
      (signed long)SystemCall::getCurrentSystemTime() ) / 2 ;
    
    // calculating the average of the values in the ringBuffer
    timeOffsetSum[i] += actualOffset;
    timeOffsetSum[i] -= offsets[i].getEntry(-1);
    
    (const_cast<TeamMessage*>(teamMessages[i]))->timeOffset =
      //((TeamMessage*)teamMessages[i])->timeOffset =
      timeOffsetSum[i] / offsets[i].getNumberOfEntries();
    offsets[i].add(actualOffset);
    
    // Setting timeStamps in teamMessages
  }
    
  for (int i=0; i<numberOfInTeamMessages; i++)
  {
    for (int j=0; j<numberOfOutTeamMessages; j++)
    {
       outTeamMessages[j]->incomingTimeStamp[(int)getPlayer().getPlayerNumber()] = 
       SystemCall::getCurrentSystemTime();
       if (teamMessages[i]->playerNumberOfSender != Player::undefinedPlayerNumber)
       {
          outTeamMessages[j]->lastReceivedTimeStamp[(int)teamMessages[i]->playerNumberOfSender] 
          = teamMessages[i]->timeStamp;
       }
    }
  }

    
  INFO(sendTimeOffsets,idText,text,"OTM: ITS: " << outTeamMessages[0]->incomingTimeStamp[0] << 
    " " << outTeamMessages[0]->incomingTimeStamp[1] << 
    " " << outTeamMessages[0]->incomingTimeStamp[2] << 
    " " << outTeamMessages[0]->incomingTimeStamp[3] << 
    " LRTS: " << outTeamMessages[0]->lastReceivedTimeStamp[0] <<
    " " << outTeamMessages[0]->lastReceivedTimeStamp[1] <<
    " " << outTeamMessages[0]->lastReceivedTimeStamp[2] <<
    " " << outTeamMessages[0]->lastReceivedTimeStamp[3]);

  INFO(sendTimeOffsets,idText,text,"TM0: TS: " << teamMessages[0]->timeStamp << 
    " POS: " << teamMessages[0]->playerNumberOfSender <<
    " ITS: " << teamMessages[0]->incomingTimeStamp[0] << 
    " " << teamMessages[0]->incomingTimeStamp[1] << 
    " " << teamMessages[0]->incomingTimeStamp[2] << 
    " " << teamMessages[0]->incomingTimeStamp[3] <<
    " LRTS: " << teamMessages[0]->lastReceivedTimeStamp[0] <<
    " " << teamMessages[0]->lastReceivedTimeStamp[1] <<
    " " << teamMessages[0]->lastReceivedTimeStamp[2] <<
    " " << teamMessages[0]->lastReceivedTimeStamp[3]);

  INFO(sendTimeOffsets,idText,text,"TM1: TS: " << teamMessages[1]->timeStamp << 
    " POS: " << teamMessages[1]->playerNumberOfSender <<
    " ITS: " << teamMessages[1]->incomingTimeStamp[0] << 
    " " << teamMessages[1]->incomingTimeStamp[1] << 
    " " << teamMessages[1]->incomingTimeStamp[2] << 
    " " << teamMessages[1]->incomingTimeStamp[3] <<
    " LRTS: " << teamMessages[1]->lastReceivedTimeStamp[0] <<
    " " << teamMessages[1]->lastReceivedTimeStamp[1] <<
    " " << teamMessages[1]->lastReceivedTimeStamp[2] <<
    " " << teamMessages[1]->lastReceivedTimeStamp[3]);

  INFO(sendTimeOffsets,idText,text,"TM2: TS: " << teamMessages[2]->timeStamp << 
    " POS: " << teamMessages[2]->playerNumberOfSender <<
    " ITS: " << teamMessages[2]->incomingTimeStamp[0] << 
    " " << teamMessages[2]->incomingTimeStamp[1] << 
    " " << teamMessages[2]->incomingTimeStamp[2] << 
    " " << teamMessages[2]->incomingTimeStamp[3] <<
    " LRTS: " << teamMessages[2]->lastReceivedTimeStamp[0] <<
    " " << teamMessages[2]->lastReceivedTimeStamp[1] <<
    " " << teamMessages[2]->lastReceivedTimeStamp[2] <<
    " " << teamMessages[2]->lastReceivedTimeStamp[3]);
   
  INFO(sendTimeOffsets,idText,text,"TimeOffsets : " 
    << teamMessages[0]->timeOffset << " " << teamMessages[0]->playerNumberOfSender <<" "
    << teamMessages[1]->timeOffset << " " << teamMessages[1]->playerNumberOfSender <<" "
    << teamMessages[2]->timeOffset << " " << teamMessages[2]->playerNumberOfSender);
}


void TeamMessageCollection::processMessages()
{   

  numberOfTeamMessages = 0;
  for(int a=0;a< numberOfInTeamMessages;a++)
  {
    // Set the teamMessagesForActualTeamMessages so that operator[] only returns actual 
    // teamMessages. 

    if (teamMessages[a]->isActual())
    {
      teamMessagesForActualTeamMessages[numberOfTeamMessages++] = a;
    }
  }

  for(int i=0; i< numberOfOutTeamMessages; i++)
  {
    outTeamMessages[i]->resetSendMarkers();                           
  }
  
  processTimeStamps();
}

void TeamMessageCollection::setOutTeamMessages(TeamMessage& tm)
{
  outTeamMessages[numberOfOutTeamMessages] = &tm;
  numberOfOutTeamMessages++;  

}

void TeamMessageCollection::setInTeamMessages(TeamMessage& tm)
{
  teamMessages[numberOfInTeamMessages] = &tm;
  numberOfInTeamMessages++;
} 


void TeamMessageCollection::send(const RobotPose& robotPose,bool sendNow) const
{
  for (int i=0;i< numberOfOutTeamMessages;i++)
  {
     outTeamMessages[i]->robotPose = robotPose ;
     outTeamMessages[i]->sendRobotPose = true;
  }
  this->sendNow |= sendNow;
}

void TeamMessageCollection::send(const SeenBallState& seenBallState, bool sendNow) const
{
  for (int i=0;i< numberOfOutTeamMessages;i++)
  {
      outTeamMessages[i]->seenBallState = seenBallState;
      outTeamMessages[i]->sendSeenBallState = true;
  }
  this->sendNow |= sendNow;
}

void TeamMessageCollection::send(const BehaviorTeamMessage& behaviorTeamMessage,bool sendNow) const
{
  for (int i=0;i< numberOfOutTeamMessages;i++)
  {
      outTeamMessages[i]->behaviorTeamMessage = behaviorTeamMessage;
      outTeamMessages[i]->sendBehaviorTeamMessage = true;
  }
  this->sendNow |= sendNow;
}

void TeamMessageCollection::send(const PlayersPercept& playersPercept,bool sendNow) const
{
  for (int i=0;i< numberOfOutTeamMessages;i++)
  {
      outTeamMessages[i]->playersPercept = playersPercept;
      outTeamMessages[i]->sendPlayersPercept = true;
  }
  this->sendNow |= sendNow;
}

void TeamMessageCollection::send(const SensorBehaviorTeamMessage& sensorBehaviorTeamMessage, bool sendNow /* = false */) const
{
  for (int i=0; i< numberOfOutTeamMessages; i++)
  {
    outTeamMessages[i]->sensorBehaviorTeamMessage = sensorBehaviorTeamMessage;
    outTeamMessages[i]->sendSensorBehaviorTeamMessage = true;
  }
  this->sendNow |= sendNow;
}

void TeamMessageCollection::send(const MultipleBallPerceptList& multipleBallPerceptList, bool sendNow /* = false */) const
{
  for (int i=0; i< numberOfOutTeamMessages; i++)
  {
    outTeamMessages[i]->multipleBallPerceptList = multipleBallPerceptList;
    outTeamMessages[i]->sendMultipleBallPerceptList = true;
  }
  this->sendNow |= sendNow;
}

void TeamMessageCollection::send(const OdometryData& odometryData, bool sendNow /* = false */) const
{
  for (int i=0; i< numberOfOutTeamMessages; i++)
  {
    outTeamMessages[i]->odometryData = odometryData;
    outTeamMessages[i]->sendOdometryData = true;   
  }
  this->sendNow |= sendNow;
}

void TeamMessageCollection::send(const GT2005ParticleContainerSend& gt2005ParticleContainerSend, bool sendNow /* = false */) const
{
  for (int i=0; i< numberOfOutTeamMessages; i++)
  {
    outTeamMessages[i]->gt2005ParticleContainerSend = gt2005ParticleContainerSend;
    outTeamMessages[i]->sendGT2005ParticleContainerSend = true;
  }
  this->sendNow |= sendNow;    
}

void TeamMessageCollection::send(const RobotPoseCollection& robotPoseCollection, bool sendNow /* = false */) const
{
  for (int i=0; i< numberOfOutTeamMessages; i++)
  {
    outTeamMessages[i]->robotPoseCollection = robotPoseCollection;
    outTeamMessages[i]->sendRobotPoseCollection = true;
    
  }
  this->sendNow |= sendNow;
}

void TeamMessageCollection::send(const ObstaclesModel& obstaclesModel,bool sendNow) const
{
  for (int i=0;i< numberOfOutTeamMessages;i++)
  {
    outTeamMessages[i]->obstaclesModel = obstaclesModel;
    outTeamMessages[i]->sendObstaclesModel = true;
  }
  this->sendNow |= sendNow;
}

bool TeamMessageCollection::processOutMessages() {
  
  // is te delay over ?
  sendNow |= ((signed)(SystemCall::getCurrentSystemTime() - (signed) lastSendTime) > delay);
  if (sendNow) 
  {
    // set timestamps for all outgoing TeamMessages
    for(int a=0;a< numberOfOutTeamMessages;a++) {
      outTeamMessages[a]->timeStamp = SystemCall::getCurrentSystemTime();
    }

    messagesSent = true;
    lastSendTime = SystemCall::getCurrentSystemTime();
    sendNow = false;
    return (true);
  }
  else return (false);
}


void TeamMessageCollection::setDelay(int delay)
{
  this->delay = delay;
}

int TeamMessageCollection::getDelay() const
{
  return (delay);
}

---