Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/SelfLocator/GTCamSelfLocator.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file Modules/SelfLocator/GTCamSelfLocator.cpp
    00003 *
    00004 * This file contains a class for self-localization using the worldstate of the GTCam.
    00005 *
    00006 * @author <a href="mailto:timo.diekmann@uni-dortmund.de">Timo Diekmann</a>
    00007 */
    00008
    00009 /*
    00010 GTCamSelfLocator:
    00011
    00012 The GTCam solution for the module SelfLocator uses the position and orintation of the GTCamWorldState for selflocalization.
    00013
    00014 These attributes of robotPose are calculated:
    00015   - translation: the position of the robot in field-coordinates, set to the translation calculated by GTCam
    00016     range: (double x double)
    00017   - rotation: the rotation of the robot relative to the field, set to the rotation calculated by GTCam
    00018     range: [-pi..pi) or (-pi..pi] or other ??
    00019   - frameNumber: set to cameraMatrix.frameNumber
    00020   - timestamp: set to current system time
    00021   - distanceToBorder: set to FieldDimensions::distanceToBorder(robotPose.translation)
    00022
    00023 These attributes of robotPose are set once in the constructor of GTCamSelfLocator:
    00024   - validity: set to 1.0
    00025   - angleToBorder: set to 0.777
    00026     remark: set only once in the whole code, see Src\Modules\SelfLocator\DistanceToBorderEstimator.cpp(38):  robotPose.angleToBorder = 0.777;
    00027   - speedbyDistanceToGoal: set to 0.0
    00028     remark: calculated only once in the whole code, only by SpecialPerceptSelfLocator.cpp(233)
    00029
    00030 Proceeding:
    00031   The main attributes are taken from the GTCamWorldState; frameNumber and timestamp are set; distanceToBorder is calculated using a complex method from FieldDimensions.
    00032 */
    00033
    00034 #include "GTCamSelfLocator.h"
    00035 #include "Tools/Player.h"
    00036 #include "Platform/SystemCall.h"
    00037 #include "Tools/FieldDimensions.h"
    00038
    00039 //#define GTCamSelfLocatorDebug // generates moving position of the robot
    00040
    00041 GTCamSelfLocator::GTCamSelfLocator(const SelfLocatorInterfaces& interfaces) : SelfLocator(interfaces) {
    00042   robotPose.setValidity(1.0);
    00043   robotPose.speedbyDistanceToGoal = 0.0;  // only calculated by SpecialPerceptSelfLocator
    00044   lastPose.setValidity(0.0);
    00045   lastPose.translation.x = 0.0;
    00046   lastPose.translation.y = 0.0;
    00047 }
    00048
    00049 void GTCamSelfLocator::execute() {
    00050   int me = getPlayer().getPlayerNumber();
    00051   int color = getPlayer().getTeamColor();
    00052
    00053 /*  robotPose.translation = gtCamWorldState.getPlayerPos(me, color);
    00054   robotPose.rotation = gtCamWorldState.getPlayerOrientation(me, color);
    00055   robotPose.frameNumber = cameraMatrix.frameNumber;
    00056   robotPose.timestamp = SystemCall::getCurrentSystemTime();*/
    00057   if (gtCamWorldState.playerWasSeen(me, color))
    00058   {
    00059     robotPose = gtCamWorldState.getRobotPose(me, color);
    00060     robotPose.frameNumber = cameraMatrix.frameNumber;
    00061     robotPose.timestamp = SystemCall::getCurrentSystemTime();
    00062
    00063     lastPose = robotPose;
    00064     //OUTPUT (idText, text, "GTCamSLSeen: " << robotPose.translation.x << ", " << robotPose.translation.y << ", " << robotPose.rotation << ", " << robotPose.getValidity());
    00065   }
    00066   else
    00067   {
    00068     unsigned long timeSinceLastSeen = SystemCall::getCurrentSystemTime() - lastPose.timestamp;
    00069     if (timeSinceLastSeen < 5000)
    00070     {
    00071       lastPose.setValidity(1.0 - ((double)(timeSinceLastSeen))/5000.0);
    00072       //OUTPUT (idText, text, "GTCamSLFalling: " << robotPose.translation.x << ", " << robotPose.translation.y << ", " << robotPose.rotation << ", " << robotPose.getValidity());
    00073     }
    00074     else
    00075     {
    00076       lastPose.setValidity(0.0);
    00077       //OUTPUT (idText, text, "GTCamSLDown: " << robotPose.translation.x << ", " << robotPose.translation.y << ", " << robotPose.rotation << ", " << robotPose.getValidity());
    00078     }
    00079     robotPose = lastPose;
    00080   }
    00081
    00082 #ifdef GTCamSelfLocatorDebug
    00083   // DEBUG(1/1)
    00084   robotPose.translation.x = 2000*cos(robotPose.timestamp/(16*1024.0));
    00085   robotPose.translation.y = 1000*sin(robotPose.timestamp/(16*713.0));
    00086   robotPose.rotation = 1.0;
    00087 #endif
    00088 }
    00089
    00090 bool GTCamSelfLocator::handleMessage(InMessage& message) {
    00091   return false;
    00092 }