Modules/BallLocator/GTCamBallLocator.cpp

Go to the documentation of this file.

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

/*
GTCamBallLocator:

The GTCam solution for the module BallLocator uses the ball-position and -speed of the GTCamWorldState. For calculation of the ballState the own position determined by robotPose is used. To use the information of the GTCam for the own pose, you have to switch the SelfLocator to GTCam!

These attributes of ballModel are calculated:
  - seen: set to gtCamWorldState.getBallModel().seen
    Attention: gtCamWorldState.getBallModel() only sets:
      - seen.x
      - seen.y
      - seen.speed.x
      - seen.speed.y
  - seen.timeUntilSeenConsecutively: set to current system time
  - seen.timeWhenLastSeen: set to current system time
  - seen.timeWhenFirstSeenConsecutively: set to the system time of executing the constructor of GTCamBallLocator
  - seen.ballInFrontOfOpponentGoal: set true, if the ball is seen in front of the opponent goal, else false
    remark: this calculation is done by code copied from GT2004
    Attention: this calculation is only based on the ballPercept and landmarksPercept and NOT on GTCam information!!!
  - ballState.ballRollsByLeft: set true, if the ball will cut the y-axis of the robot in the range (100.0..500.0), else false
  - ballState.ballRollsByRight: set true, if the ball will cut the y-axis of the robot in the range (-500.0..-100.0), else false
  - ballState.ballRollsTowardsRobot: set true, if the ball will cut the y-axis of the robot in the range [-100.0..100.0], else false
  - ballState.projectedDistanceOnYAxis: distance in mm where the ball will cut the y-axis of the robot
    remark: calculated by the expression: y_rel - (vy_rel/vx_rel)*x_rel; where y_rel and x_rel is the ball position relative to the robot, vy_rel and vx_rel the ball speed relative to the robot but not considering the speed of the robot until now!
  - ballState.timeBallCrossesYAxis: time in ms which the ball needs to cross the robots y axis
    remark: calculated by the expression: (long)(-1000.0 * x_rel / vx_rel);
    Attention: calculated in GT2004 by the expression: (long)(ballModel.ballState.projectedDistanceOnYAxis / vx_rel) * 1000; // mm/s to mm/ms
      There are two errors in the GT2004 code:
        - the long cast is done before multiplying with 1000!
        - why ballModel.ballState.projectedDistanceOnYAxis and not x_rel?
      Bad initialization in GT2004: if vx_rel > -50 (if the ball does not roll towards the robot with more than 50 mm/s), ballState.timeBallCrossesYAxis is set to 100 by ballModel.ballState.reset()!! This solution initializes with 20000!! ( 1000mm/(50mm/s) = 20s )
  - frameNumber: set to cameraMatrix.frameNumber
  - numberOfImagesWithBallPercept: incremented, initialized with 0 in the constructor of GTCamBallLocator

These attributes of ballModel are set once in the constructor of GTCamSelfLocator:
  - ballWasSeen: set to true
  - motionValidity: set to 1.0
  - validity: set to 1.0
  - numberOfImagesWithoutBallPercept: set to 0

These attributes of ballModel are not calculated:
  - seen.angleError: set by the constructor of SeenBallPosition to 0
  - seen.distanceError: set by the constructor of SeenBallPosition to 0
  - ballState.ballRollsFast: set by ballState.reset() to false every frame; not set anywhere else in the whole code!

These attributes of ballModel are not set:
  - hypothetical
*/

#include "GTCamBallLocator.h"
#include "Platform/SystemCall.h"
#include "Tools/Player.h"

//#define MyGTCamBallLocatorDebug // generates ball roling to own goal

GTCamBallLocator::GTCamBallLocator(const BallLocatorInterfaces& interfaces)
: BallLocator(interfaces)
{
  ballModel.ballWasSeen = true;
  ballModel.numberOfImagesWithBallPercept = 0;
  ballModel.numberOfImagesWithoutBallPercept = 0;
  startTime = SystemCall::getCurrentSystemTime();
}

void GTCamBallLocator::execute()
{
#ifdef MyGTCamBallLocatorDebug
  // DEBUG(1/2)
  Vector2<double> old;
  old.x = ballModel.seen.x;
  old.y = ballModel.seen.y;
#endif

  ballModel.seen = gtCamWorldState.getBallModel().seen;
  ballModel.seen.timeUntilSeenConsecutively = ballModel.seen.timeWhenLastSeen = 
    SystemCall::getCurrentSystemTime();
  ballModel.seen.timeWhenFirstSeenConsecutively = startTime;

#ifdef MyGTCamBallLocatorDebug
  // DEBUG(2/2)
  if (old.x < -2000) {
    ballModel.seen.x = 0;
    ballModel.seen.y = 0;
  } else {
    ballModel.seen.x = old.x - 300;
    ballModel.seen.y = old.y + random(256) - 128;
  }
  ballModel.seen.speed.x = (ballModel.seen.x - old.x) * 2;
  ballModel.seen.speed.y = (ballModel.seen.y - old.y) * 2;
#endif

  // calculate ballModel.seen.ballInFrontOfOpponentGoal based on GTCam data; see GT2004BallLocator
  ballModel.seen.ballInFrontOfOpponentGoal = false;
  if (robotPose.translation.x < xPosOpponentGroundline && ballModel.seen.positionField.x > robotPose.translation.x)
  {
    double ballrelx = ballModel.seen.positionField.x - robotPose.translation.x;
    double ballrely = ballModel.seen.positionField.y - robotPose.translation.y;
    if (ballrelx * ballrelx + ballrely * ballrely < 1100 * 1100)
    {
      double angleToBall = atan(ballrely / ballrelx);
      double dx = xPosOpponentGoalpost - robotPose.translation.x;
      if (atan((yPosLeftGoal - robotPose.translation.y) / dx) > angleToBall)
        if (atan((yPosRightGoal - robotPose.translation.y) / dx) < angleToBall)
          ballModel.seen.ballInFrontOfOpponentGoal = true;
    }
  }

  ballModel.propagated.setBallDataRelativeToRobot(robotPose,
                                                  ballModel.seen.positionRobot.x,
                                                  ballModel.seen.positionRobot.y,
                                                  ballModel.seen.speedRobot.x,
                                                  ballModel.seen.speedRobot.y);

  ballModel.communicated.setBallDataRelativeToRobot(robotPose,
                                                    ballModel.seen.positionRobot.x,
                                                    ballModel.seen.positionRobot.y,
                                                    ballModel.seen.speedRobot.x,
                                                    ballModel.seen.speedRobot.y);

  if (!ballModel.ballWasSeen)
  {
    INFO(sendOptionRatings, idText, text, "Ball not seen by GTCam.");
  }
  
  ballModel.frameNumber = cameraMatrix.frameNumber;
  ballModel.numberOfImagesWithBallPercept++;
}

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

---