Representations/WLan/TeamMessage.cpp

Go to the documentation of this file.

/**
* @file Representations/WLan/TeamMessage.cpp
*
* Implementation of class TeamMessage.
*
* @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
* @author <A href=mailto:sebastian.schmidt@udo.edu>Sebastian Schmidt</A>
* @author <A href=mailto:m_wachter@gmx.de>Michael Wachter</A>
*/

#include "TeamMessage.h"
#include "Platform/SystemCall.h"
//#include "Modules/BallLocator/GT2005Particles/GT2005ParticleContainer.h"
#include "Platform/GTAssert.h"

//#define STREAMMARKER(marker) stream << (int) marker + 10000
//#define CHECKMARKER(marker) { int dummy = 0; stream >> dummy ; ASSERT (dummy == marker + 10000 ); }

//int TeamMessage::cryptWord;

TeamMessage::TeamMessage() {
  timeStamp = 0;
  incomingTimeStamp[0] = 0;
  incomingTimeStamp[1] = 0;
  incomingTimeStamp[2] = 0;
  incomingTimeStamp[3] = 0;
  lastReceivedTimeStamp[0] = 0;
  lastReceivedTimeStamp[1] = 0;
  lastReceivedTimeStamp[2] = 0;
  lastReceivedTimeStamp[3] = 0;
  playerNumberOfSender = Player::undefinedPlayerNumber;
  resetSendMarkers();

 // Darmstadt ? Crpypto erts mal deaktiviert ;)
 // char* temp = getPlayer().getTeamName();
 // cryptWord = ((int) *temp) + 7 + ((((int) *temp+1) + 17) << 8) + ((((int) *temp+2) + 37) << 16) + ((((int) *temp+3) + 19) << 24);
}

void TeamMessage::resetSendMarkers()
{
  sendRobotPose = false;
  sendSeenBallState = false;
  sendBehaviorTeamMessage = false;
  sendPlayersPercept = false;
  sendSensorBehaviorTeamMessage = false;
  sendOdometryData = false;
  sendMultipleBallPerceptList = false;
  sendGT2005ParticleContainerSend = false;
  sendRobotPoseCollection = false;
  sendObstaclesModel = false;
}

TeamMessage::~TeamMessage()
{

}

bool TeamMessage::isActual() const
{
  return SystemCall::getTimeSince(lastReceivedTimeStamp[(int)getPlayer().getPlayerNumber()]) < 2000;
}

bool TeamMessage::isActual(int i) const
{
  return (SystemCall::getTimeSince(lastReceivedTimeStamp[(int)getPlayer().getPlayerNumber()]) < (unsigned int)i );
}

In& operator>>(In& stream,TeamMessage& teamMessage)
{
  stream >> teamMessage.timeStamp; 
  for (int i=0; i<4; i++)
  {
    stream >> teamMessage.lastReceivedTimeStamp[i];
    stream >> teamMessage.incomingTimeStamp[i];
  }

  char c;
  stream >> c;
  teamMessage.playerNumberOfSender = (Player::playerNumber)c;

  int fieldsInMessage;
  stream >> fieldsInMessage;

  teamMessage.sendRobotPose = ((fieldsInMessage & 1) == 1);
  teamMessage.sendSeenBallState = ((fieldsInMessage & 2) == 2);
  teamMessage.sendBehaviorTeamMessage = ((fieldsInMessage & 4) == 4);
  teamMessage.sendPlayersPercept = ((fieldsInMessage & 8) == 8);
  teamMessage.sendSensorBehaviorTeamMessage = ((fieldsInMessage & 16) == 16);
  teamMessage.sendOdometryData = ((fieldsInMessage & 32) == 32);
  teamMessage.sendMultipleBallPerceptList = ((fieldsInMessage & 64) == 64);
  teamMessage.sendGT2005ParticleContainerSend = ((fieldsInMessage & 128) == 128);
  teamMessage.sendRobotPoseCollection = ((fieldsInMessage & 256) == 256);
  teamMessage.sendObstaclesModel = ((fieldsInMessage & 512) == 512);

  // robot pose
  if (teamMessage.sendRobotPose)     
  {
    //CHECKMARKER(1);
    TeamMessage::read(stream,teamMessage.robotPose);
    stream >> c;
    teamMessage.robotPose.setValidity(c / 100.0);
  }

  if (teamMessage.sendSeenBallState) 
  {
    //CHECKMARKER(2);
    stream >> teamMessage.seenBallState;
  }

  if (teamMessage.sendBehaviorTeamMessage) 
  {
    //CHECKMARKER(3);
    stream >> teamMessage.behaviorTeamMessage;
  }

  if (teamMessage.sendPlayersPercept) 
  {
    //CHECKMARKER(4);
    stream >> teamMessage.playersPercept;
  }

  if (teamMessage.sendSensorBehaviorTeamMessage)
  {
    //CHECKMARKER(5);
    stream >> teamMessage.sensorBehaviorTeamMessage;
  }

  if (teamMessage.sendOdometryData)
  {
    //CHECKMARKER(6);
    stream >> teamMessage.odometryData;
  }

  if (teamMessage.sendMultipleBallPerceptList)
  {
    //CHECKMARKER(7);
    stream >> teamMessage.multipleBallPerceptList;
  }

  if (teamMessage.sendGT2005ParticleContainerSend)
  {
    //CHECKMARKER(8);
    stream >> teamMessage.gt2005ParticleContainerSend;
  }

  if (teamMessage.sendRobotPoseCollection)
  {
    //CHECKMARKER(9);
    stream >> teamMessage.robotPoseCollection;
  }

  if (teamMessage.sendObstaclesModel) 
  {
    //CHECKMARKER(10);
    int boolReplacement;
    stream >> boolReplacement
      >> teamMessage.obstaclesModel.widthOfFreePartOfGoal[ObstaclesModel::opponentGoal];
    if (boolReplacement)
      teamMessage.obstaclesModel.angleToFreePartOfGoalWasDetermined[ObstaclesModel::opponentGoal] = 1;
    else
      teamMessage.obstaclesModel.angleToFreePartOfGoalWasDetermined[ObstaclesModel::opponentGoal] = 0;
  }

  return stream;
}

Out& operator<<(Out& stream,const TeamMessage& teamMessage)
{
  stream << teamMessage.timeStamp;
  for (int i=0; i<4; i++)
  {
    stream << teamMessage.lastReceivedTimeStamp[i];
    stream << teamMessage.incomingTimeStamp[i];
  }

  stream << (char)getPlayer().getPlayerNumber();

  // a bitfield which tells the receiver what fields are transmitted
  int fieldsInMessage = 

    teamMessage.sendRobotPose * 1
    + (teamMessage.sendSeenBallState * 2)
    + (teamMessage.sendBehaviorTeamMessage * 4)
    + (teamMessage.sendPlayersPercept * 8)
    + (teamMessage.sendSensorBehaviorTeamMessage * 16)
    + (teamMessage.sendOdometryData * 32)
    + (teamMessage.sendMultipleBallPerceptList * 64)
    + (teamMessage.sendGT2005ParticleContainerSend * 128)
    + (teamMessage.sendRobotPoseCollection * 256)
    + (teamMessage.sendObstaclesModel * 512)
    ;
  stream << fieldsInMessage;

  // robot pose
  if (teamMessage.sendRobotPose) 
  {
    //STREAMMARKER(1);
    TeamMessage::write(stream,teamMessage.robotPose);
    stream << char(teamMessage.robotPose.getValidity() * 100.0 + 0.5);
  }

  if (teamMessage.sendSeenBallState) 
  {
    //STREAMMARKER(2);
    stream << teamMessage.seenBallState;
  }

  // BehaviorTeamMessage
  if (teamMessage.sendBehaviorTeamMessage) 
  {
    //STREAMMARKER(3);
    stream << teamMessage.behaviorTeamMessage;
  }

  if (teamMessage.sendPlayersPercept) 
  {
    //STREAMMARKER(4);
    stream << teamMessage.playersPercept;
  }

  if (teamMessage.sendSensorBehaviorTeamMessage)
  {
    //STREAMMARKER(5);
    stream << teamMessage.sensorBehaviorTeamMessage;
  }

  if (teamMessage.sendOdometryData)
  {
    //STREAMMARKER(6);
    stream << teamMessage.odometryData;
  }

  if (teamMessage.sendMultipleBallPerceptList)
  {
    //STREAMMARKER(7);
    stream << teamMessage.multipleBallPerceptList;
  }

  if (teamMessage.sendGT2005ParticleContainerSend)
  {
    //STREAMMARKER(8);
    stream << teamMessage.gt2005ParticleContainerSend;
  }

  if (teamMessage.sendRobotPoseCollection)
  {
    //STREAMMARKER(9);
    stream << teamMessage.robotPoseCollection;
  }

  if (teamMessage.sendObstaclesModel) 
  {
    //STREAMMARKER(10);
    int boolReplacement;
    teamMessage.obstaclesModel.angleToFreePartOfGoalWasDetermined[ObstaclesModel::opponentGoal] ? boolReplacement = 1 : boolReplacement = 0;
    stream << boolReplacement
      << teamMessage.obstaclesModel.widthOfFreePartOfGoal[ObstaclesModel::opponentGoal];
  }

  return stream;
}

void TeamMessage::read(In& stream,Vector2<double>& v)
{
  short x,y;
  stream >> x >> y;
  // x = x ^ cryptWord;
  // y = y ^ cryptWord;
  v.x = x; 
  v.y = y;
}

void TeamMessage::read(In& stream,Pose2D& p)
{
  read(stream,p.translation);
  short rotation;
  stream >> rotation;
  p.rotation = rotation / 1000.0;
}

void TeamMessage::write(Out& stream,const Vector2<double>& v)
{
  short x = short(v.x + 0.5),y = short(v.y + 0.5);
  // x = x ^ cryptWord;
  // y = y ^ cryptWord;
  stream << x << y;
}

void TeamMessage::write(Out& stream,const Pose2D& p)
{
  write(stream,p.translation);
  stream << short(p.rotation * 1000 + 0.5);
}

long TeamMessage::getTimeStampInOwnTime() const
{
  return(timeStamp - timeOffset);
}

long TeamMessage::getTimeInOwnTime(long time) const
{
  return(time - timeOffset);
}

long TeamMessage::getTimeInRemoteTime(long time) const
{
  return(time + timeOffset);
}

---