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

# Modules/BallLocator/GTCamBallLocator.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GTCamBallLocator.cpp
    00003 *
    00004 * This file contains a class for ball-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 GTCamBallLocator:
    00011
    00012 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!
    00013
    00014 These attributes of ballModel are calculated:
    00015   - seen: set to gtCamWorldState.getBallModel().seen
    00016     Attention: gtCamWorldState.getBallModel() only sets:
    00017       - seen.x
    00018       - seen.y
    00019       - seen.speed.x
    00020       - seen.speed.y
    00021   - seen.timeUntilSeenConsecutively: set to current system time
    00022   - seen.timeWhenLastSeen: set to current system time
    00023   - seen.timeWhenFirstSeenConsecutively: set to the system time of executing the constructor of GTCamBallLocator
    00024   - seen.ballInFrontOfOpponentGoal: set true, if the ball is seen in front of the opponent goal, else false
    00025     remark: this calculation is done by code copied from GT2004
    00026     Attention: this calculation is only based on the ballPercept and landmarksPercept and NOT on GTCam information!!!
    00027   - ballState.ballRollsByLeft: set true, if the ball will cut the y-axis of the robot in the range (100.0..500.0), else false
    00028   - ballState.ballRollsByRight: set true, if the ball will cut the y-axis of the robot in the range (-500.0..-100.0), else false
    00029   - ballState.ballRollsTowardsRobot: set true, if the ball will cut the y-axis of the robot in the range [-100.0..100.0], else false
    00030   - ballState.projectedDistanceOnYAxis: distance in mm where the ball will cut the y-axis of the robot
    00031     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!
    00032   - ballState.timeBallCrossesYAxis: time in ms which the ball needs to cross the robots y axis
    00033     remark: calculated by the expression: (long)(-1000.0 * x_rel / vx_rel);
    00034     Attention: calculated in GT2004 by the expression: (long)(ballModel.ballState.projectedDistanceOnYAxis / vx_rel) * 1000; // mm/s to mm/ms
    00035       There are two errors in the GT2004 code:
    00036         - the long cast is done before multiplying with 1000!
    00037         - why ballModel.ballState.projectedDistanceOnYAxis and not x_rel?
    00038       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 )
    00039   - frameNumber: set to cameraMatrix.frameNumber
    00040   - numberOfImagesWithBallPercept: incremented, initialized with 0 in the constructor of GTCamBallLocator
    00041
    00042 These attributes of ballModel are set once in the constructor of GTCamSelfLocator:
    00043   - ballWasSeen: set to true
    00044   - motionValidity: set to 1.0
    00045   - validity: set to 1.0
    00046   - numberOfImagesWithoutBallPercept: set to 0
    00047
    00048 These attributes of ballModel are not calculated:
    00049   - seen.angleError: set by the constructor of SeenBallPosition to 0
    00050   - seen.distanceError: set by the constructor of SeenBallPosition to 0
    00051   - ballState.ballRollsFast: set by ballState.reset() to false every frame; not set anywhere else in the whole code!
    00052
    00053 These attributes of ballModel are not set:
    00054   - hypothetical
    00055 */
    00056
    00057 #include "GTCamBallLocator.h"
    00058 #include "Platform/SystemCall.h"
    00059 #include "Tools/Player.h"
    00060
    00061 //#define MyGTCamBallLocatorDebug // generates ball roling to own goal
    00062
    00063 GTCamBallLocator::GTCamBallLocator(const BallLocatorInterfaces& interfaces)
    00064 : BallLocator(interfaces)
    00065 {
    00066   ballModel.ballWasSeen = true;
    00067   ballModel.numberOfImagesWithBallPercept = 0;
    00068   ballModel.numberOfImagesWithoutBallPercept = 0;
    00069   startTime = SystemCall::getCurrentSystemTime();
    00070 }
    00071
    00072 void GTCamBallLocator::execute()
    00073 {
    00074 #ifdef MyGTCamBallLocatorDebug
    00075   // DEBUG(1/2)
    00076   Vector2<double> old;
    00077   old.x = ballModel.seen.x;
    00078   old.y = ballModel.seen.y;
    00079 #endif
    00080
    00081   ballModel.seen = gtCamWorldState.getBallModel().seen;
    00082   ballModel.seen.timeUntilSeenConsecutively = ballModel.seen.timeWhenLastSeen =
    00083     SystemCall::getCurrentSystemTime();
    00084   ballModel.seen.timeWhenFirstSeenConsecutively = startTime;
    00085
    00086 #ifdef MyGTCamBallLocatorDebug
    00087   // DEBUG(2/2)
    00088   if (old.x < -2000) {
    00089     ballModel.seen.x = 0;
    00090     ballModel.seen.y = 0;
    00091   } else {
    00092     ballModel.seen.x = old.x - 300;
    00093     ballModel.seen.y = old.y + random(256) - 128;
    00094   }
    00095   ballModel.seen.speed.x = (ballModel.seen.x - old.x) * 2;
    00096   ballModel.seen.speed.y = (ballModel.seen.y - old.y) * 2;
    00097 #endif
    00098
    00099   // calculate ballModel.seen.ballInFrontOfOpponentGoal based on GTCam data; see GT2004BallLocator
    00100   ballModel.seen.ballInFrontOfOpponentGoal = false;
    00101   if (robotPose.translation.x < xPosOpponentGroundline && ballModel.seen.positionField.x > robotPose.translation.x)
    00102   {
    00103     double ballrelx = ballModel.seen.positionField.x - robotPose.translation.x;
    00104     double ballrely = ballModel.seen.positionField.y - robotPose.translation.y;
    00105     if (ballrelx * ballrelx + ballrely * ballrely < 1100 * 1100)
    00106     {
    00107       double angleToBall = atan(ballrely / ballrelx);
    00108       double dx = xPosOpponentGoalpost - robotPose.translation.x;
    00109       if (atan((yPosLeftGoal - robotPose.translation.y) / dx) > angleToBall)
    00110         if (atan((yPosRightGoal - robotPose.translation.y) / dx) < angleToBall)
    00111           ballModel.seen.ballInFrontOfOpponentGoal = true;
    00112     }
    00113   }
    00114
    00115   ballModel.propagated.setBallDataRelativeToRobot(robotPose,
    00116                                                   ballModel.seen.positionRobot.x,
    00117                                                   ballModel.seen.positionRobot.y,
    00118                                                   ballModel.seen.speedRobot.x,
    00119                                                   ballModel.seen.speedRobot.y);
    00120
    00121   ballModel.communicated.setBallDataRelativeToRobot(robotPose,
    00122                                                     ballModel.seen.positionRobot.x,
    00123                                                     ballModel.seen.positionRobot.y,
    00124                                                     ballModel.seen.speedRobot.x,
    00125                                                     ballModel.seen.speedRobot.y);
    00126
    00127   if (!ballModel.ballWasSeen)
    00128   {
    00129     INFO(sendOptionRatings, idText, text, "Ball not seen by GTCam.");
    00130   }
    00131
    00132   ballModel.frameNumber = cameraMatrix.frameNumber;
    00133   ballModel.numberOfImagesWithBallPercept++;
    00134 }
    00135
    00136 bool GTCamBallLocator::handleMessage(InMessage& message)
    00137 {
    00138   return false;
    00139 }