Tools/KickSelectionTable.cpp

Go to the documentation of this file.

/** 
* @file KickSelectionTable.cpp
* Implementation of class KickSelectionTable.
*
* @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Jüngel</A>
*/

#ifdef APERIOS1_3_2
#include "Platform/GTAssert.h"
#include <stdio.h>
#include <iostream.h>
#endif
#include "KickSelectionTable.h"
#include "Platform/SystemCall.h"
#include "Tools/Math/Common.h"
#include "Tools/Streams/InStreams.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Location.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Debugging/DebugDrawings.h"

KickSelectionTable::KickSelectionTable() : numberOfSectors(12)
{
  memset(action,0,sizeof(action));
}

KickSelectionTable::~KickSelectionTable()
{
}

KickSelectionTable::KickSelectionTableID KickSelectionTable::getTableIDFromName(const char* name)
{
  for (int i=0;i<numberOfKickSelectionTableIDs;i++)
  {
    if (strcmp(name,getKickSelectionTableIDName((KickSelectionTableID)i))==0)
    {
      return (KickSelectionTableID)i;
    }
  }
  
  return inCenterOfField;
}

KickSelectionTable::ActionID KickSelectionTable::getActionIDFromShortName(const char* name)
{
  for (int i=0;i<numberOfActions;i++)
  {
    if (strcmp(name,getShortActionName((ActionID)i))==0)
    {
      return (ActionID)i;
    }
  }
  
  return nothing;
}

void KickSelectionTable::load(const char* fileName)
{
  InTextFile f(getLocation().getModelFilename(fileName));
  if (f.exists())
    f >> *this;

  //OutTextFile f2(getLocation().getModelFilename("test.kst"));
  //f2 << *this;
}

int KickSelectionTable::getSectorFromAngle(double destinationAngle) const {
  return (int)((normalize(fromDegrees(destinationAngle) + pi + pi2 / KickSelectionTable::numberOfSectors / 2) + pi) / pi2 * KickSelectionTable::numberOfSectors);
}

Vector2<double> KickSelectionTable::searchKickInNeighbourhood(
  double ballOffsetX, double ballOffsetY,
  double destinationAngle, KickSelectionTableID kickSelectionTableID,
  RobotPose robotPose, double maxDistance, double& kick
  ) const
{
//  Vector2<double> ballAbsolute = Geometry::relative2FieldCoord(robotPose,ballOffsetX,ballOffsetY);
//  robotPose.translation.x=ballAbsolute.x; // hack to get ball position on field with robot rotation.
//  robotPose.translation.y=ballAbsolute.y;
  int currx=(int)(ballOffsetX / 10.0);
  int curry=(int)(ballOffsetY / 10.0) + KickSelectionTable::yRange / 2;
  int sector=getSectorFromAngle(destinationAngle);
  Vector2<double> found=Vector2<double>(10000,10000);
//  Vector2<double> founddxdy=Vector2<double>(10000,10000);
  int distance=0;
  int dx=0;
  while (distance<maxDistance){
    for (int dy=-dx; dy<=dx; dy++){
//      #ifdef APERIOS1_3_2
//      cout << "search\n";
//      #endif
      int x=currx-dx;  // ball verschieben, wie es sich verhalten würde, wenn wir den roboter um dx,dy bewegen
      int y=curry-dy;

      // roboter auch verschieben, um den punkt zu erhalten, wo der roboter sich hinbewegen müßte (absolut für debug drawings)
      Vector2<double> testPoint = Geometry::relative2FieldCoord(robotPose,dx*10,dy*10);

      //checken, ob der neue virtuelle ballpunkt noch in der kicktable ist.
      if (x >= 0 && x < KickSelectionTable::xRange && y>=0 && y<KickSelectionTable::yRange){
        CROSS(selfLocatorField,testPoint.x,testPoint.y,4,1,0,Drawings::skyblue);
//        OUTPUT(idText,text,"Search x="<<x<<" y="<<y<<" sec="<<sector<<" ==> "<<(int)action[x][y][sector][kickSelectionTableID]);
        //checken, ob am virtuellen ballpunkt ein kick möglich ist
        if (action[x][y][sector][kickSelectionTableID]!= nothing){
          // korrekturvektor speichern
          Vector2<double> current= Vector2<double>(dx*10,dy*10); 
          //checken, ob korrekturvektor in maximaldistanz und wir bisher keinen besseren kennen
          if ((current.abs()< maxDistance) && (current.abs() < found.abs())){
            // wenn dem so ist, neuen korrekturvektor speichern und debug drawing malen
            CROSS(selfLocatorField,testPoint.x,testPoint.y, 4, 1, 0, Drawings::blue);

//            OUTPUT(idText,text,"Found: dx="<<current.x<<" dy="<<current.y);
            found=current;
            kick=action[x][y][sector][kickSelectionTableID];

//            founddxdy=Vector2<double>(dx,dy);
          }
        }
      } else {
        CROSS(selfLocatorField,testPoint.x,testPoint.y,4,1,0,Drawings::black);
      }
    }
    

    dx++;
    distance = (int) (Vector2<double>(-dx*10,0)).abs();
  }
  Vector2<double> finalPoint = Geometry::relative2FieldCoord(robotPose,found.x,found.y);
  CIRCLE(selfLocatorField,finalPoint.x,finalPoint.y, 10, 10, 0, Drawings::blue);
  DEBUG_DRAWING_FINISHED(selfLocatorField);
  return found;

}

KickSelectionTable::ActionID KickSelectionTable::retrieveKick
(
 double ballOffsetX, double ballOffsetY,
 double destinationAngle,
 double destinationAngleWidth,
 KickSelectionTableID kickSelectionTableID
) const
{
  int x,y;
  x = (int)(ballOffsetX / 10.0);
  y = (int)(ballOffsetY / 10.0) + KickSelectionTable::yRange / 2;
  
  if(x < 0 || x >= KickSelectionTable::xRange ||
    y < 0 || y >= KickSelectionTable::yRange) 
    return nothing;
  
  int minSector = getSectorFromAngle(destinationAngle - destinationAngleWidth / 2.0); 
  int maxSector = getSectorFromAngle(destinationAngle + destinationAngleWidth / 2.0) + 1; 

  if (maxSector > numberOfSectors) maxSector -= numberOfSectors;
  if (maxSector == 0) maxSector = numberOfSectors;

  KickSelectionTable::ActionID selectedAction = nothing;
  double minAngleDist = pi;
  for (int i = minSector; i != maxSector; i++)
  {
    if (i >= numberOfSectors) i -= numberOfSectors;
    KickSelectionTable::ActionID sectorAction;
    sectorAction = action[x][y][i][kickSelectionTableID];
    if (sectorAction != nothing)
    {
      double centerAngle = normalize(pi * ((double)i) / ((double)numberOfSectors));
      double angleDist = fabs(normalize(fromDegrees(destinationAngle) - centerAngle));
      if (angleDist < minAngleDist)
      {
        selectedAction = sectorAction;
        minAngleDist = angleDist;
      }
    }
  }

  return selectedAction;
}

In& operator>>(In& stream, KickSelectionTable& kickSelectionTable)
{
  KickSelectionTable::KickSelectionTableID tableID;
  KickSelectionTable::ActionID actionID;
  int x,y,s;
  char buf[200];
  memset(kickSelectionTable.action,0,sizeof(kickSelectionTable.action));
  
  kickSelectionTable.numberOfSectors = 0;
  while (1)
  {
    stream >> buf;
    if (strcmp(buf,"___")==0) break;
    tableID = kickSelectionTable.getTableIDFromName(buf);
    while(1)
    {
      stream >> buf;
      if (strcmp(buf,"___")==0) break;
      
      x = atoi(buf);
      
      stream >> y >> s >> buf;
      if (s >= kickSelectionTable.numberOfSectors) 
      {
        kickSelectionTable.numberOfSectors = s + 1;
        ASSERT(kickSelectionTable.numberOfSectors <= KickSelectionTable::maxNumberOfSectors);
      }
      actionID = kickSelectionTable.getActionIDFromShortName(buf);
      kickSelectionTable.action[x][y][s][tableID] = actionID;
    }
  }
  return stream;
}

Out& operator<<(Out& stream, const KickSelectionTable& kickSelectionTable)
{
  int t,x,y,s;
  for (t=0; t<KickSelectionTable::numberOfKickSelectionTableIDs; t++)
  {
    stream << kickSelectionTable.getKickSelectionTableIDName((KickSelectionTable::KickSelectionTableID)t);
    
    for (x = 0; x< kickSelectionTable.xRange; x++)
    {
      for (y = 0; y< kickSelectionTable.yRange; y++)
      {
        for (s = 0; s< kickSelectionTable.numberOfSectors; s++)
        {
          /*if (kickSelectionTable.action[x][y][s][t] == KickSelectionTable::nothing &&
              kickSelectionTable.action[x][y][(s+1) % kickSelectionTable.numberOfSectors ][t] != KickSelectionTable::nothing &&
              kickSelectionTable.action[x][y][(s+1) % kickSelectionTable.numberOfSectors ][t] ==
                kickSelectionTable.action[x][y][(s+kickSelectionTable.numberOfSectors-1) % kickSelectionTable.numberOfSectors ][t]
              )
          {
            stream << x << y << s << kickSelectionTable.getShortActionName(
              kickSelectionTable.action[x][y][(s+1) % kickSelectionTable.numberOfSectors ][t]);
          } 
          else 
          */
          {
            if (kickSelectionTable.action[x][y][s][t] != KickSelectionTable::nothing ||
                // always write one entry from last sector, so number of sectors can be determined correctly
                (x==0 && y == 0 && s == kickSelectionTable.numberOfSectors-1 && t == 0))
            {
              stream << x << y << s << kickSelectionTable.getShortActionName(
                kickSelectionTable.action[x][y][s][t]);
            }
          }
        }
      }
    }
    stream << "___";
  }
  stream << "___";
  return stream;
}

In& operator>>(In& stream,KickCase& kickCase)
{
  stream.read(&kickCase,sizeof(KickCase));
  return stream;
}
 
Out& operator<<(Out& stream, const KickCase& kickCase)
{
  stream.write(&kickCase,sizeof(KickCase));
  return stream;
}

---