Modules/BehaviorControl/GT2005BehaviorControl/GT2005Symbols/GT2005ConfigurationSymbols.cpp

Go to the documentation of this file.

/** 
* @file GT2005ConfigurationSymbols.cpp
*
* Implementation of class GT2005ConfigurationSymbols.
*
* @author Max Risler
*/

#include "GT2005ConfigurationSymbols.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Debugging/Trace.h"

GT2005ConfigurationSymbols::GT2005ConfigurationSymbols(
                           BehaviorControlInterfaces& interfaces)
: BehaviorControlInterfaces(interfaces)
{
  //kickoff_role = BehaviorTeamMessage::goalie;
  switch (getPlayer().getPlayerNumber())
  {
    case Player::one:
      kickoff_role = BehaviorTeamMessage::goalie;
      break;
    case Player::two:
      kickoff_role = BehaviorTeamMessage::defensiveSupporter;
      break;
    case Player::three:
      kickoff_role = BehaviorTeamMessage::offensiveSupporter;
      break;
    case Player::four:
      kickoff_role = BehaviorTeamMessage::striker;
      break;
  }
  currentKickoff = 0;
    // start position for goalie (both kickoffs)
    goalie.x = xPosOwnGroundline + 100.0;
    goalie.y = 50;
    // start position for defensive supporter (both kickoffs)
    defensiveSupporter.x = xPosOwnGroundline / 2 - 100; 
    defensiveSupporter.y = -300.0;

    // start position for offensive supporter (opponent kickoffs)
    offensiveSupporterOpponentKickoff.x = xPosOwnGroundline / 2 - 100;
    offensiveSupporterOpponentKickoff.y = -1000.0;
    // start position for striker (opponent kickoffs)
    strikerOpponentKickoff.x = xPosOwnGroundline / 2 - 100;
    strikerOpponentKickoff.y = 600.0;

    // long kickoff to the right
    offensiveSupporterOwnKickoff[0].x = -200.0;
    offensiveSupporterOwnKickoff[0].y = -800.0;//-1100.0;
    offensiveSupporterOwnKickoffAngle[0] = 0.0;
    strikerOwnKickoff[0].x = -130.0;//-185.0;
    strikerOwnKickoff[0].y = 130.0;//0;
    strikerOwnKickoffAngle[0] = -45.0;//0;

    // long kickoff to the left
    offensiveSupporterOwnKickoff[1].x = -200.0;
    offensiveSupporterOwnKickoff[1].y = 800.0;//1100.0;
    offensiveSupporterOwnKickoffAngle[1] = 0.0;
    strikerOwnKickoff[1].x = -130.0;
    strikerOwnKickoff[1].y = -130.0;
    strikerOwnKickoffAngle[1] = 45.0;

}

void GT2005ConfigurationSymbols::registerSymbols(Xabsl2Engine& engine)
{
  // "kickoff role"
  engine.registerEnumeratedInputSymbol("kickoff.role", (int *)&kickoff_role);
  engine.registerEnumeratedInputSymbolEnumElement("kickoff.role", "kickoff.role.kickoff-goalie", BehaviorTeamMessage::goalie);
  engine.registerEnumeratedInputSymbolEnumElement("kickoff.role", "kickoff.role.kickoff-striker", BehaviorTeamMessage::striker);
  engine.registerEnumeratedInputSymbolEnumElement("kickoff.role", "kickoff.role.kickoff-defensive-supporter", BehaviorTeamMessage::defensiveSupporter);
  engine.registerEnumeratedInputSymbolEnumElement("kickoff.role", "kickoff.role.kickoff-offensive-supporter", BehaviorTeamMessage::offensiveSupporter);
  engine.registerDecimalInputSymbol("kickoff.rolenumber",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005ConfigurationSymbols::getKickOffRoleNumber);

  engine.registerDecimalInputSymbol("current-kickoff-variant",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005ConfigurationSymbols::getCurrentKickoff);

  engine.registerDecimalInputSymbol("own-kickoff.angle",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005ConfigurationSymbols::getOwnKickoffAngle);
  engine.registerDecimalInputSymbol("own-kickoff.x",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005ConfigurationSymbols::getOwnKickoffX);
  engine.registerDecimalInputSymbol("own-kickoff.y",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005ConfigurationSymbols::getOwnKickoffY);

  engine.registerDecimalInputSymbol("opponent-kickoff.angle",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005ConfigurationSymbols::getOpponentKickoffAngle);
  engine.registerDecimalInputSymbol("opponent-kickoff.x",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005ConfigurationSymbols::getOpponentKickoffX);
  engine.registerDecimalInputSymbol("opponent-kickoff.y",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005ConfigurationSymbols::getOpponentKickoffY);

  // "kickoff.x"
  engine.registerDecimalInputFunction("kickoff.x",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005ConfigurationSymbols::getKickoffX);
  engine.registerDecimalInputFunctionParameter("kickoff.x","kickoff.x.role",role);
  engine.registerDecimalInputFunctionParameter("kickoff.x","kickoff.x.ownkickoff",ownKickOff);
  // "kickoff.y"
  engine.registerDecimalInputFunction("kickoff.y",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005ConfigurationSymbols::getKickoffY);
  engine.registerDecimalInputFunctionParameter("kickoff.y","kickoff.y.role",role);
  engine.registerDecimalInputFunctionParameter("kickoff.y","kickoff.y.ownkickoff",ownKickOff);
  // "kickoff.angle"
  engine.registerDecimalInputFunction("kickoff.angle",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005ConfigurationSymbols::getKickoffAngle);
  engine.registerDecimalInputFunctionParameter("kickoff.angle","kickoff.angle.role",role);
  engine.registerDecimalInputFunctionParameter("kickoff.angle","kickoff.angle.ownkickoff",ownKickOff);
}

void GT2005ConfigurationSymbols::update()
{
  currentKickoff = gameControlData.getOpponentTeam().score % numOfKickoff;

  // calculate kickoff position of striker based on obstacle-model
  double radius = 250.0;

  double leftAbs = fromDegrees(10.0);
  double rightAbs = fromDegrees(55.0);
  double freeWidth = fromDegrees(15.0);

  double step = fromDegrees(5.0);
  double bestAngle = 0.0;
  double bestDist = 0.0;
  
  double start = leftAbs,
         end = rightAbs;
  enum { left, right } direction = left;

  if (currentKickoff % 2 == 0)
  {
    start *= -1;
    end *= -1;
    step *= -1;
    direction = right;
  }

  for(
    double ang = start;
    (direction == left && ang <= end) || (direction == right && ang >= end);
    ang += step
  )
  {
    double currDist = obstaclesModel.getDistanceInCorridor(ang, freeWidth);
    double angAbs = ang - robotPose.getAngle();
    currDist = obstaclesModel.getDistanceInDirection(angAbs, freeWidth);
    if(currDist > bestDist)
    {
      bestDist = currDist;
      bestAngle = ang;
    }
  }

  double posX = - cos ( bestAngle ) * radius;
  double posY = - sin ( bestAngle ) * radius;

  obstacleKickoff.translation.x = posX;
  obstacleKickoff.translation.y = posY;
  obstacleKickoff.fromAngle(toDegrees(bestAngle));
  computeKickoffRole();

  DEBUG_RESPONSE("KickOff:Role:output",OUTPUT(idText,text,"kickoffrole: "<<kickoff_role));
  DEBUG_RESPONSE("KickOff:Role:goalie",kickoff_role = BehaviorTeamMessage::goalie;return;);
  DEBUG_RESPONSE("KickOff:Role:defence",kickoff_role = BehaviorTeamMessage::defensiveSupporter;return;);
  DEBUG_RESPONSE("KickOff:Role:offence",kickoff_role = BehaviorTeamMessage::offensiveSupporter;return;);
  DEBUG_RESPONSE("KickOff:Role:striker",kickoff_role = BehaviorTeamMessage::striker;return;);
}

double GT2005ConfigurationSymbols::getCurrentKickoff()
{
  return currentKickoff;
}

double GT2005ConfigurationSymbols::getOwnKickoffAngle()
{
  switch (getPlayer().getPlayerNumber())
  {
    case Player::one: // robot number 1
    case Player::two: // robot number 2
    default:
      return 0;
    case Player::three: // robot number 3
      return offensiveSupporterOwnKickoffAngle[currentKickoff];
    case Player::four: // robot number 4
      return getOwnKickoffAngleByObstacles();
      //return strikerOwnKickoffAngle[currentKickoff];
  }
}

double GT2005ConfigurationSymbols::getOwnKickoffX()
{
  switch (getPlayer().getPlayerNumber())
  {
    case Player::one: // robot number 1
    default:
      return goalie.x;
    case Player::two: // robot number 2
      return defensiveSupporter.x;
    case Player::three: // robot number 3
      return offensiveSupporterOwnKickoff[currentKickoff].x;
    case Player::four: // robot number 4
      return getOwnKickoffXByObstacles();
      //return strikerOwnKickoff[currentKickoff].x;
  }
}

double GT2005ConfigurationSymbols::getOwnKickoffY()
{
  switch (getPlayer().getPlayerNumber())
  {
    case Player::one: // robot number 1
    default:
      return goalie.y;
    case Player::two: // robot number 2
      return defensiveSupporter.y;
    case Player::three: // robot number 3
      return offensiveSupporterOwnKickoff[currentKickoff].y;
    case Player::four: // robot number 4
      return getOwnKickoffYByObstacles();
      //return strikerOwnKickoff[currentKickoff].y;
  }
}

double GT2005ConfigurationSymbols::getOwnKickoffXByObstacles()
{
  return obstacleKickoff.translation.x;
}

double GT2005ConfigurationSymbols::getOwnKickoffYByObstacles()
{
  return obstacleKickoff.translation.y;
}

double GT2005ConfigurationSymbols::getOwnKickoffAngleByObstacles()
{
  return obstacleKickoff.getAngle();
}

double GT2005ConfigurationSymbols::getOpponentKickoffAngle()
{
  switch (getPlayer().getPlayerNumber())
  {
    case Player::one: // robot number 1
    case Player::two: // robot number 2
    case Player::three: // robot number 3
    case Player::four: // robot number 4
    default:
      return 0;
  }
}

double GT2005ConfigurationSymbols::getOpponentKickoffX()
{
  switch (getPlayer().getPlayerNumber())
  {
    case Player::one: // robot number 1
    default:
      return goalie.x;
    case Player::two: // robot number 2
      return defensiveSupporter.x;
    case Player::three: // robot number 3
      return offensiveSupporterOpponentKickoff.x;
    case Player::four: // robot number 4
      return strikerOpponentKickoff.x;
  }
} 

double GT2005ConfigurationSymbols::getOpponentKickoffY()
{
  switch (getPlayer().getPlayerNumber())
  {
    case Player::one: // robot number 1
    default:
      return goalie.y;
    case Player::two: // robot number 2
      return defensiveSupporter.y;
    case Player::three: // robot number 3
      return offensiveSupporterOpponentKickoff.y;
    case Player::four: // robot number 4
      return strikerOpponentKickoff.y;
  }
}

double GT2005ConfigurationSymbols::getKickoffX()
{
  if(ownKickOff)
  {
    switch ((int)role )
    {
      case BehaviorTeamMessage::goalie: // robot number 1
      default:
        return goalie.x;
      case BehaviorTeamMessage::defensiveSupporter: // robot number 2
        return defensiveSupporter.x;
      case BehaviorTeamMessage::offensiveSupporter: // robot number 3
        return offensiveSupporterOwnKickoff[currentKickoff].x;
      case BehaviorTeamMessage::striker: // robot number 4
        return getOwnKickoffXByObstacles();
        //return strikerOwnKickoff[currentKickoff].x;
    }
  }else{
    switch ((int)role )
    {
      case BehaviorTeamMessage::goalie: // robot number 1
      default:
        return goalie.x;
      case BehaviorTeamMessage::defensiveSupporter: // robot number 2
        return defensiveSupporter.x;
      case BehaviorTeamMessage::offensiveSupporter: // robot number 3
        return offensiveSupporterOpponentKickoff.x;
      case BehaviorTeamMessage::striker: // robot number 4
        return strikerOpponentKickoff.x;
    }
  }
  return 0;
}

double GT2005ConfigurationSymbols::getKickoffY()
{
  if(ownKickOff)
  {
    switch ((int)role)
    {
      case BehaviorTeamMessage::goalie: // robot number 1
      default:
        return goalie.y;
      case BehaviorTeamMessage::defensiveSupporter: // robot number 2
        return defensiveSupporter.y;
      case BehaviorTeamMessage::offensiveSupporter: // robot number 3
        return offensiveSupporterOwnKickoff[currentKickoff].y;
      case BehaviorTeamMessage::striker: // robot number 4
        return getOwnKickoffYByObstacles();
        //return strikerOwnKickoff[currentKickoff].y;
    }
  }else{
    switch ((int)role)
    {
      case BehaviorTeamMessage::goalie: // robot number 1
      default:
        return goalie.y;
      case BehaviorTeamMessage::defensiveSupporter: // robot number 2
        return defensiveSupporter.y;
      case BehaviorTeamMessage::offensiveSupporter: // robot number 3
        return offensiveSupporterOpponentKickoff.y;
      case BehaviorTeamMessage::striker: // robot number 4
        return strikerOpponentKickoff.y;
    }
  }
  return 0;
}


double GT2005ConfigurationSymbols::getKickoffAngle()
{
  if(ownKickOff)
  {
    switch ((int)role)
    {
      case BehaviorTeamMessage::goalie: // robot number 1
      case BehaviorTeamMessage::defensiveSupporter: // robot number 2
      default:
        return 0;
      case BehaviorTeamMessage::offensiveSupporter: // robot number 3
        return offensiveSupporterOwnKickoffAngle[currentKickoff];
      case BehaviorTeamMessage::striker: // robot number 4
        return getOwnKickoffAngleByObstacles();
        //return strikerOwnKickoffAngle[currentKickoff];
    }
  }else{
    return 0;
  }
  return 0;
}


double GT2005ConfigurationSymbols::getTimeToReachKickoffPose( BehaviorTeamMessage::DynamicRole role, Vector2<double> pos)
{
  //simple (only distance to target point)
  this->role = role;
  if (gameControlData.getKickoffTeam()==GameControlData::ownTeam)
    this->ownKickOff = 1;
  else    
    this->ownKickOff = 0;

  Vector2<double>dpos(getKickoffX(),getKickoffY());
    return (dpos-pos).abs();
}

void GT2005ConfigurationSymbols::computeKickoffRole()
{
  // switch only at ready state
  if (gameControlData.data.state != GameControlData::ready )
  {
    return;
  }

  Player::playerNumber playernumber;
  playernumber = getPlayer().getPlayerNumber();

  if (playernumber == Player::one)
  {
    kickoff_role = BehaviorTeamMessage::goalie;
    return;
  }
  
  bool wlan
  = (teamMessageCollection[0].isActual(5000) 
    && teamMessageCollection[1].isActual(5000)
    && teamMessageCollection[2].isActual(5000));

  DEBUG_RESPONSE("KickOff:WLan:output",OUTPUT(idText,text,"KickOffRole Wlan: "<<wlan));
  DEBUG_RESPONSE("KickOff:WLan:no",wlan =false;);
  DEBUG_RESPONSE("KickOff:WLan:yes",wlan =true;);

  //no wlan .
  if(!wlan)
  {
    switch (playernumber)
    {
      case Player::one:
        kickoff_role = BehaviorTeamMessage::goalie;
        break;
      case Player::two:
        kickoff_role = BehaviorTeamMessage::defensiveSupporter;
        break;
      case Player::three:
        kickoff_role = BehaviorTeamMessage::offensiveSupporter;
        break;
      case Player::four:
        kickoff_role = BehaviorTeamMessage::striker;
        break;
    }
    return;
  }
  //calculate times
  double time[Player::numOfPlayerNumbers -1/*goali*/][BehaviorTeamMessage::sizeofKickoff];// 2 player (3 teammate -1 goalie) 3 poses
  Vector2<double> pos = robotPose.translation;
  for(int j = 0; j < BehaviorTeamMessage::sizeofKickoff; ++j)
  {
    kickoffTimes[j].add( (int)(getTimeToReachKickoffPose( (BehaviorTeamMessage::DynamicRole)(j+1), pos)));
    time[0][j] = (double)kickoffTimes[j].getSum() / (double)kickoffTimes[j].getNumberOfEntries();
    outgoingBehaviorTeamMessage.estimatedTimeToReachKickoff[j] = time[0][j];
    //DEBUG_RESPONSE("KickOff:time:output:timetoreach",OUTPUT(idText,text,"time "<<j<<" to reach: "<< time[0][j]) );
  }
  int h = 1;
  for(int i = 0; i < 3; ++i)
  {
    int num = teamMessageCollection[i].playerNumberOfSender;
    //DEBUG_RESPONSE("KickOff:WLan:output:playernumber", 
    //  OUTPUT(idText,text,"playerNumberOfSender "<<num));
    if (num == 0) //calculation no goalie
      continue;
    if (num > 3) //playernumber undef
      continue;
    if (h>2)
    {
      OUTPUT(idText,text,"Error more than 3 player are not a goalie");
      OUTPUT(idText,text,"own playernumber: "<< playernumber);
      OUTPUT(idText,text,"actual message playernumber: "<< num);
      continue;
    }
    
    for(int j = 0; j < 3; ++j)
    {
      time[h][j] = teamMessageCollection[i].behaviorTeamMessage.estimatedTimeToReachKickoff[j];
      //DEBUG_RESPONSE("KickOff:time:output:timex", 
      //  OUTPUT(idText,text,"time ["<<h<<"]["<<j<<"]: "<<time[h][j]));
    }
    h++;
  }
  //calculate best permutation
  double best = 99999999999999999.0 ; 
  int bestper = -1;
  int per[3*2][3] = { {0,1,2},{0,2,1},{1,0,2},{1,2,0},{2,0,1},{2,1,0} };
  for(int i = 0; i < 3*2*1; ++i) //all perm.
  {
    double pervalue = time[0][per[i][0]];
    if (pervalue < time[1][per[i][1]])
      pervalue = time[1][per[i][1]];
    if (pervalue < time[2][per[i][2]])
      pervalue = time[2][per[i][2]];
    pervalue = 3*pervalue 
      + time[0][per[i][0]]
      + time[1][per[i][1]]
      + time[2][per[i][2]];//longes time*3 + all times
    pervalue = pervalue/6;
    if( pervalue < best)
    {
      best = pervalue;
      bestper = i;
    }
  }
   //select role
  kickoff_role = (BehaviorTeamMessage::DynamicRole)(per[bestper][0]+1);
  //DEBUG_RESPONSE("KickOff:Role:output",OUTPUT(idText,text,"kickoffrole: "<<kickoff_role));
  //DEBUG_RESPONSE("KickOff:time:output:bestper", OUTPUT(idText,text,"per: "<<bestper));
  //DEBUG_RESPONSE("KickOff:time:output:time0", OUTPUT(idText,text,"time0: "<<time[0][per[bestper][0]]));
  //DEBUG_RESPONSE("KickOff:time:output:time1", OUTPUT(idText,text,"time1: "<<time[1][per[bestper][1]]));
  //DEBUG_RESPONSE("KickOff:time:output:time2",OUTPUT(idText,text,"time2: "<<time[2][per[bestper][2]]) );
 
  //calculate times
  /* old works only in simulator
  double time[3][3];// 2 player (3 teammate -1 goalie) 3 poses
  Vector2<double> pos = robotPose.translation;
  int num = getPlayer().getPlayerNumber()-1; //playernumber -1 
  DEBUG_RESPONSE("KickOff:ownplayernumber",OUTPUT(idText,text,"playernumber: "<< num) );
  for(int j = 0; j < 3; ++j)
  {
    time[num][j] = getTimeToReachKickoffPose( (BehaviorTeamMessage::DynamicRole)(j+1), pos);
    DEBUG_RESPONSE("KickOff:time:output:timetoreach",OUTPUT(idText,text,"time "<<j<<" to reach: "<< time[num][j]) );
  }
  int counter = 0;
  for(int i = 0; i < 3; ++i)
  {
    int num = teamMessageCollection[i].playerNumberOfSender;
    DEBUG_RESPONSE("KickOff:WLan:output:playernumber", 
      OUTPUT(idText,text,"playerNumberOfSender "<<num));
    if (num == 0) //calculation no goalie
      continue;
    if (num > 3) //calculation no goalie
      continue;
    num--;
    DEBUG_RESPONSE("KickOff:WLan:output:playernumber", 
      OUTPUT(idText,text,"playerNumberOfSender2 "<<num));
    DEBUG_RESPONSE("KickOff:WLan:output:playernumbercounter",
      if(counter>2)
      {
      OUTPUT(idText,text,"error "<<counter)
      OUTPUT(idText,text,"error "<<counter)
      OUTPUT(idText,text,"error "<<counter)
      OUTPUT(idText,text,"error "<<counter)
      OUTPUT(idText,text,"error "<<counter)
      OUTPUT(idText,text,"error "<<counter)
      });
    Vector2<double> pos = teamMessageCollection[i].robotPose.translation;
    
    for(int j = 0; j < 3; ++j)
    {
      time[i][j] = getTimeToReachKickoffPose( (BehaviorTeamMessage::DynamicRole)(j+1), pos);
      DEBUG_RESPONSE("KickOff:time:output:timex", 
        OUTPUT(idText,text,"time ["<<i<<"]["<<j<<"]: "<<time[i][j]));
    }
  }
  //calculate best permutation
  double best = 99999999999999999 ; 
  int bestper = -1;
  int per[3*2][3] = { {0,1,2},{0,2,1},{1,0,2},{1,2,0},{2,0,1},{2,1,0} };
  for(int i = 0; i < 3*2*1; ++i) //all perm.
  {
    double pervalue = time[0][per[i][0]];
    if (pervalue < time[1][per[i][1]])
      pervalue = time[1][per[i][1]];
    if (pervalue < time[2][per[i][2]])
      pervalue = time[2][per[i][2]];
    pervalue = 3*pervalue 
      + time[0][per[i][0]]
      + time[1][per[i][1]]
      + time[2][per[i][2]];//longes time*3 + all times
    pervalue = pervalue/6;
    if( pervalue < best)
    {
      best = pervalue;
      bestper = i;
    }
  }
  //select role
  num = getPlayer().getPlayerNumber()-1; //playernumber -1 
  DEBUG_RESPONSE("KickOff:ownplayernumber",OUTPUT(idText,text,"playernumber: "<< num) );
  kickoff_role = (BehaviorTeamMessage::DynamicRole)(per[bestper][num]+1);
  DEBUG_RESPONSE("KickOff:Role:output",OUTPUT(idText,text,"kickoffrole: "<<kickoff_role));
  DEBUG_RESPONSE("KickOff:time:output:bestper", OUTPUT(idText,text,"per: "<<bestper));
  DEBUG_RESPONSE("KickOff:time:output:time0", OUTPUT(idText,text,"time0: "<<time[0][per[bestper][0]]));
  DEBUG_RESPONSE("KickOff:time:output:time1", OUTPUT(idText,text,"time1: "<<time[1][per[bestper][1]]));
  DEBUG_RESPONSE("KickOff:time:output:time2",OUTPUT(idText,text,"time2: "<<time[2][per[bestper][2]]) );
*/

}

double GT2005ConfigurationSymbols::getKickOffRoleNumber()
{
  return kickoff_role;
}

---