Modules/PlayersLocator/GT2004PlayersLocator.cpp

Go to the documentation of this file.

/**
* @file GT2004PlayersLocator.cpp
* 
* This file contains a class for players localization.
*/

#include "GT2004PlayersLocator.h"

#include "Tools/FieldDimensions.h"

#include "Tools/Player.h"

#include "Tools/Debugging/Debugging.h"

#include "Platform/SystemCall.h"

const double GT2004PlayersLocator::TEAM_MESSAGE_VALIDITY = 0.3;
const double GT2004PlayersLocator::MIN_MATCH_DIST = 500;

GT2004PlayersLocator::GT2004PlayersLocator(const PlayersLocatorInterfaces& interfaces)
: PlayersLocator(interfaces),
  opponentPointsWithValidityAndAge(NUMBER_OF_POINTS_FOR_OPPONENT_PLAYERS, NUMBER_OF_OPPONENT_PLAYERS_TO_LOCATE),
  ownPointsWithValidityAndAge(NUMBER_OF_POINTS_FOR_OWN_PLAYERS, NUMBER_OF_OWN_PLAYERS_TO_LOCATE)
{
}

void GT2004PlayersLocator::execute()
{
  Player::teamColor ownColor = getPlayer().getTeamColor();

  // Own perceptes
  
  if(ownColor == Player::red)
  {
    addOppPlayerPercepts(
      playersPercept.bluePlayers,
      playersPercept.numberOfBluePlayers,
      robotPose,
      1.0,
      SystemCall::getCurrentSystemTime());
    
    addOwnPlayerPercepts(
      playersPercept.redPlayers,
      playersPercept.numberOfRedPlayers,
      robotPose,
      1.0,
      SystemCall::getCurrentSystemTime());
  }
  else
  {
    addOwnPlayerPercepts(
      playersPercept.bluePlayers,
      playersPercept.numberOfBluePlayers,
      robotPose,
      1.0,
      SystemCall::getCurrentSystemTime());
    
    addOppPlayerPercepts(
      playersPercept.redPlayers,
      playersPercept.numberOfRedPlayers,
      robotPose,
      1.0,
      SystemCall::getCurrentSystemTime());
  }
  
  // Teammessages
  int t;

  for(t=0; t<teamMessageCollection.numberOfTeamMessages; t++)
  {
    if (teamMessageCollection[t].sendRobotPose)
    {
      ownPointsWithValidityAndAge.addPoint(
        (int) teamMessageCollection[t].robotPose.getPose().translation.x,
        (int) teamMessageCollection[t].robotPose.getPose().translation.y,
        teamMessageCollection[t].robotPose.getValidity(),
        teamMessageCollection[t].getTimeStampInOwnTime());
    }
  }
  
  int i, x, y;
  double validity;

  // own  
  ownPointsWithValidityAndAge.searchMaxima();
  playerPoseCollection.numberOfOwnPlayers = 0;
  for(i=0; i < NUMBER_OF_OWN_PLAYERS_TO_LOCATE; i++)
  {
    if(ownPointsWithValidityAndAge.getMaximum(i, x, y, validity))
    {
      playerPoseCollection.setOwnPlayerPose(i, x, y, 0, validity);
      playerPoseCollection.numberOfOwnPlayers++;
    }
  }
  
  // opponent
  opponentPointsWithValidityAndAge.searchMaxima();
  
  playerPoseCollection.numberOfOpponentPlayers = 0;
  for(i=0; i < NUMBER_OF_OPPONENT_PLAYERS_TO_LOCATE; i++)
  {
    if(opponentPointsWithValidityAndAge.getMaximum(i, x, y, validity))
    {
      playerPoseCollection.setOpponentPlayerPose(i, x, y, 0, validity);
      playerPoseCollection.numberOfOpponentPlayers++;
    }
  }
  
  int bestMatch;
  double currDist, nearestDist;

  for(i=0; i < playerPoseCollection.numberOfOwnPlayers; i++)
  {
    bestMatch = -1;
    nearestDist = MIN_MATCH_DIST;
    
    for(t=0; t<teamMessageCollection.numberOfTeamMessages; t++)
    {
      currDist = (playerPoseCollection.getOwnPlayerPose(i).getPose().translation - teamMessageCollection[t].robotPose.getPose().translation).abs();
      if (currDist < nearestDist)
      {
        bestMatch = t;
        nearestDist = currDist;
      }
    }

    if (bestMatch != -1)
    {
      playerPoseCollection.setOwnPlayerPose(i,
        playerPoseCollection.getOwnPlayerPose(i).getPose().translation.x,
        playerPoseCollection.getOwnPlayerPose(i).getPose().translation.y,
        teamMessageCollection[bestMatch].robotPose.getPose().getAngle(),
        playerPoseCollection.getOwnPlayerPose(i).getValidity());    
    }
  }

}

bool GT2004PlayersLocator::correctObstaclePosition
(int oldX, int oldY, int &newX, int &newY)
{
  newX = oldX;
  newY = oldY;
  bool isCorrected = false;
  
  if(newY > yPosLeftSideline)
  {
    //correct y_coordinate
    newY = yPosLeftSideline;
    //compute x_coordinate
    newX = (int)(((double)oldX / (double)oldY) * (double)newY);
    oldX = newX;
    oldY = newY;
    isCorrected = true;
  }
  else if(newY < yPosRightSideline)
  {
    //correct y_coordinate
    newY = yPosRightSideline;
    //compute x_coordinate
    newX = (int)(((double)oldX / (double)oldY) * (double)newY);
    oldX = newX;
    oldY = newY;
    isCorrected = true;
  }
  if(newX > xPosOpponentGroundline)
  {
    //correct x-coordinate
    newX = xPosOpponentGroundline;
    //compute y-coordinate
    newY = (int)(((double)newX / (double)oldX) * (double)oldY);
    oldX = newX;
    oldY = newY;
    isCorrected = true;
  }
  else if(newX < xPosOwnGroundline)
  {
    //correct x-coordinate
    newX = xPosOwnGroundline;
    //compute y-coordinate
    newY = (int)(((double)newX / (double)oldX) * (double)oldY);
    oldX = newX;
    oldY = newY;
    isCorrected = true;
  }
  return (isCorrected);
}

void GT2004PlayersLocator::addOwnPlayerPercepts(
    const SinglePlayerPercept playerPercepts[],
    int numberOfPlayerPercepts,
    const RobotPose& startRobotPose,
    const double collectionValidity,
    const unsigned long collectionTimestamp)
{
  Vector2<double> p;
  int playerPositionX = 0,
    playerPositionY = 0;
  int correctedPlayerPositionX = 0,
    correctedPlayerPositionY = 0;
  
  for(int players=0; players < numberOfPlayerPercepts; players++)
  {
    p = (startRobotPose.getPose() + 
      Pose2D(playerPercepts[players].offset)).translation;
    
    playerPositionX = (int)p.x;
    playerPositionY = (int)p.y;
    
    correctObstaclePosition(
      playerPositionX, playerPositionY, 
      correctedPlayerPositionX, correctedPlayerPositionY);
    
    ownPointsWithValidityAndAge.addPoint(
      correctedPlayerPositionX,
      correctedPlayerPositionY,
      playerPercepts[players].validity * startRobotPose.getValidity() * collectionValidity,
      collectionTimestamp);
  }
}

void GT2004PlayersLocator::addOppPlayerPercepts(
    const SinglePlayerPercept playerPercepts[],
    int numberOfPlayerPercepts,
    const RobotPose& startRobotPose,
    const double collectionValidity,
    const unsigned long collectionTimestamp)
{
  Vector2<double> p;
  int playerPositionX = 0,
    playerPositionY = 0;
  int correctedPlayerPositionX = 0,
    correctedPlayerPositionY = 0;
  
  for(int players=0; players < numberOfPlayerPercepts; players++)
  {
    p = (startRobotPose.getPose() + 
      Pose2D(playerPercepts[players].offset)).translation;
    
    playerPositionX = (int)p.x;
    playerPositionY = (int)p.y;
    
    correctObstaclePosition(
      playerPositionX, playerPositionY, 
      correctedPlayerPositionX, correctedPlayerPositionY);
    
    opponentPointsWithValidityAndAge.addPoint(
      correctedPlayerPositionX,
      correctedPlayerPositionY,
      playerPercepts[players].validity * startRobotPose.getValidity() * collectionValidity,
      collectionTimestamp);
  }
}

/*
* Change log :
* 
* $Log: GT2004PlayersLocator.cpp,v $
* Revision 1.1  2004/07/10 00:18:31  spranger
* renamed (readded) for coderelease
*
* Revision 1.1.1.1  2004/05/22 17:20:39  cvsadm
* created new repository GT2004_WM
*
* Revision 1.2  2004/03/08 01:07:15  roefer
* Interfaces should be const
*
* Revision 1.1  2003/10/06 14:10:15  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.2  2003/07/02 19:14:53  loetzsch
* cleaned up team message collection
*
* Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.8  2003/05/27 12:48:32  mkunz
* parameter tuning
*
* Revision 1.7  2003/05/27 09:06:39  mkunz
* adapted to changes in VAPoints
*
* Revision 1.6  2003/05/16 14:50:13  mkunz
* more points again
*
* Revision 1.5  2003/05/14 13:19:57  mkunz
* bigger reorganization
*
* Revision 1.4  2003/05/13 14:31:16  mkunz
* full transfer toVAPoints
*
* Revision 1.3  2003/05/08 19:51:23  mkunz
* switched to VAPoints
*
* Revision 1.2  2003/04/15 15:52:09  risler
* DDD GO 2003 code integrated
*
* Revision 1.3  2003/04/12 17:01:27  mkunz
* back again
*
* Revision 1.2  2003/03/31 18:17:43  max
* tamara update
*
* Revision 1.1  2003/03/29 12:51:03  mkunz
* just a copy of GT2001
*
*
*/

---