Representations/Cognition/GTCamWorldState.cpp

Go to the documentation of this file.

/**
* @file GTCamWorldState.cpp
* 
* Implementation of the GTCamWorldState.
*
* @author Matthias Meyer
*/

#include "GTCamWorldState.h"
#include "Platform/GTAssert.h"

//#include "Platform/UDPEndpoint.h"
#include "Tools/Player.h"
#include <Tools/debugging/debugging.h>
#include "Representations/Cognition/GTCamWorldState.h"
//#include "NetConfig.h"
#include "Tools/TRingBufferWithSum.h"
#include <stdio.h>
#include <float.h>
//#include <iostream.h>

/**
* Contructor
*/
GTCamWorldState::GTCamWorldState(bool myFlip) {
  setFlip(myFlip);
  setTimestamp(0.0);
  for (int i = 0; i < SIZEOFWORLDSTATE; i++) {
    camWorldstate[i] = 0;
  }
}

void GTCamWorldState::setFlip(bool myFlip) {
//  INFO(sendOptionRatings,idText,text,"Flipped State");
  if (myFlip) {
    flip = -1;
  } else {
    flip = 1;
  } 
}

GTCamWorldState::~GTCamWorldState() {
}


/** 
* Get player position from worldstate
*/
Vector2<double> GTCamWorldState::getPlayerPos(int player,int color) const {
  Vector2<double> pos;
  if (playerWasSeen(player, color))
  {
    if (color == Player::blue) {
      player += 4;
    }
    pos.x = camWorldstate[(((player )* 5) + 6) + 0] * flip;
    pos.y = camWorldstate[(((player )* 5) + 6) + 1] * flip;
  }
  else
  {
    pos.x = 0.0;
    pos.y = 0.0;
  }
  return pos;
}

/**
* Get player speed from worldstate
*/
Vector2<double> GTCamWorldState::getPlayerSpeed(int player,int color) const {
  Vector2<double> speed;
  if (playerWasSeen(player, color))
  {
    if (color == Player::blue) {
      player += 4;
    }
    speed.x = camWorldstate[(((player )* 5) + 6) + 3] * flip;
    speed.y = camWorldstate[(((player )* 5) + 6) + 4] * flip;
  }
  else
  {
    speed.x = 0.0;
    speed.y = 0.0;
  }
  return speed;
}

/**
* Get player orientation from worldstate
*/
double GTCamWorldState::getPlayerOrientation(int player,int color) const {
  if (playerWasSeen(player, color))
  {
    if (color == Player::blue) {
      player += 4;
    }
    //  return camWorldstate[(((player )* 5) + 6) + 2] * flip;

    if (flip == 1) {
      // Red dog (no flip needed)
      return camWorldstate[(((player )* 5) + 6) + 2];
    } else {
      // Blue dog (need flipping)
      if (camWorldstate[(((player )* 5) + 6) + 2] > 0) {
        return camWorldstate[(((player )* 5) + 6) + 2] - pi;
      } else {
        return camWorldstate[(((player )* 5) + 6) + 2] + pi;
      }
    }
  }
  return 0.0;
}

/**
* Get timestamp from worldstate
*/
double GTCamWorldState::getTimestamp() const {
  if (!is_NAN(camWorldstate[0]))
  {
    return camWorldstate[0];
  }
  else
  {
    return 0.0;
  }
}

/**
* Set player position in worldstate
*/
void GTCamWorldState::setPlayerPos(Vector2<double> pos,int player,int color) {
  if (color == Player::blue) {
    player += 4;
  }
  camWorldstate[(((player )* 5) + 6) + 0] = pos.x;
  camWorldstate[(((player )* 5) + 6) + 1] = pos.y;
}

/**
* set player speed in worldstate
*/
void GTCamWorldState::setPlayerSpeed(Vector2<double> speed,int player,int color) {
  if (color == Player::blue) {
    player += 4;
  }
  camWorldstate[(((player )* 5) + 6) + 3] = speed.x;
  camWorldstate[(((player )* 5) + 6) + 4] = speed.y;
}

/**
* set player orientation in worldstate
*/
void GTCamWorldState::setPlayerOrientation(double angle, int player = 0,int color) {
  if (color == Player::blue) {
    player += 4;
  }
  camWorldstate[(((player )* 5) + 6) + 2] = angle;
}

/**
* set timestamp in worldstate
*/
void GTCamWorldState::setTimestamp(double stamp) {
  camWorldstate[0] = stamp;
}


/**
* Set Sync Timestamp
*/
void GTCamWorldState::setSyncTimestamp(double stamp) {
  camWorldstate[1] = stamp;
}


/**
* Return the sync timestamp of the worldstate
*/
double GTCamWorldState::getSyncTimestamp() const {
  if (!is_NAN(camWorldstate[1]))
  {
    return camWorldstate[1];
  }
  else
  {
    return 0.0;
  }
}

/**
* get ballModel from worldstate
*/
BallModel GTCamWorldState::getBallModel() const {
  BallModel ball;

  if (!is_NAN(camWorldstate[2]))
  {
    RobotPose poseDummy; 
    ball.seen.setBallDataRelativeToField(
      poseDummy,
      camWorldstate[2]*flip, camWorldstate[3]*flip,
      camWorldstate[4]*flip, camWorldstate[5]*flip);
    ball.ballWasSeen = true;
  }
  else
  {
    ball.ballWasSeen = false;
  }

  /*
  ball.seen.x = camWorldstate[2] * flip;
  ball.seen.y = camWorldstate[3]  * flip;
  ball.seen.speed.x = camWorldstate[4] * flip;
  ball.seen.speed.y = camWorldstate[5] * flip;
  */
//  OUTPUT(idText,text,"Flipstate: " << flip);
  //cout << "============================\n";
  //cout << "FlipState: " << flip << "\n";
  //cout << "Ball.x : " << ball.seen.x << "\n";
  //cout << "Ball.y : " << ball.seen.y << "\n";
  return ball;
}


/**
* Return robot pose of player
*/
RobotPose GTCamWorldState::getRobotPose(int player, int color) const 
{
  RobotPose pose;
  if (playerWasSeen(player, color))
  {
    pose.translation = getPlayerPos(player,color);
    pose.rotation = getPlayerOrientation(player,color);
    pose.setValidity(1.0);
  }
  else
  {
    pose.translation.x = 0.0;
    pose.translation.y = 0.0;
    pose.rotation = 0.0;
    pose.setValidity(0.0);
  }
  return pose;
}

RobotPose GTCamWorldState::getRobotPose() const
{
  return getRobotPose(getPlayer().getPlayerNumber(), getPlayer().getTeamColor());
}

/** Does not work for now */
PlayerPoseCollection GTCamWorldState::getPlayerPoseCollection(int player, int color) {
  PlayerPoseCollection c;
  PlayerPose p;

  return c;
}

/** 
* Set the ball position
*/
void GTCamWorldState::setBallPos(double X, double Y)
{
  camWorldstate[2] = X;
  camWorldstate[3] = X;
}

/** 
* Set the X ball speed
*/
void GTCamWorldState::setBallSpeedX(double speedX) {
  camWorldstate[4] = speedX;
}

/**
* Set the Y Ball speed
*/
void GTCamWorldState::setBallSpeedY(double speedY) {
  camWorldstate[5] = speedY;
}

/**
* Set the Ball speed
*/
void GTCamWorldState::setBallSpeed(double speedX, double speedY) {
  setBallSpeedX(speedX);
  setBallSpeedY(speedY);
}


unsigned long GTCamWorldState::getFrameNumber() const
{
  return frameNumber;
}

/**
* Was the player seen?
*/
bool GTCamWorldState::playerWasSeen(int player, int color) const
{
  if (color == Player::blue) {
    player += 4;
  }
/*  double test=(camWorldstate[player*5 + 6]);

  bool result1 = !is_NAN_d(test);
  bool result2 = !is_NAN_d(test);

  unsigned long* pTest;
  pTest = (unsigned long*) &(camWorldstate[player*5 + 6]);
  unsigned long nan[2]={0xffffffff, 0x7fffffff};

  bool result = !( ((pTest[0] - nan[0])==0) && ((pTest[1] - nan[1])==0) );

//#ifdef APERIOS1_3_2
  OUTPUT (idText, text, "NaNso: " << nan[0] << ", " << nan[1]);
  OUTPUT (idText, text, "NaNis: " << pTest[0] << ", " << pTest[1] << ", " << result1 << ", " << result2 << ", " << result);
//#endif
*/
  return !is_NAN(camWorldstate[player*5 + 6]);
}

/**
* Is test NaN?
*/
bool GTCamWorldState::is_NAN(double test) const
{
#ifdef _WIN32
  return (_isnan(test)!=0);
#else
  return (isnan(test)!=0);
#endif

/*  return (test!=test);
  unsigned long* pTest;
  pTest = (unsigned long*) &test;
  unsigned long nan[2]={0xffffffff, 0x7fffffff};

  bool result = ( ((pTest[0] - nan[0])==0) && ((pTest[1] - nan[1])==0) );
  return result;
//  return ( (pTest[0] == nan[0]) && (pTest[1] == nan[1]) );*/
}

const int GTCamWorldState::SIZEOFWORLDSTATE = 46;

/** 
* Do the streaming
*/
In& operator>>(In& stream,GTCamWorldState& state) {
  //  stream >> state.camWorldstate;
//  stream.read(&state.camWorldstate,sizeof(state.camWorldstate));
  stream.read(&state,sizeof(state));
  return stream;
}

Out& operator<<(Out& stream, const GTCamWorldState& state) {
  //  stream << state.camWorldstate;
//  stream.write(&state.camWorldstate,sizeof(state.camWorldstate));
  stream.write(&state,sizeof(state));
  return stream;
}

---