Modules/BehaviorControl/CommonXabsl2Symbols/AngleSymbols.cpp

Go to the documentation of this file.

/** 
* @file AngleSymbols.cpp
*
* Implementation of class AngleSymbols.
*
* @author Matthias Jüngel
*/

#include "AngleSymbols.h"
#include "Platform/SystemCall.h"
#include "Tools/Math/Geometry.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Debugging/DebugDrawings.h"
#include "Tools/StringFunctions.h"

AngleSymbols::AngleSymbols(const BehaviorControlInterfaces& interfaces)
: BehaviorControlInterfaces(interfaces)
{
  angleShownByLEDs = undefined;
  angles[undefined] = 0;
}


void AngleSymbols::registerSymbols(Xabsl2Engine& engine)
{
  // localisation based:
  engine.registerDecimalInputSymbol("angle.angle-to-center-of-field",this,
    (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToCenterOfField);

  engine.registerDecimalInputSymbol("angle.angle-to-opponent-goal",this,
    (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToOpponentGoal);

  engine.registerDecimalInputSymbol("angle.angle-to-point-behind-opponent-goal",this,
    (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToPointBehindOpponentGoal);
  
  // vision based
  // ...

  // combined: vision + localisation based
  
  
  engine.registerDecimalInputSymbol("angle.best-angle-to-opponent-goal",this,
    (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestAngleToOpponentGoal);

  engine.registerDecimalInputSymbol("angle.best-angle-to-opponent-goal-no-obstacles",this,
    (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestAngleToOpponentGoalNoObstacles);
  
  engine.registerDecimalInputSymbol("angle.best-angle-away-from-own-goal",this,
    (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestAngleAwayFromOwnGoal);
  
  engine.registerDecimalInputSymbol("angle.best-angle-away-from-own-goal-no-obstacles",this,
    (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestAngleAwayFromOwnGoalNoObstacles);
  
  engine.registerDecimalInputSymbol("angle.goal-kick-angle",this,
    (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getGoalieGoalKickAngle);


  engine.registerEnumeratedOutputSymbol("angle.angle-shown-by-leds",(int*)&angleShownByLEDs);

  int i;
  char s[256];
  for (i = 0; i < numberOfAngles; i++)
  {
    sprintf(s,"angle.");
    getXmlString(s+strlen(s),getAngleName((Angles)i));
    engine.registerEnumeratedOutputSymbolEnumElement("angle.angle-shown-by-leds",s,i);
  }
}

void AngleSymbols::update()
{
  calculateLocalisationBasedAngles();
  calculateVisionBasedAngles();
  calculateCombinedAngles();
  drawAngleShownByLeds();
}

// localisation based
double AngleSymbols::getAngleToCenterOfField()           {return toDegrees(normalize(angles[angleToCenterOfField])); }
double AngleSymbols::getAngleToOpponentGoal()            {return toDegrees(normalize(angles[angleToOpponentGoal])); }
double AngleSymbols::getAngleToPointBehindOpponentGoal() {return toDegrees(normalize(angles[angleToPointBehindOpponentGoal])); }

// combined: vision + localisation
double AngleSymbols::getBestAngleToOpponentGoal()            {return toDegrees(normalize(angles[bestAngleToOpponentGoal])); }
double AngleSymbols::getBestAngleToOpponentGoalNoObstacles() {return toDegrees(normalize(angles[bestAngleToOpponentGoalNoObstacles])); }
double AngleSymbols::getBestAngleAwayFromOwnGoal()           {return toDegrees(normalize(angles[bestAngleAwayFromOwnGoal])); }
double AngleSymbols::getBestAngleAwayFromOwnGoalNoObstacles(){return toDegrees(normalize(angles[bestAngleAwayFromOwnGoalNoObstacles])); }
double AngleSymbols::getGoalieGoalKickAngle()                {return toDegrees(normalize(angles[goalieGoalKickAngle])); }

void AngleSymbols::calculateLocalisationBasedAngles()
{
  angles[angleToCenterOfField]     = Geometry::angleTo(robotPose, Vector2<double>(0.0,0.0));

  angles[angleToLeftOpponentGoalPost]  = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGroundline,yPosLeftGoal));
  angles[angleToRightOpponentGoalPost] = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGroundline,yPosRightGoal));
  if(angles[angleToLeftOpponentGoalPost] < angles[angleToRightOpponentGoalPost])
  {
    angles[angleToLeftOpponentGoalPost] += pi2;
  }
  angles[angleToOpponentGoal] = (angles[angleToLeftOpponentGoalPost] + angles[angleToRightOpponentGoalPost]) / 2.0;

  angles[angleToLeftOpponentGoalCorner] = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGoal,yPosLeftGoal));
  angles[angleToRightOpponentGoalCorner] = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGoal,yPosRightGoal));
  if(angles[angleToLeftOpponentGoalCorner] < angles[angleToRightOpponentGoalCorner])
  {
    angles[angleToLeftOpponentGoalCorner] += pi2;
  }
  angles[angleToPointBehindOpponentGoal] = (angles[angleToLeftOpponentGoalCorner] + angles[angleToRightOpponentGoalCorner]) / 2.0;
}

void AngleSymbols::calculateVisionBasedAngles()
{
  angles[angleToFreePartOfOpponentGoal] = obstaclesModel.angleToFreePartOfGoal[ObstaclesModel::opponentGoal];
}

void AngleSymbols::calculateCombinedAngles()
{
  //best angle to opponent goal ///////////
  if(obstaclesModel.angleToFreePartOfGoalWasDetermined[ObstaclesModel::opponentGoal])
  {
    angles[bestAngleToOpponentGoalNoObstacles] = angles[angleToFreePartOfOpponentGoal];
  }
  else
  {
    angles[bestAngleToOpponentGoalNoObstacles] = angles[angleToOpponentGoal];
  }
  
  if(FieldDimensions::distanceToOpponentPenaltyArea(robotPose.translation) > 300)
  {
    int distanceToNextTeammate = 10000;
    double angleToNextTeammate = 0;
    bool foundTeammate = false;

    for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++)
    {
      PlayerPose currentPose = playerPoseCollection.getOwnPlayerPose(i);
      int currentDistance = (int)((currentPose.getPose().translation - robotPose.translation).abs());
      double currentAngle = normalize(Geometry::angleTo(robotPose, currentPose.getPose().translation));

      if(currentDistance < distanceToNextTeammate)
      {
        foundTeammate = true;
        angleToNextTeammate = currentAngle;
        distanceToNextTeammate = currentDistance;
      }
    }

    
    double leftAngleToOpponentGoal = obstaclesModel.getAngleOfNextFreeSectorLeft(pi/8.0, angles[bestAngleToOpponentGoalNoObstacles], 600);
    double rightAngleToOpponentGoal = obstaclesModel.getAngleOfNextFreeSectorRight(pi/8.0, angles[bestAngleToOpponentGoalNoObstacles], 600);
    
    if(foundTeammate && 
      fabs(
      fabs(normalize(angles[bestAngleToOpponentGoalNoObstacles] - leftAngleToOpponentGoal)) -
      fabs(normalize(angles[bestAngleToOpponentGoalNoObstacles] - rightAngleToOpponentGoal))
      )
      < fromDegrees(20) )
    {
      if(angleToNextTeammate > 0) angles[bestAngleToOpponentGoal] = leftAngleToOpponentGoal;
      else angles[bestAngleToOpponentGoal] = rightAngleToOpponentGoal;
    }
    else
    {
      if(
        fabs(normalize(angles[bestAngleToOpponentGoalNoObstacles] - leftAngleToOpponentGoal)) <
        fabs(normalize(angles[bestAngleToOpponentGoalNoObstacles] - rightAngleToOpponentGoal))
        ) angles[bestAngleToOpponentGoal] = leftAngleToOpponentGoal;
      else angles[bestAngleToOpponentGoal] = rightAngleToOpponentGoal;
    }

    if(fabs(angles[bestAngleToOpponentGoal] - angles[bestAngleToOpponentGoalNoObstacles]) > fromDegrees(70))
    {
      angles[bestAngleToOpponentGoal] = angles[bestAngleToOpponentGoalNoObstacles];
    }
  }
  else
  {
    angles[bestAngleToOpponentGoal] = angles[bestAngleToOpponentGoalNoObstacles];
  }

  //best angle away from own goal ///////////
  if(obstaclesModel.angleToFreePartOfGoalWasDetermined[ObstaclesModel::ownGoal])
  {
    angles[bestAngleAwayFromOwnGoalNoObstacles] = obstaclesModel.angleToFreePartOfGoal[ObstaclesModel::ownGoal] + pi;

/*
    LINE(behavior_kickAngles, 
    robotPose.getPose().translation.x, 
    robotPose.getPose().translation.y, 
    robotPose.getPose().translation.x + cos(bestAngleAwayFromOwnGoalNoObstacles + robotPose.getAngle()) * 2000,
    robotPose.getPose().translation.y + sin(bestAngleAwayFromOwnGoalNoObstacles + robotPose.getAngle()) * 2000,
    2, Drawings::ps_solid, Drawings::green
    );
*/
  }
  else
  {
    double angleToLeftOwnGoalPost = Geometry::angleTo(robotPose, Vector2<double>(xPosOwnGroundline,yPosLeftGoal));
    double angleToRightOwnGoalPost = Geometry::angleTo(robotPose, Vector2<double>(xPosOwnGroundline,yPosRightGoal));
    
    if(angleToLeftOwnGoalPost < angleToRightOwnGoalPost)
    {
      angleToLeftOwnGoalPost += pi2;
    }
    angles[bestAngleAwayFromOwnGoalNoObstacles] = (angleToLeftOwnGoalPost + angleToRightOwnGoalPost) / 2.0;
/*
  LINE(behavior_kickAngles, 
    robotPose.getPose().translation.x, 
    robotPose.getPose().translation.y, 
    robotPose.getPose().translation.x + cos(angleToLeftOwnGoalPost + robotPose.getAngle()) * 2000,
    robotPose.getPose().translation.y + sin(angleToLeftOwnGoalPost + robotPose.getAngle()) * 2000,
    2, Drawings::ps_solid, Drawings::white);

  LINE(behavior_kickAngles, 
    robotPose.getPose().translation.x, 
    robotPose.getPose().translation.y, 
    robotPose.getPose().translation.x + cos(angleToRightOwnGoalPost + robotPose.getAngle()) * 2000,
    robotPose.getPose().translation.y + sin(angleToRightOwnGoalPost + robotPose.getAngle()) * 2000,
    2, Drawings::ps_solid, Drawings::white
    );
*/
  }

  if(
    robotPose.translation.y > yPosLeftSideline - 350 || 
    robotPose.translation.y < yPosRightSideline + 350
    )
  {
    angles[bestAngleAwayFromOwnGoalNoObstacles] = normalize(-robotPose.getAngle());
  }

  angles[bestAngleAwayFromOwnGoal] = angles[bestAngleAwayFromOwnGoalNoObstacles];
/*
  LINE(behavior_kickAnglesRadar, 
    0, 0, 
    cos(bestAngleAwayFromOwnGoalNoObstacles) * 2000,
    sin(bestAngleAwayFromOwnGoalNoObstacles) * 2000,
    40, Drawings::ps_solid, Drawings::red
    );
  LINE(behavior_kickAngles, 
    robotPose.getPose().translation.x, 
    robotPose.getPose().translation.y, 
    robotPose.getPose().translation.x + cos(angles[bestAngleAwayFromOwnGoalNoObstacles] + robotPose.getAngle()) * 2000,
    robotPose.getPose().translation.y + sin(angles[bestAngleAwayFromOwnGoalNoObstacles] + robotPose.getAngle()) * 2000,
    2, Drawings::ps_solid, Drawings::red
    );



  LINE(behavior_kickAnglesRadar, 
    0, 0, 
    cos(angles[bestAngleToOpponentGoalNoObstacles]) * 1000,
    sin(angles[bestAngleToOpponentGoalNoObstacles]) * 1000,
    40, Drawings::ps_solid, Drawings::red
    );
  LINE(behavior_kickAngles, 
    robotPose.getPose().translation.x, 
    robotPose.getPose().translation.y, 
    robotPose.getPose().translation.x + cos(angles[bestAngleToOpponentGoalNoObstacles] + robotPose.getAngle()) * 1000,
    robotPose.getPose().translation.y + sin(angles[bestAngleToOpponentGoalNoObstacles] + robotPose.getAngle()) * 1000,
    2, Drawings::ps_solid, Drawings::red
    );

  LINE(behavior_kickAngles, 
    robotPose.getPose().translation.x, 
    robotPose.getPose().translation.y, 
    robotPose.getPose().translation.x + cos(angles[angleToPointBehindOpponentGoal] + robotPose.getAngle()) * 1000,
    robotPose.getPose().translation.y + sin(angles[angleToPointBehindOpponentGoal] + robotPose.getAngle()) * 1000,
    2, Drawings::ps_solid, Drawings::yellow
    );


  LINE(behavior_kickAnglesRadar, 
    0, 0, 
    cos(angles[bestAngleToOpponentGoal]) * 1000,
    sin(angles[bestAngleToOpponentGoal]) * 1000,
    20, Drawings::ps_solid, Drawings::blue
    );
  LINE(behavior_kickAngles, 
    robotPose.getPose().translation.x, 
    robotPose.getPose().translation.y, 
    robotPose.getPose().translation.x + cos(angles[bestAngleToOpponentGoal] + robotPose.getAngle()) * 1000,
    robotPose.getPose().translation.y + sin(angles[bestAngleToOpponentGoal] + robotPose.getAngle()) * 1000,
    2, Drawings::ps_solid, Drawings::blue
    );
*/

/*
  // goal kick angle ///////////
  double angle;
//  if(motionInfo.executedMotionRequest.walkType != MotionRequest::turnWithBall)
  if(headControlMode.headControlMode != HeadControlMode::catchBall)
  {
    angle = obstaclesModel.getAngleOfLargeGapInRange2(0, pi, ObstaclesModel::searchLeftAndRight);
  }
  else if(motionInfo.executedMotionRequest.walkRequest.walkParams.rotation < 0)
  {
    angle = obstaclesModel.getAngleOfLargeGapInRange2(0, pi, ObstaclesModel::searchRight);
  }
  else
  {
    angle = obstaclesModel.getAngleOfLargeGapInRange2(0, pi, ObstaclesModel::searchLeft);
  }

  angles[goalieGoalKickAngle] = normalize(angle + robotPose.getAngle());
  if(toDegrees(angles[goalieGoalKickAngle]) > 65) angles[goalieGoalKickAngle] = fromDegrees(65);
  if(toDegrees(angles[goalieGoalKickAngle]) < -65) angles[goalieGoalKickAngle] = fromDegrees(-65);
  angles[goalieGoalKickAngle] = normalize(angles[goalieGoalKickAngle] - robotPose.getAngle());

  if(robotPose.rotation > fromDegrees(95) || robotPose.rotation < fromDegrees(-95) )
  {
    angles[goalieGoalKickAngle] = angles[bestAngleAwayFromOwnGoal];
  }
*/
  angles[goalieGoalKickAngle] = -robotPose.getAngle();

/*
  LINE(behavior_kickAnglesRadar, 
    0, 0, 
    cos(goalieGoalKickAngle) * 1000,
    sin(goalieGoalKickAngle) * 1000,
    20, Drawings::ps_solid, Drawings::green
    );
  LINE(behavior_kickAngles, 
    robotPose.getPose().translation.x, 
    robotPose.getPose().translation.y,
    robotPose.getPose().translation.x + cos(goalieGoalKickAngle + robotPose.getAngle()) * 1000,
    robotPose.getPose().translation.y + sin(goalieGoalKickAngle + robotPose.getAngle()) * 1000,
    2, Drawings::ps_solid, Drawings::green
    );

  DEBUG_DRAWING_FINISHED(behavior_kickAnglesRadar);
  DEBUG_DRAWING_FINISHED(behavior_kickAngles);
*/
}

double AngleSymbols::getAngle(Angles id)
{
  return angles[id];
}

void AngleSymbols::drawAngleShownByLeds()
{
  LINE(behavior_kickAngles, 
    robotPose.getPose().translation.x, 
    robotPose.getPose().translation.y,
    robotPose.getPose().translation.x + cos(angles[angleShownByLEDs] + robotPose.getAngle()) * 1500,
    robotPose.getPose().translation.y + sin(angles[angleShownByLEDs] + robotPose.getAngle()) * 1500,
    30, Drawings::ps_solid, Drawings::light_gray
    );

  DEBUG_DRAWING_FINISHED(behavior_kickAngles);
}

/*
* Change Log
* 
* $Log: AngleSymbols.cpp,v $
* Revision 1.2  2004/06/23 19:31:10  juengel
* Added angle "undefined"
*
* Revision 1.1  2004/06/22 18:48:56  juengel
* kickAngles clean up
*
*/

---