#include <GT2004BallLocator.h>
Inheritance diagram for GT2004BallLocator:
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< KalmanUpdateResult > | kalmanUpdateResults |
std::vector< KalmanPredictResult > | kalmanPredictResults |
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 |
Definition at line 24 of file GT2004BallLocator.h.
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] |
void GT2004BallLocator::addModel | ( | KalmanProcessModelBase * | pModel | ) | [private] |
Adds a process model to the list of managed process models.
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
message | The message |
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.
x | x-position in millimeter | |
y | y-position in millimeter |
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().
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))
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] |
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] |
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().