GT2004BallLocator Class Reference

#include <GT2004BallLocator.h>

Inheritance diagram for GT2004BallLocator:

BallLocator Module BallLocatorInterfaces MessageHandler List of all members.

Public Member Functions

 GT2004BallLocator (BallLocatorInterfaces &interfaces)
virtual ~GT2004BallLocator ()
virtual void execute ()
virtual bool handleMessage (InMessage &message)

Private Member Functions

void determineNumberOfImagesWith_WithoutBall ()
void handleSeenBall ()
void handleUnseenBall ()
void compensateOdometry ()
void setBallState ()
void setBallStateV2 ()
void setUnknownResult ()
bool isNonSensePos (double x, double y)
void addModel (KalmanProcessModelBase *pModel)
void destroyModels ()
void sendProcessModelStates ()
void drawBallPosition ()
Vector2< double > relative2FieldCoord (RobotPose rp, double x, double y)

Private Attributes

unsigned long int lastFrameNumber
bool ballWasSeenInLastImage
std::vector< KalmanProcessModelBase * > kalmanModels
 Vector containing all instantiated process models.
std::vector< KalmanUpdateResultkalmanUpdateResults
std::vector< KalmanPredictResultkalmanPredictResults
double x_rel
 x-position of ball relative to robot
double y_rel
 y-position relative to robot
double x_abs
 x-position of ball on field
double y_abs
 y-position of ball on field
double vx_rel
 speed of ball in x-direction relative to robot
double vy_rel
 speed of ball in y-direction relative to robot
double vx_abs
 speed of ball in x-direction on field
double vy_abs
 speed of ball in y-direction on field
bool ballWasSeen
 true if ball was seen
double last_seen_x_rel
 x-position of last seen ball relative to robot
double last_seen_y_rel
 y-position of last seen ball relative to robot
OdometryData lastOdometry
 Last odometry data of robot.
bool bSendProcessModelStates
 If true, process model states are sent as debug messages.
bool freshDefend
Vector2< double > ballSensedPos
Vector2< double > ballSensedRelPos
unsigned long ballSensedTime
CameraMatrix prevCameraMatrix
Vector2< double > lastBallSeen
Pose2D lastRobotOdometry

Static Private Attributes

static double dFieldDiagonalLength
 1.2*length of field diagonal

Detailed Description

A BallLocator using GT2004 filtering. This class is the main class managing different GT2004 process models

Definition at line 24 of file GT2004BallLocator.h.


Constructor & Destructor Documentation

GT2004BallLocator::GT2004BallLocator ( BallLocatorInterfaces interfaces  ) 

Definition at line 22 of file GT2004BallLocator.cpp.

References addModel(), ballSensedPos, ballSensedRelPos, ballSensedTime, ballWasSeen, ballWasSeenInLastImage, bSendProcessModelStates, freshDefend, last_seen_x_rel, last_seen_y_rel, vx_abs, vx_rel, vy_abs, vy_rel, Vector2< V >::x, x_abs, x_rel, Vector2< V >::y, y_abs, and y_rel.

GT2004BallLocator::~GT2004BallLocator (  )  [virtual]

Definition at line 59 of file GT2004BallLocator.cpp.

References destroyModels().


Member Function Documentation

void GT2004BallLocator::addModel ( KalmanProcessModelBase pModel  )  [private]

Adds a process model to the list of managed process models.

Parameters:
pModel Pointer to process model derived from GT2004ProcessModelBase

Definition at line 604 of file GT2004BallLocator.cpp.

References kalmanModels, kalmanPredictResults, and kalmanUpdateResults.

Referenced by GT2004BallLocator().

void GT2004BallLocator::compensateOdometry (  )  [private]

Compensates the body movements

Definition at line 446 of file GT2004BallLocator.cpp.

References last_seen_x_rel, last_seen_y_rel, lastOdometry, BallLocatorInterfaces::odometryData, Vector2< V >::x, and Vector2< V >::y.

Referenced by execute().

void GT2004BallLocator::destroyModels (  )  [private]

Frees the managed process models

Definition at line 612 of file GT2004BallLocator.cpp.

References kalmanModels.

Referenced by ~GT2004BallLocator().

void GT2004BallLocator::determineNumberOfImagesWith_WithoutBall (  )  [private]

Counts the number of percepts that contained a ball or no ball

Definition at line 793 of file GT2004BallLocator.cpp.

References BallLocatorInterfaces::ballModel, BallLocatorInterfaces::ballPercept, BallModel::ballWasSeen, BallPercept::ballWasSeen, ballWasSeenInLastImage, lastFrameNumber, BallModel::numberOfImagesWithBallPercept, and BallModel::numberOfImagesWithoutBallPercept.

Referenced by execute().

void GT2004BallLocator::drawBallPosition (  )  [private]

Draws the global ball positions and data model ball positions on the field

Definition at line 741 of file GT2004BallLocator.cpp.

References ballWasSeen, Drawings::blue, CIRCLE, DEBUG_DRAWING_FINISHED, kalmanModels, kalmanPredictResults, kalmanUpdateResults, LINE, Drawings::orange, Geometry::relative2FieldCoord(), BallLocatorInterfaces::robotPose, Drawings::skyblue, vx_abs, vy_abs, Vector2< V >::x, x_abs, Vector2< V >::y, y_abs, and Drawings::yellowOrange.

Referenced by execute().

void GT2004BallLocator::execute (  )  [virtual]

Executes the GT2004BallLocator module

Implements Module.

Definition at line 65 of file GT2004BallLocator.cpp.

References SeenBallPosition::angleError, SeenBallPosition::ballInFrontOfOpponentGoal, BallLocatorInterfaces::ballModel, BallLocatorInterfaces::ballPercept, ballWasSeen, BallPercept::ballWasSeen, bSendProcessModelStates, BallLocatorInterfaces::cameraMatrix, Goal::color, compensateOdometry(), determineNumberOfImagesWith_WithoutBall(), SeenBallPosition::distanceError, drawBallPosition(), BallModel::frameNumber, CameraMatrix::frameNumber, BallPercept::getAngleSizeBased(), BallPercept::getDistance(), BallPercept::getDistanceSizeBased(), BallPercept::getOffset(), getPlayer(), Player::getTeamColor(), LandmarksPercept::goals, handleSeenBall(), handleUnseenBall(), BallLocatorInterfaces::landmarksPercept, lastBallSeen, lastRobotOdometry, Range< T >::max, Range< T >::min, LandmarksPercept::numberOfGoals, BallLocatorInterfaces::odometryData, openingAngleHeight_ERS210, prevCameraMatrix, Player::red, Geometry::relative2FieldCoord(), BallLocatorInterfaces::robotPose, BallModel::seen, sendProcessModelStates(), setBallState(), skyblue, Vector2< V >::x, Boundary< T >::x, Vector2< V >::y, and yellow.

bool GT2004BallLocator::handleMessage ( InMessage message  )  [virtual]

Handles idGT2004Data debug messages

Parameters:
message The message
Returns:
true if message was handled

Reimplemented from Module.

Definition at line 620 of file GT2004BallLocator.cpp.

References OutMessage::bin, InMessage::bin, OutMessage::finishMessage(), getDebugOut(), InMessage::getMessageID(), idGT2004BallLocatorData, GT2004BallLocatorDebugData::idRequestParameters, GT2004BallLocatorDebugData::idSendingParameters, GT2004BallLocatorDebugData::idSetParameters, kalmanModels, and InMessage::resetReadPosition().

void GT2004BallLocator::handleSeenBall (  )  [private]

Called if ball was seen. Performs update step.

Definition at line 152 of file GT2004BallLocator.cpp.

References BallLocatorInterfaces::ballModel, BallLocatorInterfaces::ballPercept, ballWasSeen, BallLocatorInterfaces::cameraMatrix, CameraMatrix::frameNumber, SystemCall::getCurrentSystemTime(), BallPercept::getOffset(), getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), handleUnseenBall(), idText, int(), isNonSensePos(), kalmanModels, kalmanUpdateResults, last_seen_x_rel, last_seen_y_rel, lastOdometry, RobotDimensions::motionCycleTime, normalize(), PropagatedBallPosition::observedSpeed, BallLocatorInterfaces::odometryData, OUTPUT, PropagatedBallPosition::positionWhenLastObserved, prevCameraMatrix, BallModel::propagated, Geometry::relative2FieldCoord(), BallLocatorInterfaces::robotPose, Pose2D::rotation, Pose3D::rotation, BallModel::seen, setUnknownResult(), SeenBallPosition::speed, text, PropagatedBallPosition::timeOfObservation, SeenBallPosition::timeUntilSeenConsecutively, SeenBallPosition::timeWhenFirstSeenConsecutively, SeenBallPosition::timeWhenLastSeen, vx_abs, vx_rel, vy_abs, vy_rel, Vector3< V >::x, Vector2< V >::x, x_abs, x_rel, Vector3< V >::y, Vector2< V >::y, y_abs, and y_rel.

Referenced by execute().

void GT2004BallLocator::handleUnseenBall (  )  [private]

Called if ball was not seen. Performs predict step.

Definition at line 330 of file GT2004BallLocator.cpp.

References BallLocatorInterfaces::ballModel, SystemCall::getCurrentSystemTime(), idText, int(), isNonSensePos(), kalmanModels, kalmanPredictResults, last_seen_x_rel, last_seen_y_rel, lastOdometry, PropagatedBallPosition::observedSpeed, BallLocatorInterfaces::odometryData, OUTPUT, PropagatedBallPosition::positionWhenLastObserved, BallModel::propagated, Geometry::relative2FieldCoord(), BallLocatorInterfaces::robotPose, Pose2D::rotation, BallModel::seen, setUnknownResult(), text, PropagatedBallPosition::timeOfObservation, vx_abs, vx_rel, vy_abs, vy_rel, Vector2< V >::x, x_abs, x_rel, Vector2< V >::y, y_abs, and y_rel.

Referenced by execute(), and handleSeenBall().

bool GT2004BallLocator::isNonSensePos ( double  x,
double  y 
) [private]

True if passed position is nonsense.

Parameters:
x x-position in millimeter
y y-position in millimeter
Returns:
true if position is nonsense

Definition at line 595 of file GT2004BallLocator.cpp.

References dFieldDiagonalLength, and dist().

Referenced by handleSeenBall(), and handleUnseenBall().

Vector2<double> GT2004BallLocator::relative2FieldCoord ( RobotPose  rp,
double  x,
double  y 
) [private]

void GT2004BallLocator::sendProcessModelStates (  )  [private]

Sends the process model states as debug messages

Definition at line 669 of file GT2004BallLocator.cpp.

References ballWasSeen, OutMessage::bin, OutMessage::finishMessage(), SystemCall::getCurrentSystemTime(), getDebugOut(), GT2004BallLocatorDebugData::idFilterState, idGT2004BallLocatorData, kalmanModels, kalmanPredictResults, kalmanUpdateResults, BallLocatorInterfaces::robotPose, vx_rel, vy_rel, x_rel, and y_rel.

Referenced by execute().

void GT2004BallLocator::setBallState (  )  [private]

Checks if the ball rolls by right or left of the robot

Definition at line 458 of file GT2004BallLocator.cpp.

References BallLocatorInterfaces::ballModel, BallState::ballRollsByLeft, BallState::ballRollsByRight, BallState::ballRollsTowardsRobot, BallModel::ballState, dist(), BallState::projectedDistanceOnYAxis, BallState::reset(), BallState::timeBallCrossesYAxis, vx_rel, vy_rel, x_rel, and y_rel.

Referenced by execute().

void GT2004BallLocator::setBallStateV2 (  )  [private]

Checks if the ball rolls by right or left of the robot, v2

Meanings of the constants:

If the ball is farer away than defendZone2Border from the robot then freshDefend is set to true. This is just a kind of hysteresis to reset the "sensing" for the ballStates.

To sense a ball state, the ball has to be detected first in a range smaller than defendZone2Border, and after that in a range smaller than defendZone1Border. The maximum time between the detections in zone2 and zone1 has to be smaller than speedTrigger (in ms).

The ballHasMovedThreshold is used to determine, if the ball has moved or the robot, since only relative ball positions are used. The difference between the absolute ball positions in zone2 and zone1 has to be greater than this threshold to identify the ball as moving, rather than the robot.

The ballIsFastThreshold is used to determine if the ball rolls fast. Value is mm/s.

Definition at line 494 of file GT2004BallLocator.cpp.

References abs(), Geometry::angleTo(), BallLocatorInterfaces::ballModel, BallState::ballRollsByLeft, BallState::ballRollsByRight, BallState::ballRollsTowardsRobot, ballSensedPos, ballSensedRelPos, ballSensedTime, BallModel::ballState, Geometry::distanceTo(), freshDefend, SystemCall::getCurrentSystemTime(), RobotPose::getPose(), BallState::projectedDistanceOnYAxis, BallState::reset(), BallLocatorInterfaces::robotPose, BallModel::seen, BallState::timeBallCrossesYAxis, toDegrees(), vx_rel, Vector2< V >::x, and Vector2< V >::y.

void GT2004BallLocator::setUnknownResult (  )  [private]

setUnknownResult is called if an exception occurs. However, this should not happen, but if, this prevents the robot from crashing.

Definition at line 588 of file GT2004BallLocator.cpp.

References BallLocatorInterfaces::ballModel, BallModel::propagated, Vector2< V >::x, x_abs, Vector2< V >::y, and y_abs.

Referenced by handleSeenBall(), and handleUnseenBall().


Member Data Documentation

Vector2<double> GT2004BallLocator::ballSensedPos [private]

Definition at line 135 of file GT2004BallLocator.h.

Referenced by GT2004BallLocator(), and setBallStateV2().

Vector2<double> GT2004BallLocator::ballSensedRelPos [private]

Definition at line 136 of file GT2004BallLocator.h.

Referenced by GT2004BallLocator(), and setBallStateV2().

unsigned long GT2004BallLocator::ballSensedTime [private]

Definition at line 137 of file GT2004BallLocator.h.

Referenced by GT2004BallLocator(), and setBallStateV2().

bool GT2004BallLocator::ballWasSeen [private]

true if ball was seen

Definition at line 119 of file GT2004BallLocator.h.

Referenced by drawBallPosition(), execute(), GT2004BallLocator(), handleSeenBall(), and sendProcessModelStates().

bool GT2004BallLocator::ballWasSeenInLastImage [private]

Definition at line 99 of file GT2004BallLocator.h.

Referenced by determineNumberOfImagesWith_WithoutBall(), and GT2004BallLocator().

bool GT2004BallLocator::bSendProcessModelStates [private]

If true, process model states are sent as debug messages.

Definition at line 127 of file GT2004BallLocator.h.

Referenced by execute(), and GT2004BallLocator().

double GT2004BallLocator::dFieldDiagonalLength [static, private]

Initial value:

  1.2*sqrt((2.0*xPosOpponentGoal)*(2.0*xPosOpponentGoal) +
           (2.0*yPosLeftFlags)*(2.0*yPosLeftFlags))
1.2*length of field diagonal

Definition at line 130 of file GT2004BallLocator.h.

Referenced by isNonSensePos().

bool GT2004BallLocator::freshDefend [private]

Definition at line 134 of file GT2004BallLocator.h.

Referenced by GT2004BallLocator(), and setBallStateV2().

std::vector<KalmanProcessModelBase*> GT2004BallLocator::kalmanModels [private]

Vector containing all instantiated process models.

Definition at line 102 of file GT2004BallLocator.h.

Referenced by addModel(), destroyModels(), drawBallPosition(), handleMessage(), handleSeenBall(), handleUnseenBall(), and sendProcessModelStates().

std::vector<KalmanPredictResult> GT2004BallLocator::kalmanPredictResults [private]

Vector containing the result of calls to the predict function of the process models

Definition at line 108 of file GT2004BallLocator.h.

Referenced by addModel(), drawBallPosition(), handleUnseenBall(), and sendProcessModelStates().

std::vector<KalmanUpdateResult> GT2004BallLocator::kalmanUpdateResults [private]

Vector containing the result of calls to the update function of the process models

Definition at line 105 of file GT2004BallLocator.h.

Referenced by addModel(), drawBallPosition(), handleSeenBall(), and sendProcessModelStates().

double GT2004BallLocator::last_seen_x_rel [private]

x-position of last seen ball relative to robot

Definition at line 120 of file GT2004BallLocator.h.

Referenced by compensateOdometry(), GT2004BallLocator(), handleSeenBall(), and handleUnseenBall().

double GT2004BallLocator::last_seen_y_rel [private]

y-position of last seen ball relative to robot

Definition at line 121 of file GT2004BallLocator.h.

Referenced by compensateOdometry(), GT2004BallLocator(), handleSeenBall(), and handleUnseenBall().

Vector2<double> GT2004BallLocator::lastBallSeen [private]

Definition at line 142 of file GT2004BallLocator.h.

Referenced by execute().

unsigned long int GT2004BallLocator::lastFrameNumber [private]

Definition at line 98 of file GT2004BallLocator.h.

Referenced by determineNumberOfImagesWith_WithoutBall().

OdometryData GT2004BallLocator::lastOdometry [private]

Last odometry data of robot.

Definition at line 124 of file GT2004BallLocator.h.

Referenced by compensateOdometry(), handleSeenBall(), and handleUnseenBall().

Pose2D GT2004BallLocator::lastRobotOdometry [private]

Definition at line 144 of file GT2004BallLocator.h.

Referenced by execute().

CameraMatrix GT2004BallLocator::prevCameraMatrix [private]

Definition at line 139 of file GT2004BallLocator.h.

Referenced by execute(), and handleSeenBall().

double GT2004BallLocator::vx_abs [private]

speed of ball in x-direction on field

Definition at line 117 of file GT2004BallLocator.h.

Referenced by drawBallPosition(), GT2004BallLocator(), handleSeenBall(), and handleUnseenBall().

double GT2004BallLocator::vx_rel [private]

speed of ball in x-direction relative to robot

Definition at line 114 of file GT2004BallLocator.h.

Referenced by GT2004BallLocator(), handleSeenBall(), handleUnseenBall(), sendProcessModelStates(), setBallState(), and setBallStateV2().

double GT2004BallLocator::vy_abs [private]

speed of ball in y-direction on field

Definition at line 118 of file GT2004BallLocator.h.

Referenced by drawBallPosition(), GT2004BallLocator(), handleSeenBall(), and handleUnseenBall().

double GT2004BallLocator::vy_rel [private]

speed of ball in y-direction relative to robot

Definition at line 115 of file GT2004BallLocator.h.

Referenced by GT2004BallLocator(), handleSeenBall(), handleUnseenBall(), sendProcessModelStates(), and setBallState().

double GT2004BallLocator::x_abs [private]

x-position of ball on field

Definition at line 112 of file GT2004BallLocator.h.

Referenced by drawBallPosition(), GT2004BallLocator(), handleSeenBall(), handleUnseenBall(), and setUnknownResult().

double GT2004BallLocator::x_rel [private]

x-position of ball relative to robot

Definition at line 110 of file GT2004BallLocator.h.

Referenced by GT2004BallLocator(), handleSeenBall(), handleUnseenBall(), sendProcessModelStates(), and setBallState().

double GT2004BallLocator::y_abs [private]

y-position of ball on field

Definition at line 113 of file GT2004BallLocator.h.

Referenced by drawBallPosition(), GT2004BallLocator(), handleSeenBall(), handleUnseenBall(), and setUnknownResult().

double GT2004BallLocator::y_rel [private]

y-position relative to robot

Definition at line 111 of file GT2004BallLocator.h.

Referenced by GT2004BallLocator(), handleSeenBall(), handleUnseenBall(), sendProcessModelStates(), and setBallState().


The documentation for this class was generated from the following files:
Generated on Thu Dec 7 01:32:57 2006 for DT2005.panorama by  doxygen 1.4.7