Modules/SelfLocator/GTCamSelfLocator.cpp

Go to the documentation of this file.

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

/*
GTCamSelfLocator:

The GTCam solution for the module SelfLocator uses the position and orintation of the GTCamWorldState for selflocalization.

These attributes of robotPose are calculated:
  - translation: the position of the robot in field-coordinates, set to the translation calculated by GTCam
    range: (double x double)
  - rotation: the rotation of the robot relative to the field, set to the rotation calculated by GTCam
    range: [-pi..pi) or (-pi..pi] or other ??
  - frameNumber: set to cameraMatrix.frameNumber
  - timestamp: set to current system time
  - distanceToBorder: set to FieldDimensions::distanceToBorder(robotPose.translation)

These attributes of robotPose are set once in the constructor of GTCamSelfLocator:
  - validity: set to 1.0
  - angleToBorder: set to 0.777
    remark: set only once in the whole code, see Src\Modules\SelfLocator\DistanceToBorderEstimator.cpp(38):  robotPose.angleToBorder = 0.777;
  - speedbyDistanceToGoal: set to 0.0
    remark: calculated only once in the whole code, only by SpecialPerceptSelfLocator.cpp(233)

Proceeding:
  The main attributes are taken from the GTCamWorldState; frameNumber and timestamp are set; distanceToBorder is calculated using a complex method from FieldDimensions.
*/

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

//#define GTCamSelfLocatorDebug // generates moving position of the robot

GTCamSelfLocator::GTCamSelfLocator(const SelfLocatorInterfaces& interfaces) : SelfLocator(interfaces) {
  robotPose.setValidity(1.0);
  robotPose.speedbyDistanceToGoal = 0.0;  // only calculated by SpecialPerceptSelfLocator
  lastPose.setValidity(0.0);
  lastPose.translation.x = 0.0;
  lastPose.translation.y = 0.0;
}

void GTCamSelfLocator::execute() {
  int me = getPlayer().getPlayerNumber();
  int color = getPlayer().getTeamColor();

/*  robotPose.translation = gtCamWorldState.getPlayerPos(me, color);
  robotPose.rotation = gtCamWorldState.getPlayerOrientation(me, color);
  robotPose.frameNumber = cameraMatrix.frameNumber;
  robotPose.timestamp = SystemCall::getCurrentSystemTime();*/
  if (gtCamWorldState.playerWasSeen(me, color))
  {
    robotPose = gtCamWorldState.getRobotPose(me, color);
    robotPose.frameNumber = cameraMatrix.frameNumber;
    robotPose.timestamp = SystemCall::getCurrentSystemTime();

    lastPose = robotPose;
    //OUTPUT (idText, text, "GTCamSLSeen: " << robotPose.translation.x << ", " << robotPose.translation.y << ", " << robotPose.rotation << ", " << robotPose.getValidity());
  }
  else
  {
    unsigned long timeSinceLastSeen = SystemCall::getCurrentSystemTime() - lastPose.timestamp;
    if (timeSinceLastSeen < 5000)
    {
      lastPose.setValidity(1.0 - ((double)(timeSinceLastSeen))/5000.0);
      //OUTPUT (idText, text, "GTCamSLFalling: " << robotPose.translation.x << ", " << robotPose.translation.y << ", " << robotPose.rotation << ", " << robotPose.getValidity());
    }
    else
    {
      lastPose.setValidity(0.0);
      //OUTPUT (idText, text, "GTCamSLDown: " << robotPose.translation.x << ", " << robotPose.translation.y << ", " << robotPose.rotation << ", " << robotPose.getValidity());
    }
    robotPose = lastPose;
  }

#ifdef GTCamSelfLocatorDebug
  // DEBUG(1/1)
  robotPose.translation.x = 2000*cos(robotPose.timestamp/(16*1024.0));
  robotPose.translation.y = 1000*sin(robotPose.timestamp/(16*713.0));
  robotPose.rotation = 1.0;
#endif
}  

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

---