Representations/Cognition/BallModel.cpp

Go to the documentation of this file.

//------------------------------------------------------------------------------
/**
* @file BallModel.cpp
*
* Implementation of class BallModel.
*
* @author <A href="mailto:xiang@informatik.uni-bremen.de">Hong Xiang</A>
* @author <A href="mailto:rlx@informatik.uni-bremen.de">Lang Lang</A>
* @author <A href="mailto:roefer@tzi.de">Thomas Röfer</A>
* @author <A href="mailto:jan@aiboteamhumboldt.com">Jan Hoffmann</A>
* @author <A href="mailto:stefanuhrig@gmx.net">Stefan Uhrig</A>
*/
//------------------------------------------------------------------------------
#include "BallModel.h"
#include "Platform/SystemCall.h"
#include "Tools/FieldDimensions.h"
//------------------------------------------------------------------------------
BallState::BallState()
{
  reset();
  useGivenProbabilities = false;
}
//------------------------------------------------------------------------------
void BallState::reset()
{
  positionProb = 0;
  velocityProb = 0;


  positionRobot = Vector2<double>(0.0, 0.0);
  speedRobot = Vector2<double>(0.0, 0.0);
  positionField = Vector2<double>(0.0, 0.0);
  speedField = Vector2<double>(0.0, 0.0);
  robotPose = RobotPose();

  ballInFrontOfOpponentGoal = false;
  ballRollsByLeft = false;
  ballRollsByRight = false;
  ballRollsFarByLeft = false;
  ballRollsFarByRight = false;
  ballRollsTowardsRobot = false;
  ballRollsFast = false;
  projectedDistanceOnYAxis = 0.0;
  timeTillBallCrossesYAxis = 1000000000;

  positionGaussSet = false;
  speedGaussSet = false;
}
//------------------------------------------------------------------------------
void BallState::setBallDataRelativeToRobot(
                    const RobotPose& robotPose,
                    double px,
                    double py,
                    double vx,
                    double vy)
{
  this->robotPose = robotPose;
  positionRobot.x = px;
  positionRobot.y = py;
  speedRobot.x = vx;
  speedRobot.y = vy;

  positionField = positionVectorFromRobotToField(robotPose, positionRobot);
  speedField = directionVectorFromRobotToField(robotPose, speedRobot);

  setBallFlags();
}
//------------------------------------------------------------------------------
void BallState::setBallDataRelativeToField(
                    const RobotPose& robotPose,
                    double px,
                    double py,
                    double vx,
                    double vy)
{
  this->robotPose = robotPose;
  positionField.x = px;
  positionField.y = py;
  speedField.x = vx;
  speedField.y = vy;

  positionRobot = positionVectorFromFieldToRobot(robotPose, positionField);
  speedRobot = directionVectorFromFieldToRobot(robotPose, speedField);

  setBallFlags();
}
//------------------------------------------------------------------------------
void BallState::setPositionGaussianDataRelativeToRobot(
                     const RobotPose& robotPose,
                     double orientation,
                     double majorAxis,
                     double minorAxis)
{
  positionGaussSet = true;
  positionGaussOrientationRobot = orientation;
  positionGaussOrientationField =
    orientationFromRobotToField(robotPose, orientation);
  positionGaussMajAxis = majorAxis;
  positionGaussMinAxis = minorAxis;
}
//------------------------------------------------------------------------------
void BallState::setPositionGaussianDataRelativeToField(
                     const RobotPose& robotPose,
                     double orientation,
                     double majorAxis,
                     double minorAxis)
{
  positionGaussSet = true;
  positionGaussOrientationField = orientation;
  positionGaussOrientationRobot =
    orientationFromFieldToRobot(robotPose, orientation);
  positionGaussMajAxis = majorAxis;
  positionGaussMinAxis = minorAxis;
}
//------------------------------------------------------------------------------
void BallState::setSpeedGaussianDataRelativeToRobot(
                     const RobotPose& robotPose,
                     double orientation,
                     double majorAxis,
                     double minorAxis)
{
  speedGaussSet = true;
  speedGaussOrientationRobot = orientation;
  speedGaussOrientationField =
    orientationFromRobotToField(robotPose, orientation);
  speedGaussMajAxis = majorAxis;
  speedGaussMinAxis = minorAxis;
}
//------------------------------------------------------------------------------
void BallState::setSpeedGaussianDataRelativeToField(
                     const RobotPose& robotPose,
                     double orientation,
                     double majorAxis,
                     double minorAxis)
{
  speedGaussSet = true;
  speedGaussOrientationField = orientation;
  speedGaussOrientationRobot =
    orientationFromFieldToRobot(robotPose, orientation);
  speedGaussMajAxis = majorAxis;
  speedGaussMinAxis = minorAxis;
}
//------------------------------------------------------------------------------
double BallState::getPositionValidity() const
{
  if (useGivenProbabilities)
    return positionProb;

  if (!positionGaussSet)
    return 1.0;
  return max(0.0, 1.0 - (positionGaussMajAxis + positionGaussMinAxis)/1000.0);
}
//------------------------------------------------------------------------------
double BallState::getSpeedValidity() const
{
  if (useGivenProbabilities)
    return velocityProb;

  if (!speedGaussSet)
    return 1.0;
  return max(0.0, 1.0 - (speedGaussMajAxis + speedGaussMinAxis)/1000.0);
}
//------------------------------------------------------------------------------
void BallState::drawPosition(Drawings::Color color) const
{
  CIRCLE(ballLocatorField, positionField.x, positionField.y,
         100, 20, Drawings::ps_solid, color);
}
//------------------------------------------------------------------------------
void BallState::drawSpeed(Drawings::Color color) const
{
  ARROW(ballLocatorField, positionField.x, positionField.y,
        positionField.x + speedField.x, positionField.y + speedField.y,
        10, Drawings::ps_solid, color);
}
//------------------------------------------------------------------------------
void BallState::drawPositionGaussian(Drawings::Color color) const
{
  if (!positionGaussSet)
    return;

  double c = cos(positionGaussOrientationField);
  double s = sin(positionGaussOrientationField);
  double x1 = positionGaussMajAxis*c;
  double y1 = positionGaussMajAxis*s;
  double x2 = -positionGaussMinAxis*s;
  double y2 = positionGaussMinAxis*c;

  LINE(ballLocatorField,
       positionField.x-x1, positionField.y-y1,
       positionField.x+x1, positionField.y+y1,
       20, Drawings::ps_solid, color);

  LINE(ballLocatorField,
       positionField.x-x2, positionField.y-y2,
       positionField.x+x2, positionField.y+y2,
       20, Drawings::ps_solid, color);

}
//------------------------------------------------------------------------------
void BallState::drawSpeedGaussian(Drawings::Color color) const
{
}
//------------------------------------------------------------------------------
void BallState::setBallFlags()
{
  // Reset flags
  ballInFrontOfOpponentGoal = false;
  ballRollsByLeft = false;
  ballRollsByRight = false;
  ballRollsFarByLeft = false;
  ballRollsFarByRight = false;
  ballRollsTowardsRobot = false;
  ballRollsFast = false;
  projectedDistanceOnYAxis = 0.0;
  timeTillBallCrossesYAxis = 1000000000;
  
  // Check if ball the line from robot to ball crosses the goal line
  if ((robotPose.translation.x < xPosOpponentGroundline) &&
      (robotPose.translation.x < positionField.x))
  {
    double p1x = xPosOpponentGoal;
    double p1y = yPosRightGoal;
    double p2y = yPosLeftGoal;
    double bx = positionField.x;
    double by = positionField.y;
    double rx = robotPose.translation.x;
    double ry = robotPose.translation.y;
    double lambda = (p1x*by - p1x*ry + bx*ry - p1y*bx + p1y*rx - rx*by) /
                    ((bx - rx)*(p1y - p2y));
    ballInFrontOfOpponentGoal = ((lambda > 0.0) && (lambda < 1.0));
  }
  
  // Ball is considered fast if speed is higher than 400.0mm/s
  if (speedRobot.abs() > 400.0)
    ballRollsFast = true;

  // Ball must be approaching faster than 50 millimeters per second
  if (speedRobot.x > -50.0)
    return;

  // Calculate y-position (side) of ball when the ball has the same height
  // as the robot und set appropriate ball state
  //timeTillBallCrossesYAxis = (long)(-speedRobot.y/speedRobot.x * 1000.0);
  timeTillBallCrossesYAxis = (long)(-positionRobot.x / speedRobot.x);
  projectedDistanceOnYAxis =
    positionRobot.y - (speedRobot.y/speedRobot.x)*positionRobot.x;

  // Ball must be nearer than 1 meter
  double dist = positionRobot.abs();
  if (dist > 1000.0)
    return;

  if ((projectedDistanceOnYAxis < -150.0) &&
      (projectedDistanceOnYAxis > -500.0))
  {
    ballRollsByRight = true;
  }
  else if ((projectedDistanceOnYAxis > 150.0) &&
           (projectedDistanceOnYAxis < 500.0))
  {
    ballRollsByLeft = true;
  }
  else if ((projectedDistanceOnYAxis >= -150.0) &&
           (projectedDistanceOnYAxis <= 150.0))
  {
    ballRollsTowardsRobot = true;
  }
  if ((projectedDistanceOnYAxis < -500.0) &&
      (projectedDistanceOnYAxis > -1000.0))
  {
    ballRollsFarByRight = true;
  }
  else if ((projectedDistanceOnYAxis > 500.0) &&
           (projectedDistanceOnYAxis < 1000.0))
  {
    ballRollsFarByLeft = true;
  }
}
//------------------------------------------------------------------------------
Vector2<double> BallState::positionVectorFromRobotToField(
                                const RobotPose& robotPose,
                                const Vector2<double>& pv)
{
  double c = robotPose.getCos();
  double s = robotPose.getSin();
  Vector2<double> res(pv.x*c - pv.y*s + robotPose.translation.x,
                      pv.x*s + pv.y*c + robotPose.translation.y);
  return res;
}
//------------------------------------------------------------------------------
Vector2<double> BallState::directionVectorFromRobotToField(
                                const RobotPose& robotPose,
                                const Vector2<double>& dv)
{
  double c = robotPose.getCos();
  double s = robotPose.getSin();
  Vector2<double> res(dv.x*c - dv.y*s,
                      dv.x*s + dv.y*c);
  return res;
}
//------------------------------------------------------------------------------
double BallState::orientationFromRobotToField(
                       const RobotPose& robotPose,
                       double orientation)
{
  return normalize(robotPose.rotation + orientation);
}
//------------------------------------------------------------------------------
Vector2<double> BallState::positionVectorFromFieldToRobot(
                                const RobotPose& robotPose,
                                const Vector2<double>& pv)
{
  double c = robotPose.getCos();
  double s = robotPose.getSin();
  double x = robotPose.translation.x;
  double y = robotPose.translation.y;
  Vector2<double> res((pv.x - x)*c + (pv.y - y)*s,
                      (pv.y - y)*c - (pv.x - x)*s);
  return res;
}
//------------------------------------------------------------------------------
Vector2<double> BallState::directionVectorFromFieldToRobot(
                                const RobotPose& robotPose,
                                const Vector2<double>& dv)
{
  double c = robotPose.getCos();
  double s = robotPose.getSin();
  Vector2<double> res(dv.x*c + dv.y*s,
                      -dv.x*s + dv.y*c);
  return res;
}
//------------------------------------------------------------------------------
double BallState::orientationFromFieldToRobot(
                       const RobotPose& robotPose,
                       double orientation)
{
  return normalize(orientation - robotPose.rotation);
}
//------------------------------------------------------------------------------
void BallState::serialize(In* in, Out* out)
{
  STREAM_REGISTER_BEGIN();
  STREAM( positionRobot);
  STREAM( speedRobot);
  STREAM( positionField);
  STREAM( speedField);
  STREAM( robotPose);
  STREAM( ballInFrontOfOpponentGoal);
  STREAM( ballRollsByLeft);
  STREAM( ballRollsByRight);
  STREAM( ballRollsTowardsRobot);
  STREAM( ballRollsFast);
  STREAM( projectedDistanceOnYAxis);
  STREAM( timeTillBallCrossesYAxis);
  STREAM( positionGaussSet);
  STREAM( positionGaussOrientationRobot);
  STREAM( positionGaussOrientationField);
  STREAM( positionGaussMajAxis);
  STREAM( positionGaussMinAxis);
  STREAM( speedGaussSet);
  STREAM( speedGaussOrientationRobot);
  STREAM( speedGaussOrientationField);
  STREAM( speedGaussMajAxis);
  STREAM( speedGaussMinAxis);
  STREAM_REGISTER_FINISH();
}

/*Out& operator<<(Out& stream, const BallState& bs)
{
  char boolHelp;

  stream << bs.positionRobot << bs.speedRobot;
  stream << bs.positionField << bs.speedField;
  stream << bs.robotPose;

  boolHelp = bs.ballInFrontOfOpponentGoal? 1: 0;
  stream << boolHelp;
  boolHelp = bs.ballRollsByLeft? 1: 0;
  stream << boolHelp;
  boolHelp = bs.ballRollsByRight? 1: 0;
  stream << boolHelp;
  boolHelp = bs.ballRollsTowardsRobot? 1: 0;
  stream << boolHelp;
  boolHelp = bs.ballRollsFast? 1: 0;
  stream << boolHelp;
  stream << bs.projectedDistanceOnYAxis;
  stream << bs.timeTillBallCrossesYAxis;

  boolHelp = bs.positionGaussSet? 1: 0;
  stream << boolHelp;
  stream << bs.positionGaussOrientationRobot
         << bs.positionGaussOrientationField;
  stream << bs.positionGaussMajAxis << bs.positionGaussMinAxis;

  boolHelp = bs.speedGaussSet? 1: 0;
  stream << boolHelp;
  stream << bs.speedGaussOrientationRobot << bs.speedGaussOrientationField;
  stream << bs.speedGaussMajAxis << bs.speedGaussMinAxis;

  return stream;
}
//------------------------------------------------------------------------------
In& operator>>(In& stream, BallState& bs)
{
  char boolHelp;

  stream >> bs.positionRobot >> bs.speedRobot;
  stream >> bs.positionField >> bs.speedField;
  stream >> bs.robotPose;

  stream >> boolHelp;
  bs.ballInFrontOfOpponentGoal = (boolHelp == 1);
  stream >> boolHelp;
  bs.ballRollsByLeft = (boolHelp == 1);
  stream >> boolHelp;
  bs.ballRollsByRight = (boolHelp == 1);
  stream >> boolHelp;
  bs.ballRollsTowardsRobot = (boolHelp == 1);
  stream >> boolHelp;
  bs.ballRollsFast = (boolHelp == 1);
  stream >> bs.projectedDistanceOnYAxis;
  stream >> bs.timeTillBallCrossesYAxis;

  stream >> boolHelp;
  bs.positionGaussSet = (boolHelp == 1);
  stream >> bs.positionGaussOrientationRobot
         >> bs.positionGaussOrientationField;
  stream >> bs.positionGaussMajAxis >> bs.positionGaussMinAxis;

  stream >> boolHelp;
  bs.speedGaussSet = (boolHelp == 1);
  stream >> bs.speedGaussOrientationRobot >> bs.speedGaussOrientationField;
  stream >> bs.speedGaussMajAxis >> bs.speedGaussMinAxis;

  return stream;
}*/
//------------------------------------------------------------------------------
SeenBallState::SeenBallState()
: BallState()
{
  reset();
}
//------------------------------------------------------------------------------
void SeenBallState::reset()
{
  BallState::reset();
  timeWhenLastSeen = 0;
  timeWhenFirstSeenConsecutively = 0;
  timeUntilSeenConsecutively = 0;
}
//------------------------------------------------------------------------------
long SeenBallState::getConsecutivelySeenTime() const
{
  long notSeenTime = (long)SystemCall::getTimeSince(timeUntilSeenConsecutively);
  if (notSeenTime<200)
  {
    return ((long)timeUntilSeenConsecutively -
            (long)timeWhenFirstSeenConsecutively);
  }
  else
  {
    return -notSeenTime;
  }
}

void SeenBallState::serialize(In *in, Out *out)
{
  STREAM_REGISTER_BEGIN();
  getStreamHandler().registerBase();
  STREAM_BASE(BallState);
  STREAM( timeWhenLastSeen);
  STREAM( timeWhenFirstSeenConsecutively);
  STREAM( timeUntilSeenConsecutively);
  STREAM_REGISTER_FINISH();
}

/*
//------------------------------------------------------------------------------
Out& operator<<(Out& stream, const SeenBallState& sbs)
{
  stream << static_cast<const BallState&>(sbs);
  stream << sbs.timeWhenLastSeen;
  stream << sbs.timeWhenFirstSeenConsecutively;
  stream << sbs.timeUntilSeenConsecutively;

  return stream;
}
//------------------------------------------------------------------------------
In& operator>>(In& stream, SeenBallState& sbs)
{
  stream >> static_cast<BallState&>(sbs);
  stream >> sbs.timeWhenLastSeen;
  stream >> sbs.timeWhenFirstSeenConsecutively;
  stream >> sbs.timeUntilSeenConsecutively;

  return stream;
}*/
//------------------------------------------------------------------------------
PropagatedBallState::PropagatedBallState()
: BallState()
{
  reset();
}
//------------------------------------------------------------------------------
void PropagatedBallState::reset()
{
  lastSeenBallState.reset();
  BallState::reset();
}

void PropagatedBallState::serialize(In *in, Out *out)
{
  STREAM_REGISTER_BEGIN();
  STREAM_BASE(BallState);
  STREAM( lastSeenBallState);
  STREAM_REGISTER_FINISH();
}
//------------------------------------------------------------------------------
/*Out& operator<<(Out& stream, const PropagatedBallState& pbs)
{
  stream << static_cast<const BallState&>(pbs);
  stream << pbs.lastSeenBallState;

  return stream;
}
//------------------------------------------------------------------------------
In& operator>>(In& stream, PropagatedBallState& pbs)
{
  stream >> static_cast<BallState&>(pbs);
  stream >> pbs.lastSeenBallState;

  return stream;
}*/
//------------------------------------------------------------------------------
CommunicatedBallState::CommunicatedBallState()
: BallState()
{
  reset();
}
//------------------------------------------------------------------------------
void CommunicatedBallState::reset()
{
  timeWhenLastObserved = 0;
  BallState::reset();
}
void CommunicatedBallState::serialize(In *in, Out *out)
{
  STREAM_REGISTER_BEGIN();
  STREAM_BASE(BallState);
  STREAM( timeWhenLastObserved);
  STREAM_REGISTER_FINISH();
}
/*//------------------------------------------------------------------------------
Out& operator<<(Out& stream, const CommunicatedBallState& cbs)
{
  stream << static_cast<const BallState&>(cbs);
  stream << cbs.timeWhenLastObserved;

  return stream;
}
//------------------------------------------------------------------------------
In& operator>>(In& stream, CommunicatedBallState& cbs)
{
  stream >> static_cast<BallState&>(cbs);
  stream >> cbs.timeWhenLastObserved;
  
  return stream;
}*/
//------------------------------------------------------------------------------
HypotheticalBallState::HypotheticalBallState()
: BallState()
{
}
//------------------------------------------------------------------------------
/*Out& operator<<(Out& stream, const HypotheticalBallState& hbs)
{
  stream << static_cast<const BallState&>(hbs);
  return stream;
}
//------------------------------------------------------------------------------
In& operator>>(In& stream, HypotheticalBallState& hbs)
{
  stream >> static_cast<BallState&>(hbs);
  return stream;
}*/
//------------------------------------------------------------------------------
BallModel::BallModel()
{
  reset();
}
//------------------------------------------------------------------------------
void BallModel::reset()
{
  frameNumber = 0;
  numberOfImagesWithBallPercept = 0;
  numberOfImagesWithoutBallPercept = 0;
  ballWasSeen = false;
  seen.reset();
  propagated.reset();
  communicated.reset();
  hypothetical.reset();
  //behaviorBallState = noneBallHandling;
}
//------------------------------------------------------------------------------
void BallModel::setFrameNumber(unsigned long frameNumber)
{
  this->frameNumber = frameNumber;
}
//------------------------------------------------------------------------------
const Vector2<double>& BallModel::getKnownPosition(
  unsigned long timeAfterWhichCommunicatedBallsAreAccepted) const
{
  if (SystemCall::getTimeSince(seen.timeWhenLastSeen) <
      timeAfterWhichCommunicatedBallsAreAccepted)
  {
    // use the seen position
    return seen.positionField;
  }
  else
  {
    if (SystemCall::getTimeSince(seen.timeWhenLastSeen) <
        SystemCall::getTimeSince(communicated.timeWhenLastObserved))
    {
      return seen.positionField;
    }
    else
    {
      return communicated.positionField;
    }
  }
}
//------------------------------------------------------------------------------
const Vector2<double>& BallModel::getKnownPosition(
  unsigned long timeAfterWhichCommunicatedBallsAreAccepted,
  unsigned long timeAfterWhichPropagatedAreUsed,
  double maxDistanceToUseSeen,
  double minDistanceToUseSeen) const
{
  //in the BallSymbols: get-known-or-near
  if ((SystemCall::getTimeSince(seen.timeWhenLastSeen)
       < timeAfterWhichPropagatedAreUsed)
      && (seen.positionRobot.abs() < maxDistanceToUseSeen)
      && (seen.positionRobot.abs() > minDistanceToUseSeen)
      && (seen.getPositionValidity() > 0.5))
  {
    INFO(printPixelUsage, idText, text, "known-or-near decided to use seen");
    return seen.positionField;
  }
  else
  {
    if ((SystemCall::getTimeSince(seen.timeWhenLastSeen) <
         timeAfterWhichCommunicatedBallsAreAccepted))
    {
      if ((propagated.getPositionValidity() > 0.1)
        || (propagated.getPositionValidity() > communicated.getPositionValidity()))
      {
        INFO(printPixelUsage, idText, text, "known-or-near decided to use propagated");
        return propagated.positionField;
      }
      else
      {
        INFO(printPixelUsage, idText, text, "known-or-near decided to use communicated");
        return communicated.positionField;
      }
    }
    else
    {
      if (SystemCall::getTimeSince(seen.timeWhenLastSeen) <
          SystemCall::getTimeSince(communicated.timeWhenLastObserved))
      {
        INFO(printPixelUsage, idText, text, "known-or-near decided to use propagated");
        return propagated.positionField;
      }
      else
      {
        INFO(printPixelUsage, idText, text, "known-or-near decided to use communicated");
        return communicated.positionField;
      }
    }
  }
}
//------------------------------------------------------------------------------
const BallState& BallModel::getKnownBallState(
  unsigned long timeAfterWhichCommunicatedBallsAreAccepted,
  unsigned long timeAfterWhichPropagatedAreUsed,
  double maxDistanceToUseSeen,
  double minDistanceToUseSeen) const
{
  //in the BallSymbols: get-known-or-near
  if ((SystemCall::getTimeSince(seen.timeWhenLastSeen)
       < timeAfterWhichPropagatedAreUsed)
      && (seen.positionRobot.abs() < maxDistanceToUseSeen)
      && (seen.positionRobot.abs() > minDistanceToUseSeen)
      && (seen.getPositionValidity() > 0.5))
  {
    return seen;
  }
  else
  {
    if ((SystemCall::getTimeSince(seen.timeWhenLastSeen) <
      timeAfterWhichCommunicatedBallsAreAccepted))
    {
      if ((propagated.getPositionValidity() > 0.1)
        || (propagated.getPositionValidity() > communicated.getPositionValidity()))
      {
        return propagated;
      }
      else
      {
        return communicated;
      }
    }
    else
    {
      if (SystemCall::getTimeSince(seen.timeWhenLastSeen) <
          SystemCall::getTimeSince(communicated.timeWhenLastObserved))
      {
        return propagated;
      }
      else
      {
        return communicated;
      }
    }
  }
}
//------------------------------------------------------------------------------
unsigned long BallModel::getTimeSinceLastKnown(
  unsigned long timeAfterWhichCommunicatedBallsAreAccepted) const
{
  if (SystemCall::getTimeSince(seen.timeWhenLastSeen) <
      timeAfterWhichCommunicatedBallsAreAccepted)
  {
    // use the seen position
    return SystemCall::getTimeSince(seen.timeWhenLastSeen);
  }
  else
  {
    return min(SystemCall::getTimeSince(seen.timeWhenLastSeen),
      SystemCall::getTimeSince(communicated.timeWhenLastObserved));
  }
}

---