Modules/BehaviorControl/CommonXabsl2Symbols/GtCamSymbols.cpp

Go to the documentation of this file.

/**
* @file GtCamSymbols.cpp
* 
* @author Matthias Meyer
*/

#include "GtCamSymbols.h"



// Macro definitions
#define GETROBOTXimpl(number,pnumber,color) \
  double GTCamSymbols::getRobot##number##PosX() {\
  return gtCamWorldState.getPlayerPos(pnumber,color).x;\
}\
double GTCamSymbols::getRobot##number##PosY() {\
  return gtCamWorldState.getPlayerPos(pnumber,color).y;\
}\
Vector2<double> GTCamSymbols::getRobot##number##Pos() {\
  Vector2<double> v;\
  v.x = getRobot##number##PosX();\
  v.y = getRobot##number##PosY();\
  return v;\
}\
double GTCamSymbols::getRobot##number##SpeedX() {\
  return gtCamWorldState.getPlayerSpeed(pnumber,color).x;\
}\
double GTCamSymbols::getRobot##number##SpeedY() {\
  return gtCamWorldState.getPlayerSpeed(pnumber,color).y;\
}\
Vector2<double> GTCamSymbols::getRobot##number##Speed() {\
  Vector2<double> v;\
  v.x = getRobot##number##SpeedX();\
  v.y = getRobot##number##SpeedY();\
  return v;\
}\
double GTCamSymbols::getRobot##number##Rotation() {\
  return gtCamWorldState.getPlayerOrientation(pnumber,color);\
}\
double GTCamSymbols::getRobot##number##Distance() {\
  Vector2<double> v;\
  v.x = robotPose.translation.x - getRobot##number##PosX();\
  v.y = robotPose.translation.y - getRobot##number##PosY();\
  return v.abs();\
}\

#define REGISTERROBOTXimpl(number)\
  engine.registerDecimalInputSymbol("GTCam.Robot" #number ".x",this,\
    (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getRobot##number##PosX);\
  engine.registerDecimalInputSymbol("GTCam.Robot" #number ".y",this,\
    (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getRobot##number##PosY);\
  engine.registerDecimalInputSymbol("GTCam.Robot" #number ".speed.x",this,\
    (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getRobot##number##SpeedX);\
  engine.registerDecimalInputSymbol("GTCam.Robot" #number ".speed.y",this,\
    (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getRobot##number##SpeedY);\
  engine.registerDecimalInputSymbol("GTCam.Robot" #number ".rotation",this,\
    (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getRobot##number##Rotation);\
  engine.registerDecimalInputSymbol("GTCam.DistanceToRobot" #number ".abs",this,\
  (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getRobot##number##Distance );\





/**
* Constructor
*/
GTCamSymbols::GTCamSymbols(const BehaviorControlInterfaces& interfaces) : 
BehaviorControlInterfaces(interfaces)
{
}


/**
* Register XABSL2 Symbols
*/
void GTCamSymbols::registerSymbols(Xabsl2Engine& engine)
{
  // Register ball Symbols  
  engine.registerDecimalInputSymbol("GTCam.ball.x",this,
    (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getBallX);
  engine.registerDecimalInputSymbol("GTCam.ball.y",this,
    (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getBallY);
  engine.registerDecimalInputSymbol("GTCam.ball.speed.x",this,
    (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getBallSpeedX);
  engine.registerDecimalInputSymbol("GTCam.ball.speed.y",this,
    (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getBallSpeedY);

  // Register Robot 1-8 Symbols
  REGISTERROBOTXimpl(1)
  REGISTERROBOTXimpl(2)
  REGISTERROBOTXimpl(3)
  REGISTERROBOTXimpl(4)
  REGISTERROBOTXimpl(5)
  REGISTERROBOTXimpl(6)
  REGISTERROBOTXimpl(7)
  REGISTERROBOTXimpl(8)

  // Register own Symbols
  engine.registerDecimalInputSymbol("GTCam.Own.rotation",this,
  (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getOwnRotation);
  engine.registerDecimalInputSymbol("GTCam.Own.x",this,
  (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getOwnPosX);
  engine.registerDecimalInputSymbol("GTCam.Own.y",this,
  (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getOwnPosY);
  engine.registerDecimalInputSymbol("GTCam.Own.speed.x",this,
  (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getOwnSpeedX);
  engine.registerDecimalInputSymbol("GTCam.Own.speed.y",this,
  (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getOwnSpeedY);
  engine.registerDecimalInputSymbol("GTCam.Own.Distance.abs",this,
  (double (Xabsl2FunctionProvider::*)())&GTCamSymbols::getOwnDistance);
}

/**
* Do Update - not implemented yet
*/
void GTCamSymbols::update()
{ 
}


/**
* Implementations for Player 1 - 8
*/
GETROBOTXimpl(1,0,Player::red)
GETROBOTXimpl(2,1,Player::red)
GETROBOTXimpl(3,2,Player::red)
GETROBOTXimpl(4,3,Player::red)
GETROBOTXimpl(5,0,Player::blue)
GETROBOTXimpl(6,1,Player::blue)
GETROBOTXimpl(7,2,Player::blue)
GETROBOTXimpl(8,3,Player::blue)

/**
* Return Ball.x
*/
double GTCamSymbols::getBallX() {
  return gtCamWorldState.getBallModel().seen.positionField.x;
}

/**
* Return Ball.y
*/
double GTCamSymbols::getBallY() {
  return gtCamWorldState.getBallModel().seen.positionField.y;
}

/**
* Return BallSpeed.x
*/
double GTCamSymbols::getBallSpeedX() {
  return gtCamWorldState.getBallModel().seen.speedField.x;
}

/**
* Return BallSpeed.y
*/
double GTCamSymbols::getBallSpeedY() {
  return gtCamWorldState.getBallModel().seen.speedField.y;
}

/**
* Return a vector of BallSpeed.x and BallSpeed.y
*/
Vector2<double> GTCamSymbols::getBall() {
  Vector2<double> v;
  v.x = getBallX();
  v.y = getBallY();
  return v;
}


/**
* get Distance between cam and self-location
*/
double GTCamSymbols::getOwnDistance() {
  Vector2<double> v;
  v.x = robotPose.translation.x - getOwnPosX();
  v.y = robotPose.translation.y - getOwnPosY();
  return v.abs();
}

/**
* get own rotation from camstate
*/
double GTCamSymbols::getOwnRotation() {
  return gtCamWorldState.getPlayerOrientation(getPlayer().getPlayerNumber(),getPlayer().getTeamColor());
}

/**
* get own x coord from camstate
*/
double GTCamSymbols::getOwnPosX() {
  return gtCamWorldState.getPlayerPos(getPlayer().getPlayerNumber() ,getPlayer().getTeamColor()).x;
}

/**
* get own y coor from camstate
*/
double GTCamSymbols::getOwnPosY() {
  return gtCamWorldState.getPlayerPos(getPlayer().getPlayerNumber() ,getPlayer().getTeamColor()).y;
}

/**
* get own x speed from camstate
*/
double GTCamSymbols::getOwnSpeedX() {
  return gtCamWorldState.getPlayerSpeed(getPlayer().getPlayerNumber(),getPlayer().getTeamColor()).x;
}

/**
* get own y speed from camstate
*/
double GTCamSymbols::getOwnSpeedY() {
  return gtCamWorldState.getPlayerSpeed(getPlayer().getPlayerNumber(),getPlayer().getTeamColor()).y;
}

---