Modules/BallLocator/GT2004BallLocator.h

Go to the documentation of this file.

/**
* @file GT2004BallLocator.h
* Contains a BallLocator implementation using GT2004 filtering
*
* @author <a href="mailto:stefanuhrig@gmx.net">Stefan Uhrig</a>
*/
//------------------------------------------------------------------------------
#ifndef KALMANBALLLOCATOR_H_INCLUDED
#define KALMANBALLLOCATOR_H_INCLUDED
//------------------------------------------------------------------------------
#include "BallLocator.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "GT2004ProcessModels/BaseModel.h"
#include <vector>
#include "Tools/Actorics/RobotDimensions.h"
#include "Tools/RobotConfiguration.h"
#include "Tools/Math/Common.h"
//------------------------------------------------------------------------------
/**
* @class GT2004BallLocator
* A BallLocator using GT2004 filtering. This class is the main class managing
* different GT2004 process models
*/
class GT2004BallLocator : public BallLocator
{
public:
  GT2004BallLocator(BallLocatorInterfaces& interfaces);
  virtual ~GT2004BallLocator();

public:
  /**
  * Executes the GT2004BallLocator module
  */
  virtual void execute();
  /**
  * Handles idGT2004Data debug messages
  * @param message The message
  * @return true if message was handled
  */
  virtual bool handleMessage(InMessage& message);

private:

  /** Counts the number of percepts that contained a ball or no ball */
  void determineNumberOfImagesWith_WithoutBall();

  /**
  * Called if ball was seen. Performs update step.
  */
  void handleSeenBall();
  /**
  * Called if ball was not seen. Performs predict step.
  */
  void handleUnseenBall();

  /** Compensates the body movements */
  void compensateOdometry();

  /**
  * Checks if the ball rolls by right or left of the robot
  */
  void setBallState();
  /**
  * Checks if the ball rolls by right or left of the robot, v2
  */
  void setBallStateV2();
  /**
  * setUnknownResult is called if an exception occurs.
  * However, this should not happen, but if, this prevents the robot from crashing.
  */
  void setUnknownResult();
  /**
  * True if passed position is nonsense.
  * @param x x-position in millimeter
  * @param y y-position in millimeter
  * @return true if position is nonsense
  */
  bool isNonSensePos(double x, double y);
  /**
  * Adds a process model to the list of managed process models.
  * @param pModel Pointer to process model derived from GT2004ProcessModelBase
  */
  void addModel(KalmanProcessModelBase* pModel);
  /**
  * Frees the managed process models
  */
  void destroyModels();
  /**
  * Sends the process model states as debug messages
  */
  void sendProcessModelStates();
  /**
  * Draws the global ball positions and data model ball positions on the field
  */
  void drawBallPosition();

private:
  unsigned long int lastFrameNumber;
  bool ballWasSeenInLastImage;

  //! Vector containing all instantiated process models
  std::vector<KalmanProcessModelBase*> kalmanModels;
  //! Vector containing the result of calls to the update function of the
  //! process models
  std::vector<KalmanUpdateResult> kalmanUpdateResults;
  //! Vector containing the result of calls to the predict function of the
  //! process models
  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

  //! Last odometry data of robot
  OdometryData lastOdometry;

  //! If true, process model states are sent as debug messages
  bool bSendProcessModelStates;

  //! 1.2*length of field diagonal
  static double dFieldDiagonalLength;

  // some data for ball state information 
  // used in setBallStateV2();
  bool freshDefend;
  Vector2<double> ballSensedPos;
  Vector2<double> ballSensedRelPos;
  unsigned long ballSensedTime;

  CameraMatrix prevCameraMatrix;

  // percept-part
  Vector2<double> lastBallSeen;
  
  Pose2D lastRobotOdometry; 
  
  Vector2<double> relative2FieldCoord(RobotPose rp, double x, double y);
};
//------------------------------------------------------------------------------
#endif
//------------------------------------------------------------------------------

/*
 * Change log :
 * $Log: GT2004BallLocator.h,v $
 * Revision 1.1  2004/07/14 20:56:34  spranger
 * moved KalmanCombo (incl percept stuff) to gt2004balllocator
 *
 * Revision 1.9  2004/06/21 08:15:57  nistico
 * The dot product of normalized vectors (versors) is guaranteed to be within [-1, 1] ;
 * the angle was not normalized, however. (-pi, pi)
 *
 * Revision 1.8  2004/06/18 10:24:51  nistico
 * Kalman measurement covariance matrix now adjusted also based on
 * head panning angular velocity
 *
 * Revision 1.7  2004/06/15 21:40:14  uhrig
 * Added possibility to disable and enable process models in the Kalman filter.
 *
 * Revision 1.6  2004/06/15 17:48:32  juengel
 * Added method determineNumberOfImagesWith_WithoutBall().
 *
 * Revision 1.5  2004/06/14 10:44:29  risler
 * removed speed averaging
 * reactivated old working ball state
 *
 * Revision 1.4  2004/05/27 18:49:17  kerdels
 * added a small 5 frames sliding average for the relative ballspeed,
 * added new ballState Representation and adjusted the apropriate files
 *
 * Revision 1.3  2004/05/26 10:08:03  loetzsch
 * correct use of odometry
 *
 * Revision 1.1.1.1  2004/05/22 17:15:19  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.5  2004/04/20 14:12:16  uhrig
 *  - odometry processing corrected
 *  - fixed position model disabled
 *  - covariance matrices tuned
 *
 * Revision 1.4  2004/04/07 12:28:56  risler
 * ddd checkin after go04 - first part
 *
 * Revision 1.3  2004/03/31 00:09:46  risler
 * set last seen position when ball is not seen
 *
 * Revision 1.2  2004/03/30 15:48:06  risler
 * various bugfixes from stefan
 *
 * Revision 1.1.1.1  2004/03/29 08:28:51  Administrator
 * initial transfer from tamara
 *
 * Revision 1.3  2004/03/15 12:28:52  risler
 * change log added
 *
 *
 */

---