Modules/BehaviorControl/GT2004BehaviorControl/GT2004Symbols/GT2004StrategySymbols.cpp

Go to the documentation of this file.

/** 
* @file GT2004StrategySymbols.cpp
*
* Implementation of class GT2004StrategySymbols.
*
* @author Martin Lötzsch
*/

#include "GT2004StrategySymbols.h"
#include "Tools/FieldDimensions.h"


GT2004StrategySymbols::GT2004StrategySymbols(BehaviorControlInterfaces& interfaces)
: BehaviorControlInterfaces(interfaces),
doProfile(GT2004StrategySymbols::dontDoProfiling), writeProfile(GT2004StrategySymbols::dontWriteProfiles)
{
  role = BehaviorTeamMessage::goalie;
  estimatedTimeToReachBall = 0.0;
}

void GT2004StrategySymbols::registerSymbols(Xabsl2Engine& engine)
{
  // "robot-number"
  engine.registerDecimalInputSymbol("robot-number", this,
    (double(Xabsl2FunctionProvider::*)())&GT2004StrategySymbols::getRobotNumber);
  
  // "role"
  engine.registerEnumeratedInputSymbol("role", (int *)&role);
  engine.registerEnumeratedInputSymbolEnumElement("role", "goalie", BehaviorTeamMessage::goalie);
  engine.registerEnumeratedInputSymbolEnumElement("role", "striker", BehaviorTeamMessage::striker);
  engine.registerEnumeratedInputSymbolEnumElement("role", "defensive-supporter", BehaviorTeamMessage::defensiveSupporter);
  engine.registerEnumeratedInputSymbolEnumElement("role", "offensive-supporter", BehaviorTeamMessage::offensiveSupporter);
  
  // "estimated-time-to-reach-ball"
  engine.registerDecimalInputSymbol("estimated-time-to-reach-ball", (double *)&estimatedTimeToReachBall);
  
  // "sent-game-state"
  engine.registerEnumeratedOutputSymbol("sent-game-state",(int*)&outgoingBehaviorTeamMessage.gameState);
  engine.registerEnumeratedOutputSymbolEnumElement("sent-game-state","sent-game-state.sleep",BehaviorTeamMessage::sleep);
  engine.registerEnumeratedOutputSymbolEnumElement("sent-game-state","sent-game-state.initial",BehaviorTeamMessage::initial);
  engine.registerEnumeratedOutputSymbolEnumElement("sent-game-state","sent-game-state.ready",BehaviorTeamMessage::ready);
  engine.registerEnumeratedOutputSymbolEnumElement("sent-game-state","sent-game-state.playing",BehaviorTeamMessage::playing);
  engine.registerEnumeratedOutputSymbolEnumElement("sent-game-state","sent-game-state.penalized",BehaviorTeamMessage::penalized);
  engine.registerEnumeratedOutputSymbolEnumElement("sent-game-state","sent-game-state.finished",BehaviorTeamMessage::finished);
  engine.registerEnumeratedOutputSymbolEnumElement("sent-game-state","sent-game-state.set",BehaviorTeamMessage::set);
  
  
  // "another-player-is-in-ready-state"
  engine.registerBooleanInputSymbol("another-player-is-in-ready-state",
    this,(bool(Xabsl2FunctionProvider::*)())&GT2004StrategySymbols::getAnotherPlayerIsInReadyState);
  
  // "another-player-is-in-initial-state"
  engine.registerBooleanInputSymbol("another-player-is-in-initial-state",
    this,(bool(Xabsl2FunctionProvider::*)())&GT2004StrategySymbols::getAnotherPlayerIsInInitialState);
  
  // "team-message"
  engine.registerEnumeratedOutputSymbol("team-message",(int*)&outgoingBehaviorTeamMessage.message);
  engine.registerEnumeratedOutputSymbolEnumElement("team-message","team-message.none", BehaviorTeamMessage::none);
  engine.registerEnumeratedOutputSymbolEnumElement("team-message","team-message.performing-a-kick",BehaviorTeamMessage::performingAKick);
  
  // "another-teammate-is-performing-a-kick"
  engine.registerBooleanInputSymbol("another-teammate-is-performing-a-kick",
    this,(bool(Xabsl2FunctionProvider::*)())&GT2004StrategySymbols::getAnotherTeammateIsPerformingAKick);
  
  // "the-striker-is-playing-near-the-opponent-goal"
  engine.registerBooleanInputSymbol("the-striker-is-playing-near-the-opponent-goal",
    this,(bool(Xabsl2FunctionProvider::*)())&GT2004StrategySymbols::getTheStrikerIsPlayingNearTheOpponentGoal);
  
  // "the-striker-is-not-playing-near-the-opponent-goal"
  engine.registerBooleanInputSymbol("the-striker-is-not-playing-near-the-opponent-goal",
    this,(bool(Xabsl2FunctionProvider::*)())&GT2004StrategySymbols::getTheStrikerIsNotPlayingNearTheOpponentGoal);
  
  // "the-striker-is-playing-near-the-own-goal"
  engine.registerBooleanInputSymbol("the-striker-is-playing-near-the-own-goal",
    this,(bool(Xabsl2FunctionProvider::*)())&GT2004StrategySymbols::getTheStrikerIsPlayingNearTheOwnGoal);
  
  // "the-striker-is-not-playing-near-the-own-goal"
  engine.registerBooleanInputSymbol("the-striker-is-not-playing-near-the-own-goal",
    this,(bool(Xabsl2FunctionProvider::*)())&GT2004StrategySymbols::getTheStrikerIsNotPlayingNearTheOwnGoal);

  // "goalie-max-position-speed"
  engine.registerDecimalInputSymbol("goalie-max-position-speed", this,
    (double (Xabsl2FunctionProvider::*)())&GT2004StrategySymbols::getGoalieMaxPositionSpeed);
    
  engine.registerDecimalInputSymbol("search-ball.x", this,
    (double (Xabsl2FunctionProvider::*)())&GT2004StrategySymbols::getSearchBallX);
  
  // "get-angle-to-teammate
  engine.registerDecimalInputFunction("get-angle-to-teammate",this,
    (double (Xabsl2FunctionProvider::*)())&GT2004StrategySymbols::getAngleToTeammate);
  engine.registerDecimalInputFunctionParameter("get-angle-to-teammate","get-angle-to-teammate.index",GT2004StrategySymbols::angleToTeammateIndex);
}

void GT2004StrategySymbols::update()
{
  estimateTimeToReachBall();
  computeRole();
  
  outgoingBehaviorTeamMessage.estimatedTimeToReachBall = estimatedTimeToReachBall;
  outgoingBehaviorTeamMessage.dynamicRole = role;
}

void GT2004StrategySymbols::computeRole()
{
  int i;
  
  if (getPlayer().getPlayerNumber() == Player::one)
  {
    // no role changes for the goalie
    role = BehaviorTeamMessage::goalie;
    return;
  }

  
  // if in the goalzone and ball lies within angle infront, role = striker
  if(FieldDimensions::distanceToOpponentPenaltyArea(robotPose.translation) < 400
    && SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) < 1000)
  {
    if(ballModel.seen.ballInFrontOfOpponentGoal)
    {
        role = BehaviorTeamMessage::striker;
        return;
    }
  }

  // fall-back if no wlan
  if (SystemCall::getTimeSince(teamMessageCollection[0].lastReceivedTimeStamp) > 5000 
    && SystemCall::getTimeSince(teamMessageCollection[1].lastReceivedTimeStamp) > 5000
    && SystemCall::getTimeSince(teamMessageCollection[2].lastReceivedTimeStamp) > 5000)
  {
    switch(getPlayer().getPlayerNumber())
    {
    case Player::two:
      if (SystemCall::getTimeSince (ballModel.seen.timeWhenLastSeen) > 5000)
        role = BehaviorTeamMessage::defensiveSupporter;
      else if(role == BehaviorTeamMessage::defensiveSupporter)
      { 
        if (ballModel.seen.x < 400)
          role = BehaviorTeamMessage::striker;
        else
          role = BehaviorTeamMessage::defensiveSupporter;
      } else {
        if (ballModel.seen.x < 600)
          role = BehaviorTeamMessage::striker;
        else
          role = BehaviorTeamMessage::defensiveSupporter;
      }
      break;
    case Player::three:
    case Player::four:
      if (SystemCall::getTimeSince (ballModel.seen.timeWhenLastSeen) > 5000)
        role = BehaviorTeamMessage::offensiveSupporter;
      else if(role == BehaviorTeamMessage::offensiveSupporter)
      { 
        if (ballModel.seen.x > -400)
          role = BehaviorTeamMessage::striker;
        else
          role = BehaviorTeamMessage::offensiveSupporter;
      } else {
        if (ballModel.seen.x > -600)
          role = BehaviorTeamMessage::striker;
        else
          role = BehaviorTeamMessage::offensiveSupporter;
      }
      break;
    }
    return;
  }
  
  // with wlan
  
  // estimate the closest other teammate to the ball
  double minTeammateTime = 100000.0;
  
  for (i = 0; i < teamMessageCollection.numberOfTeamMessages; i++ ) 
  {
    if ( teamMessageCollection[i].isActual() ) // only new messages
    {
      if ( teamMessageCollection[i].behaviorTeamMessage.dynamicRole != BehaviorTeamMessage::goalie 
        && teamMessageCollection[i].behaviorTeamMessage.gameState == BehaviorTeamMessage::playing)
        // the robot is not interested in 
        // (1) how fast the goalie can approach the ball 
        // (2) how fast a penalized field player can approach the ball
      {
        double time = 
          teamMessageCollection[i].behaviorTeamMessage.estimatedTimeToReachBall;
        
        // bonus for current striker
        if ( teamMessageCollection[i].behaviorTeamMessage.dynamicRole == BehaviorTeamMessage::striker ) 
          time -= 500.0;
        
        if (time < minTeammateTime ) 
        {
          minTeammateTime = time;
        }
      }
    }
  }
  
  // bonus for current striker
  if (role == BehaviorTeamMessage::striker)
    minTeammateTime += 1000.0;
  
  // assign striker-role if nearest to the ball
  if ( estimatedTimeToReachBall < minTeammateTime )
  {
    role = BehaviorTeamMessage::striker;
    return;
  }
  
  // assign supporting roles
  double maxTeammateX = (double)xPosOwnGroundline;
  for (i = 0; i < teamMessageCollection.numberOfTeamMessages; i++ ) 
  {
    if ( teamMessageCollection[i].isActual() )
    {
      if ( teamMessageCollection[i].behaviorTeamMessage.dynamicRole != BehaviorTeamMessage::striker 
        && teamMessageCollection[i].behaviorTeamMessage.dynamicRole != BehaviorTeamMessage::goalie 
        && teamMessageCollection[i].behaviorTeamMessage.gameState == BehaviorTeamMessage::playing)
      {
        double teammateX = 
          teamMessageCollection[i].robotPose.translation.x;
        
        // bonus for current offensive supporter
        if ( teamMessageCollection[i].behaviorTeamMessage.dynamicRole == BehaviorTeamMessage::offensiveSupporter ) 
          teammateX += 300.0;
        
        if (teammateX > maxTeammateX ) 
        {
          maxTeammateX = teammateX;
        }
      }
    }
  }
  
  // bonus for current offensive supporter
  if ( role == BehaviorTeamMessage::offensiveSupporter ) 
    maxTeammateX -= 300.0;
  
  if ( robotPose.translation.x >= maxTeammateX ) 
    role = BehaviorTeamMessage::offensiveSupporter;
  else 
    role = BehaviorTeamMessage::defensiveSupporter;
   
}

double GT2004StrategySymbols::getGoalieMaxPositionSpeed()
{
  double maxSpeed = 150;
  double minDistance = 300; 

  double minSpeed = 60;
  double maxDistance = 1500;

  double ballDistance = Geometry::distanceTo(robotPose.getPose(),ballModel.seen);

  double speed = maxSpeed - ((ballDistance - minDistance) * maxSpeed) / (maxDistance ) ;

  speed = max(minSpeed, speed);
  speed = min(maxSpeed, speed);

  return speed;
}

void GT2004StrategySymbols::estimateTimeToReachBall()
{
  
  // account for distance to ball
  estimatedTimeToReachBall = Geometry::distanceTo(robotPose.translation, ballModel.seen) / 0.2;
  
  // account if the robot is between the ball and the opponent goal
  
  // the position of the robot
  Vector2<double> robotPosition = robotPose.translation;
  double angleToLeftOpponentGoalPost = Geometry::angleTo(robotPosition, Vector2<double>(xPosOpponentGroundline,yPosLeftGoal));
  double angleToRightOpponentGoalPost = Geometry::angleTo(robotPosition, Vector2<double>(xPosOpponentGroundline,yPosRightGoal));
  if(angleToLeftOpponentGoalPost < angleToRightOpponentGoalPost)
  {
    angleToLeftOpponentGoalPost += pi2;
  }
  double angleToOpponentGoal= (angleToLeftOpponentGoalPost + angleToRightOpponentGoalPost) / 2.0;
  double absoluteAngleToBall = Geometry::angleTo(robotPosition,ballModel.seen);
  double angleBetweenBallAndOpponentGoal = normalize(angleToOpponentGoal - absoluteAngleToBall);
  
  estimatedTimeToReachBall += 400.0 * fabs(angleBetweenBallAndOpponentGoal);
  
  // longer if ball not seen
  estimatedTimeToReachBall += 2.0 * SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen);
  
  // test for obstacles
  /*
  if ( isOpponentBetweenRobotAndBall(robotPose) || 
  isOwnBetweenRobotAndBall (robotPose) )
  time += 5000.0;
  */
  
}

double GT2004StrategySymbols::getAngleToTeammate()
{
  
  //OUTPUT(idText, text,"::nrofownplayer:"<<playerPoseCollection.numberOfOwnPlayers);
  
  
  if(1 <= playerPoseCollection.numberOfOwnPlayers)
  {
    return toDegrees(Geometry::angleTo(robotPose, playerPoseCollection.getOwnPlayerPose((int)angleToTeammateIndex).getPose().translation));
  }
  else
  {
    return toDegrees(Geometry::angleTo(robotPose,
      Vector2<double>(xPosOpponentGroundline,yPosCenterGoal)));
  }
}

bool GT2004StrategySymbols::getAnotherPlayerIsInReadyState()
{
  for (int i=0; i<3;i++)
  {
    if (teamMessageCollection[i].isActual())
    {
      if (teamMessageCollection[i].behaviorTeamMessage.gameState 
        == BehaviorTeamMessage::ready)
      {
        return true;
      }
    }
  }
  return false;
}

bool GT2004StrategySymbols::getAnotherPlayerIsInInitialState()
{
  for (int i=0; i<3;i++)
  {
    if (teamMessageCollection[i].isActual())
    {
      if (teamMessageCollection[i].behaviorTeamMessage.gameState 
        == BehaviorTeamMessage::initial)
      {
        return true;
      }
    }
  }
  return false;
}

bool GT2004StrategySymbols::getAnotherTeammateIsPerformingAKick()
{
  for (int i=0; i<3;i++)
  {
    if (teamMessageCollection[i].isActual())
    {
      if (teamMessageCollection[i].behaviorTeamMessage.message
        == BehaviorTeamMessage::performingAKick)
      {
        return true;
      }
    }
  }
  return false;
}

bool GT2004StrategySymbols::getTheStrikerIsPlayingNearTheOpponentGoal()
{
  for (int i=0; i<3;i++)
  {
    if (teamMessageCollection[i].isActual())
    {
      if (teamMessageCollection[i].behaviorTeamMessage.dynamicRole == BehaviorTeamMessage::striker
        && teamMessageCollection[i].robotPose.translation.x > 1500)
      {
        return true;
      }
    }
  }
  return false;
}

bool GT2004StrategySymbols::getTheStrikerIsNotPlayingNearTheOpponentGoal()
{
  for (int i=0; i<3;i++)
  {
    if (teamMessageCollection[i].isActual())
    {
      if (teamMessageCollection[i].behaviorTeamMessage.dynamicRole == BehaviorTeamMessage::striker
        && teamMessageCollection[i].robotPose.translation.x < 1300)
      {
        return true;
      }
    }
  }
  return false;
}

bool GT2004StrategySymbols::getTheStrikerIsPlayingNearTheOwnGoal()
{
  for (int i=0; i<3;i++)
  {
    if (teamMessageCollection[i].isActual())
    {
      if (teamMessageCollection[i].behaviorTeamMessage.dynamicRole == BehaviorTeamMessage::striker
        && teamMessageCollection[i].robotPose.translation.x < -1200)
      {
        return true;
      }
    }
  }
  return false;
}

bool GT2004StrategySymbols::getTheStrikerIsNotPlayingNearTheOwnGoal()
{
  for (int i=0; i<3;i++)
  {
    if (teamMessageCollection[i].isActual())
    {
      if (teamMessageCollection[i].behaviorTeamMessage.dynamicRole == BehaviorTeamMessage::striker
        && teamMessageCollection[i].robotPose.translation.x > -1000)
      {
        return true;
      }
    }
  }
  return false;
}

double GT2004StrategySymbols::getSearchBallX()
{
  // fall-back if no wlan
  if (SystemCall::getTimeSince(teamMessageCollection[0].lastReceivedTimeStamp) > 5000 
    && SystemCall::getTimeSince(teamMessageCollection[1].lastReceivedTimeStamp) > 5000
    && SystemCall::getTimeSince(teamMessageCollection[2].lastReceivedTimeStamp) > 5000)
  {
    switch(getPlayer().getPlayerNumber())
    {
    case Player::two:
      return -1000;
    case Player::three:
      return 0;
    case Player::four:
    default:
      return 1000;
    }
  }
  else
  {
    switch (role)
    {
    case BehaviorTeamMessage::defensiveSupporter:
      return -1000;
    case BehaviorTeamMessage::offensiveSupporter:
      return 1000;
    case BehaviorTeamMessage::striker:
    default:
      return 0;
    }
  }
}



/*
* Change Log
* 
* $Log: GT2004StrategySymbols.cpp,v $
* Revision 1.8  2004/06/23 18:18:47  spranger
* reintroduced ballModel.seen.ballInFrontOfOpponentGoal
*
* Revision 1.7  2004/06/18 11:11:26  risler
* kick team messages clean up
* intercept behavior improved
*
* Revision 1.6  2004/06/16 14:43:51  risler
* added obstacles symbols
* added symbol obstacles.opponent-close-to-ball
*
* Revision 1.5  2004/06/14 15:54:07  risler
* improved no wavelan fallback role assignment, added hysteresis for supporter assignment
*
* Revision 1.4  2004/05/26 18:56:41  loetzsch
* clean up in the behavior control interfaces
*
* Revision 1.3  2004/05/23 19:00:17  spranger
* removed profiler-symbols
*
* Revision 1.2  2004/05/22 20:43:55  juengel
* Renamed ballP_osition to ballModel.
*
* Revision 1.1.1.1  2004/05/22 17:18:04  cvsadm
* created new repository GT2004_WM
*
* Revision 1.2  2004/05/03 14:33:20  loetzsch
* some cleanup
*
* Revision 1.1  2004/05/02 13:23:45  juengel
* Added GT2004BehaviorControl.
*
*/

---