Modules/ObstaclesLocator/GTCamObstaclesLocator.cpp

Go to the documentation of this file.

/**
* @file GTCamObstaclesLocator.cpp
*
* This file contains a class for obstacle localization using the worldstate of the GTCam.
*
* @author <a href="mailto:timo.diekmann@uni-dortmund.de">Timo Diekmann</a>
*/

/*
GTCamObstaclesLocator:

The GTCam solution for the module ObstaclesLocator uses the positions and orientations of the three teammates and four opponents of the GTCamWorldState to model them as obstacles in the ObstaclesModel. Further the 16 segments of the border including the goals are modeled. The ObstacleModell is filled relative to the own position determined by robotPose. To use the information of the GTCam for the own pose, you have to switch the SelfLocator to GTCam!

These attributes of obstaclesModel are calculated:
  - distance[]: the distance to the nearest border, goal, or player for each sector
    range: [0..8000)
  - obstacleType[]: the type of the nearest obstacle for each sector
    range: {ObstaclesPercept::border, ...::goal, ...::teammate, ...::opponent, ...::unknown}
    remark: normaly no sector is marked with the type ObstaclesPercept::unknown. This only happens if the robot is standing very close to one border or outside the field
  - frameNumber: set to cameraMatrix.frameNumber
  - distanceToFreePartOfGoal[]: the two distances to free part of goals
    range: [0..8000)
    remark: 0 is the index for own goal, 1 the index for opponent goal
    Attention: the calculation of this and the following three goal-attributes may not be correct, if the robot is posed far back in one goal!
  - widthOfFreePartOfGoal[]: the two widths of free part of goals
    range: [2pi/90..2pi)
    remark: see distanceToFreePartOfGoal[]
    Attention: see distanceToFreePartOfGoal[]!
  - angleToFreePartOfGoal[]: the two angles to the center of the free part of goals
    range: (-pi..pi)
    remark: see distanceToFreePartOfGoal[]
    Attention: see distanceToFreePartOfGoal[]!
  - angleToFreePartOfGoalWasDetermined[]: the two boolean values, whether there is a free part of the goals found
    range: {true, false}
    remark: see distanceToFreePartOfGoal[]
    Attention: see distanceToFreePartOfGoal[]!
  - distanceToNextFreeTeammate: the distance to the next teammate with no obstacle between
    range: [0..8000)
    remark: this calculation is based on GTCam data, not on the PlayersPoseCollection
  - angleToNextFreeTeammateWasDetermined: the angle to the next teammate with no obstacle between
    range: (-pi..pi)
    remark: see distanceToNextFreeTeammate
  - corridorInFront: length of a free rectangular corridor in front of the robot
    range: [0..8000)
    remark: aproximated sector width: 280 mm within a length of ca. 2000 mm
    Attention: only aproximated by observing four sectors!

These attributes of obstaclesModel are not set:
  - bodyPSD: it is set in the constructor of ObstaclesModel to 0.0

Proceeding:
  For every border the sectors are determined. For every sector the distance to the border is determined.
  For each of the three teammates and four opponents the sectors are (aproximately) determined. For every sector the distance to the circle around the player is (aproximately) determined.
  When the sector modell is filled, the "XABSL-Symbols" are calculated based on the sector modell and GTCam data.
*/

#include "GTCamObstaclesLocator.h"
#include "Tools/Player.h"
#include "Platform/SystemCall.h"
#include "Tools/FieldDimensions.h"

GTCamObstaclesLocator::GTCamObstaclesLocator(const ObstaclesLocatorInterfaces& interfaces)
: ObstaclesLocator(interfaces)
{
  minStep = 0.1/ObstaclesModel::numOfSectors;
  sectorWidth = pi2/ObstaclesModel::numOfSectors;
  frontSectorNr = obstaclesModel.getSectorFromAngle(0.0);
  deltaSector45Nr = obstaclesModel.getSectorFromAngle(pi/4)-frontSectorNr;
}


void GTCamObstaclesLocator::execute()
{
  //long myTime = SystemCall::getCurrentSystemTime();
  obstaclesModel.frameNumber = cameraMatrix.frameNumber;

  rx = robotPose.translation.x;
  ry = robotPose.translation.y;
  ra = robotPose.rotation;
  int sector;
  for(sector = 0; sector < ObstaclesModel::numOfSectors; sector++)
  {
    obstaclesModel.obstacleType[sector] = ObstaclesPercept::unknown;
    obstaclesModel.distance[sector] = 8000;
  }

  // model borders and goals
  modelLine(false, xPosOpponentGoal, yPosRightGoal, yPosLeftGoal, ObstaclesPercept::goal);
  modelLine(false, xPosOwnGoal, yPosRightGoal, yPosLeftGoal, ObstaclesPercept::goal);
  modelLine(true, yPosLeftGoal, xPosOpponentGoalpost, xPosOpponentGoal, ObstaclesPercept::goal);
  modelLine(true, yPosLeftGoal, xPosOwnGoal, xPosOwnGoalpost, ObstaclesPercept::goal);
  modelLine(true, yPosRightGoal, xPosOpponentGoalpost, xPosOpponentGoal, ObstaclesPercept::goal);
  modelLine(true, yPosRightGoal, xPosOwnGoal, xPosOwnGoalpost, ObstaclesPercept::goal);
  modelLine(true, yPosLeftGoal, xPosOpponentGroundline, xPosOpponentGoalpost, ObstaclesPercept::border);
  modelLine(true, yPosLeftGoal, xPosOwnGoalpost, xPosOwnGroundline, ObstaclesPercept::border);
  modelLine(true, yPosRightGoal, xPosOpponentGroundline, xPosOpponentGoalpost, ObstaclesPercept::border);
  modelLine(true, yPosRightGoal, xPosOwnGoalpost, xPosOwnGroundline, ObstaclesPercept::border);
  modelLine(false, xPosOpponentGroundline, yPosRightSideline, yPosRightGoal, ObstaclesPercept::border);
  modelLine(false, xPosOwnGroundline, yPosRightSideline, yPosRightGoal, ObstaclesPercept::border);
  modelLine(false, xPosOpponentGroundline, yPosLeftGoal, yPosLeftSideline, ObstaclesPercept::border);
  modelLine(false, xPosOwnGroundline, yPosLeftGoal, yPosLeftSideline, ObstaclesPercept::border);
  modelLine(true, yPosLeftSideline, xPosOwnGroundline, xPosOpponentGroundline, ObstaclesPercept::border);
  modelLine(true, yPosRightSideline, xPosOwnGroundline, xPosOpponentGroundline, ObstaclesPercept::border);

  const int me = getPlayer().getPlayerNumber();
  const int color = getPlayer().getTeamColor();
  const int oppColor = (color == Player::red ? Player::blue : Player:: red);
  Vector2<double> pos;

  // model other players
  int i;
  for (i = 0; i < 4; i++)
  {
    if (i != me)
    {
      pos = gtCamWorldState.getPlayerPos(i, color);
      modelRobot(pos.x, pos.y, gtCamWorldState.getPlayerOrientation(i, color), ObstaclesPercept::teammate);
    }
    pos = gtCamWorldState.getPlayerPos(i, oppColor);
    modelRobot(pos.x, pos.y, gtCamWorldState.getPlayerOrientation(i, oppColor), ObstaclesPercept::opponent);
  }
  
  // find nextFreeTeammate
  obstaclesModel.angleToNextFreeTeammate = 0.0;
  obstaclesModel.distanceToNextFreeTeammate = 8000.0 * 8000.0;
  obstaclesModel.angleToNextFreeTeammateWasDetermined = false;
  for (i = 0; i < 4; i++)
  {
    if (i != me)
    {
      pos = gtCamWorldState.getPlayerPos(i, color);
      double dx = pos.x - rx;
      double dy = pos.y - ry;
      double angle = atan2(dy, dx) - ra;
      int sector = obstaclesModel.getSectorFromAngle(angle);
      if (obstaclesModel.obstacleType[sector] == ObstaclesPercept::teammate)
      {
        double d = dx * dx + dy * dy;
        if (d < obstaclesModel.distanceToNextFreeTeammate)
        {
          // normalize angle:
          if (angle < -pi) angle += pi2;
          if (angle >  pi) angle -= pi2;
          obstaclesModel.angleToNextFreeTeammate = angle;
          obstaclesModel.distanceToNextFreeTeammate = d;
          obstaclesModel.angleToNextFreeTeammateWasDetermined = true;
        }
      }
    }
  }
  obstaclesModel.distanceToNextFreeTeammate = sqrt(obstaclesModel.distanceToNextFreeTeammate);

  // calculate angleToFreePartOfGoal...
  int gs[2] = {0, 0}; // goalSector
  int gsc = 0; // goalSectorCount
  int gscmax[2] = {0, 0};
  int gi = 0; // goal index: 0 = own goal, 1 = opponent goal // Is this correct??
  obstaclesModel.distanceToFreePartOfGoal[0] = 8000.0;
  obstaclesModel.distanceToFreePartOfGoal[1] = 8000.0;
  for(sector = 0; sector < ObstaclesModel::numOfSectors; sector++)
  {
    if (obstaclesModel.obstacleType[sector] == ObstaclesPercept::goal)
    {
      // if we saw no goal in the last sector, we have to determine which goal we see in the current sector
      if (gsc == 0)
      {
        int degree = (int)((180.0/pi)*((sector+0.5)*sectorWidth - pi + robotPose.rotation));
        degree = (degree + 360 + 180) % 360 - 180;
        if (rx > 0)
        {
          if (degree > 120  || degree < -120)
            gi = 0;
          else
            gi = 1;
        }
        else
        {
          if (degree > 60 || degree < -60)
            gi = 0;
          else
            gi = 1;
        }
      }
      ++gsc;
      if (gsc > gscmax[gi])
      {
        gscmax[gi] = gsc;
        gs[gi] = sector;
      }
      if (obstaclesModel.distance[sector] < obstaclesModel.distanceToFreePartOfGoal[gi])
        obstaclesModel.distanceToFreePartOfGoal[gi] = obstaclesModel.distance[sector];
    }
    else
    {
      gsc = 0;
    }
  }
  if (gsc > 0)
  {
    sector = 0;
    while (obstaclesModel.obstacleType[sector++] == ObstaclesPercept::goal)
      ++gsc;
    if (gsc > gscmax[gi])
    {
      gscmax[gi] = gsc;
      gs[gi] = sector-2;
    }
  }
  for (gi = 0; gi < 2; gi++)
  {
    if (gscmax[gi] > 0)
    {
      obstaclesModel.widthOfFreePartOfGoal[gi] = gscmax[gi]*sectorWidth;
      obstaclesModel.angleToFreePartOfGoal[gi] = (gs[gi]+1)*sectorWidth - pi -
        (obstaclesModel.widthOfFreePartOfGoal[gi])/2.0;
      // normalize angleToFreePartOfGoal
      if (obstaclesModel.angleToFreePartOfGoal[gi] < -pi)
        obstaclesModel.angleToFreePartOfGoal[gi] += pi2;
      obstaclesModel.angleToFreePartOfGoalWasDetermined[gi] = true;
      obstaclesModel.lastTimeFreePartOfGoalWasDetermined[gi] = SystemCall::getCurrentSystemTime();
    }
    else
    {
      obstaclesModel.angleToFreePartOfGoalWasDetermined[gi] = false;
    }
  }

  // approximation for obstaclesModel.corridorInFront:
  obstaclesModel.corridorInFront = obstaclesModel.distance[frontSectorNr-1];
  if (obstaclesModel.distance[frontSectorNr+1] < obstaclesModel.corridorInFront)
    obstaclesModel.corridorInFront = obstaclesModel.distance[frontSectorNr+1];
  if (obstaclesModel.distance[frontSectorNr+deltaSector45Nr] < 200)
  {
    int dist = (int)(obstaclesModel.distance[frontSectorNr+deltaSector45Nr] * 0.707);
    if (dist < obstaclesModel.corridorInFront)
      obstaclesModel.corridorInFront = dist;
  }
  if (obstaclesModel.distance[frontSectorNr-deltaSector45Nr] < 200)
  {
    int dist = (int)(obstaclesModel.distance[frontSectorNr-deltaSector45Nr] * 0.707);
    if (dist < obstaclesModel.corridorInFront)
      obstaclesModel.corridorInFront = dist;
  }
  //OUTPUT(idText,text,"dist: " << obstaclesModel.corridorInFront);

  //OUTPUT(idText,text,"time: " << SystemCall::getTimeSince(myTime));
}

bool GTCamObstaclesLocator::handleMessage(InMessage& message)
{
  return false;
}

/**
 * Models a line with constant x or constant y-value as an obstacle.
 * If you want to model a line with constant x-value (a horizontal line) you have to call modelLine with:
 *  - vert = false
 *  - x = the constant x-value of the line
 *  - sy with the minimum y-value of the line
 *  - ey with the maximum y-value of the line
 * If you want to model a line with constant y-value (a verlical line) you have to call modelLine with:
 *  - vert = true
 *  - x = the constant y-value of the line
 *  - sy with the minimum x-value of the line
 *  - ey with the maximum x-value of the line
 * In any case you determine the type of the obstacle with
 *  - type (border or goal, for example)
 * Attention: make sure that sy is less than ey, or uncomment the swap-passage in the method!
 */
void GTCamObstaclesLocator::modelLine(bool vert, double x, double sy, double ey, ObstaclesPercept::ObstacleType type)
{
  double r = (vert ? ry : rx);
  // break, if robot is standing to close on line
  if (x-1 < r && x+1 > r) return;
  /* swap-passage:
  if (sy > ey)
  {
    double h = sy;
    sy = ey;
    ey = h;
    OUTPUT(idText,text,"swap!");
  }
  // now sy is less or equal than ey
  */
  const double dx = x - r;
  double startAngle = 0;
  double endAngle = 0;
  if (vert)
  {
    startAngle = atan2(dx, sy - rx);
    endAngle = atan2(dx, ey - rx);
  } else {
    startAngle = atan2(sy - ry, dx);
    endAngle = atan2(ey - ry, dx);
  }
  int sector = obstaclesModel.getSectorFromAngle(startAngle - ra);
  int endSector = obstaclesModel.getSectorFromAngle(endAngle - ra);

  double subtract = 0;
  if ((dx < 0) ^ vert)
  {
    int hi = sector;
    sector = endSector;
    endSector = hi;
    double hd = startAngle;
    startAngle = endAngle;
    endAngle = hd;
    subtract = pi;
  }
  startAngle += (int)vert * pi_2 - subtract;
  endAngle += (int)vert * pi_2 - subtract;
  // normalize angles
  if (startAngle < -pi) startAngle += pi2;
  if (startAngle >  pi) startAngle -= pi2;
  if (endAngle < -pi) endAngle += pi2;
  if (endAngle >  pi) endAngle -= pi2;
  if (startAngle < -pi) startAngle += pi2;
  if (startAngle >  pi) startAngle -= pi2;
  if (endAngle < -pi) endAngle += pi2;
  if (endAngle >  pi) endAngle -= pi2;

  double angle = obstaclesModel.getAngleOfSector(sector) + ra - subtract + (int)vert * pi_2;
  // normalize angle
  if (angle < -pi) angle += pi2;
  if (angle >  pi) angle -= pi2;
  do 
  {
    double d = angle;
    if (d >= startAngle && d <= endAngle)
    {
      // calculate distance
      d = dx*tan(d);
      d *= d;
      d += dx*dx;
      d = sqrt(d);
      if (obstaclesModel.distance[sector] > d)
      {
        obstaclesModel.distance[sector] = (int)d;
        obstaclesModel.obstacleType[sector] = type;
      }
    }
    if (sector == endSector) break;
    if (++sector == ObstaclesModel::numOfSectors) sector = 0;
    angle += sectorWidth; // angle is already normalized
  } while (true);
}

/**
 * Models a robot as a circular obstacle
 *  - px = the x-coordinate of the robots neck
 *  - py = the y-coordinate of the robots neck
 *  - rotation = the rotation of the robot
 *  - type = the type of the obstacle (teammate or opponent, for example)
 * Remark: This Method uses the attribute obstaclesDist, wich determines the distance between the neck of the robot and the center of the circle around the robot.
 */
void GTCamObstaclesLocator::modelRobot(double px, double py, double rotation, ObstaclesPercept::ObstacleType type)
{
  px -= obstaclesDist*cos(rotation);
  py -= obstaclesDist*sin(rotation);
  double relx = px-rx;
  double rely = py-ry;
  double dist = sqrt(relx*relx+rely*rely);
  double angle = atan2(rely, relx)-ra;
  double openangle = atan(obstaclesRadius/dist)*(1+exp(-dist/obstaclesRadius));
  int sl = obstaclesModel.getSectorFromAngle(angle+openangle);
  int sr = obstaclesModel.getSectorFromAngle(angle-openangle);
  int sector = sl;
  do 
  {
    double x = angle-obstaclesModel.getAngleOfSector(sector);
    if (x < -pi)
      x += 2*pi;
    if (x > pi)
      x -= 2*pi;
    x /= openangle;
    x *= x;
    if (x <= 1)
    {
      int d = (int)(dist - obstaclesRadius*sqrt(1.0-x));
      if (d < 100) d = 100;
      if (obstaclesModel.distance[sector] > d)
      {
        obstaclesModel.distance[sector] = d;
        obstaclesModel.obstacleType[sector] = type;
      }
    }
    if (sector == sr)
      break;
    if (--sector < 0)
      sector = ObstaclesModel::numOfSectors-1;
  } while (true);
}

---