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"

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)
{
  // "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);
}

void ChallengeSymbols::update()
{
}


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

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

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

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;
}

/*
* Change Log
* 
* $Log: ChallengeSymbols.cpp,v $
* Revision 1.2  2004/06/28 12:27:59  schumann
* part 1 of the almost slam challenge behavior
*
* Revision 1.1.1.1  2004/05/22 17:16:57  cvsadm
* created new repository GT2004_WM
*
* Revision 1.2  2004/03/08 00:58:54  roefer
* Interfaces should be const
*
* Revision 1.1  2003/10/22 22:18:44  loetzsch
* prepared the cloning of the GT2003BehaviorControl
*
*/

---