Modules/BehaviorControl/CommonXabsl2Symbols/ChallengeSymbols.cpp

Go to the documentation of this file.

/** 
* @file ChallengeSymbols.cpp
*
* Implementation of class ChallengeSymbols.
*
* @author Thomas Röfer
*/

#include "ChallengeSymbols.h"
#include "Tools/FieldDimensions.h"
#include "Tools/StringFunctions.h"

#include "Representations/Perception/SlamPercept.h"

#define slamPercept (*(SlamPercept *)specialPercept.slamData)

ChallengeSymbols::ChallengeSymbols(const BehaviorControlInterfaces& interfaces)
: BehaviorControlInterfaces(interfaces)
{
#ifdef _WIN32
  InConfigFile stream("points.cfg");
#else
  InConfigFile stream("/MS/points.cfg");
#endif
  if(stream.exists())
  {
    for(int i = 0; i < 5; ++i)
    {
      stream >> targetPositions[i].translation.x >> targetPositions[i].translation.y;
      if(getPlayer().getTeamColor() == Player::red)
        targetPositions[i].translation *= -10;
      else
        targetPositions[i].translation *= 10;
      challenge2OptimalRotation(targetPositions[i]);
    }
  }
  lastIndex = 10;
}

void ChallengeSymbols::registerSymbols(Xabsl2Engine& engine)
{
  lastSeenCenterCircle = SystemCall::getCurrentSystemTime ();

  int i;
  char s[256];

  // "challenge-slam.current-target-point"
  engine.registerDecimalInputSymbol("challenge-slam.current-target-point", &index);

  engine.registerDecimalInputSymbol("challenge-slam.imgY", &((SlamPercept *) specialPercept.slamData)->imageCharacteristic.averageY);
  engine.registerDecimalInputSymbol("challenge-slam.imgU", &((SlamPercept *) specialPercept.slamData)->imageCharacteristic.averageU);
  engine.registerDecimalInputSymbol("challenge-slam.imgV", &((SlamPercept *) specialPercept.slamData)->imageCharacteristic.averageV);

  engine.registerBooleanInputSymbol("challenge-slam.wlan-bearing-finished", &((SlamPercept *) specialPercept.slamData)->wlanBearing.finished);
  engine.registerDecimalInputSymbol("challenge-slam.wlan-bearing-direction", &((SlamPercept *) specialPercept.slamData)->wlanBearing.direction);

  engine.registerDecimalInputSymbol("challenge-slam.wlan-bearing-pos-center", &((SlamPercept *) specialPercept.slamData)->wlanBearing.posCenter);
  engine.registerDecimalInputSymbol("challenge-slam.wlan-bearing-neg-center", &((SlamPercept *) specialPercept.slamData)->wlanBearing.negCenter);

  engine.registerBooleanInputSymbol("challenge-slam.wlan-bearing-facing-blue", &((SlamPercept *) specialPercept.slamData)->wlanBearing.facingBlue);

  engine.registerBooleanInputSymbol("challenge-slam.green-location-finished", &((SlamPercept *) specialPercept.slamData)->greenLocation.greenLocationFinished);

  engine.registerBooleanInputSymbol("challenge-slam.angle-to-max-green-valid", &((SlamPercept *) specialPercept.slamData)->greenLocation.angleToMaxGreenValid);
  engine.registerDecimalInputSymbol("challenge-slam.angle-to-max-green", &((SlamPercept *) specialPercept.slamData)->greenLocation.angleToMaxGreen);
  engine.registerDecimalInputSymbol("challenge-slam.length-of-max-green", &((SlamPercept *) specialPercept.slamData)->greenLocation.lengthOfMaxGreen);

  engine.registerBooleanInputSymbol("challenge-slam.turn-finished", &((SlamPercept *) specialPercept.slamData)->turnFinished);

  engine.registerDecimalInputSymbol("challenge-slam.general-purpose", &((SlamPercept *) specialPercept.slamData)->GPVal);

  /*
  * Center circle symbols
  */
  engine.registerBooleanInputSymbol("challenge-slam.center-circle-visible", &slamPercept.centerCircleInformation.centerCircleIsVisible);
  engine.registerDecimalInputSymbol("challenge-slam.direction-to-center-circle", &slamPercept.centerCircleInformation.directionToCenterCircle);
  engine.registerDecimalInputSymbol("challenge-slam.distance-to-center-circle", &slamPercept.centerCircleInformation.distanceToCenterCircle);
  engine.registerDecimalInputSymbol("challenge-slam.seen-center-circle-orientation", &slamPercept.centerCircleInformation.seenCenterCircleOrientation);

  engine.registerDecimalInputSymbol("challenge-slam.time-since-center-circle-visible", &slamPercept.centerCircleInformation.timeSinceLastSeenCenterCircle);

  // "challenge-slam.probable-direction"
  engine.registerDecimalInputFunction("challenge-slam.probable-direction",this,
    (double (Xabsl2FunctionProvider::*)())&ChallengeSymbols::getProbableOrientation );

  // "challenge-slam.target-position.x"
  engine.registerDecimalInputFunction("challenge-slam.target-position.x",this,
    (double (Xabsl2FunctionProvider::*)())&ChallengeSymbols::_challenge2TargetPositionX);
  engine.registerDecimalInputFunctionParameter("challenge-slam.target-position.x",
                                               "challenge-slam.target-position.x.index",index);

  // "challenge-slam.target-position.y"
  engine.registerDecimalInputFunction("challenge-slam.target-position.y",this,
    (double (Xabsl2FunctionProvider::*)())&ChallengeSymbols::_challenge2TargetPositionY);
  engine.registerDecimalInputFunctionParameter("challenge-slam.target-position.y",
                                               "challenge-slam.target-position.y.index",index);

  // "challenge-slam.target-position.rotation"
  engine.registerDecimalInputFunction("challenge-slam.target-position.rotation",this,
    (double (Xabsl2FunctionProvider::*)())&ChallengeSymbols::_challenge2TargetPositionRotation);
  engine.registerDecimalInputFunctionParameter("challenge-slam.target-position.rotation",
                                               "challenge-slam.target-position.rotation.index",index);

  // "challenge-slam.state"
  engine.registerEnumeratedOutputSymbol("challenge-slam.state",(int*)&slamPercept.slamState);
  for (i=0;i<SlamPercept::numOfSlamStates;i++)
  {
    sprintf(s,"challenge-slam.state.");
  getXmlString(s+strlen(s),SlamPercept::getSlamStateName((SlamPercept::SlamState)i));
    engine.registerEnumeratedOutputSymbolEnumElement("challenge-slam.state",s,i);
  }
}

void ChallengeSymbols::update()
{
}

double ChallengeSymbols::_challenge2TargetPositionX()
{
  return challenge2TargetPositionX (index);
};

double ChallengeSymbols::_challenge2TargetPositionY()
{
  return challenge2TargetPositionY (index);
};

double ChallengeSymbols::_challenge2TargetPositionRotation()
{
  return challenge2TargetPositionRotation (index);
};

double ChallengeSymbols::challenge2TargetPositionX(double index)
{
  challenge2ShortestPath();
  return targetPositions[int(index)].translation.x;
}

double ChallengeSymbols::challenge2TargetPositionY(double index)
{
  challenge2ShortestPath();
  return targetPositions[int(index)].translation.y;
}

double ChallengeSymbols::challenge2TargetPositionRotation(double index)
{
  challenge2ShortestPath();
  return toDegrees(targetPositions[int(index)].rotation);
}

double ChallengeSymbols::imageCharacteristicsAvgY ()
{
  return ((SlamPercept *) specialPercept.slamData)->imageCharacteristic.averageY;
};

double ChallengeSymbols::imageCharacteristicsAvgU ()
{
  return ((SlamPercept *) specialPercept.slamData)->imageCharacteristic.averageU;
};

double ChallengeSymbols::imageCharacteristicsAvgV ()
{
  return ((SlamPercept *) specialPercept.slamData)->imageCharacteristic.averageV;
};


void ChallengeSymbols::challenge2OptimalRotation(Pose2D& pose)
{
  if(pose.translation.x < -2000 ||
     pose.translation.x > 700 && pose.translation.x < 2000)
    pose.rotation = 0;
  else if(pose.translation.x > 2000 ||
          pose.translation.x < -700 && pose.translation.x > -2000)
    pose.rotation = pi;
  else if(pose.translation.y > 0)
    pose.rotation = pi_2;
  else
    pose.rotation = -pi_2;
    
//    OUTPUT(idText,text,pose.translation.x << " " << pose.translation.y << " " << int(pose.rotation * 180 / pi));

  // reach position with center of body
  pose += Pose2D(Vector2<double>(50,0));
}

void ChallengeSymbols::challenge2ShortestPath()
{
  // this is a hack to recalculate path on restart
  if(index == 0 && lastIndex > 0)
  {
    Pose2D p[6];
    p[0] = robotPose;
    memcpy(p + 1, targetPositions, sizeof(targetPositions));
    double minLength = 1e9;
    challenge2ShortestPermutation(p, 1, minLength);
  }
  lastIndex = index;
}

void ChallengeSymbols::challenge2ShortestPermutation(Pose2D* p, int n, double& minLength)
{
  if(n < 6) 
    for(int i = n; i < 6; ++i)
    {
      challenge2Swap(p, i, n);
      challenge2ShortestPermutation(p, n + 1, minLength);
      challenge2Swap(p, i, n);
    }
  else
  {
    double length = 0;
    for(int i = 0; i < 5; ++i)
      length += (p[i].translation - p[i + 1].translation).abs();
    if(length < minLength)
    {
      memcpy(targetPositions, p + 1, sizeof(targetPositions));
      minLength = length;
    }
  }
}

void ChallengeSymbols::challenge2Swap(Pose2D* p, int i, int j)
{
  Pose2D t = p[i];
  p[i] = p[j];
  p[j] = t;
}

double ChallengeSymbols::getProbableOrientation ()
{
  return 0;
}

---