ballprediction.cc

该文件是包含了机器人足球比赛中的整个系统的代码

#include "BallPrediction.h"
#include "../Globals.h"
#include "../Common/Common.h"

BallPrediction::BallPrediction()
{
  framesBallSeen_ = 0;
  for (int i=0; i < 5; i++) {
    prevBall_[i].x = 0; 
    prevBall_[i].y = 0;
    prevBall_[i].distance = 0;
    prevBall_[i].heading = 0;
  }
  predictedBall_.x = 0; 
  predictedBall_.y = 0;
  predictedBall_.distance = 0;
  predictedBall_.heading = 0;
  diveDelay = 0;
  hadOutlier = false;
}

void BallPrediction::LoadParameters(char* filename)
{
  gMoney.LoadParameters(filename);
}

void BallPrediction::ProcessFrame()
{ 
  AddBallToArray();
  //DoPrediction(3); // 3 is for 3 frames ahead
  //if (CanPredict()) DetermineAction();
  //else shouldDive = false;
  gMoney.ProcessFrame();
}

bool BallPrediction::CanPredict() {
 if (framesBallSeen_ == 5) return true; else return false;
}

bool BallPrediction::ShouldIDive()
{ 
  //DoPrediction(30);
  //DetermineAction();
  return gMoney.ShouldIDive();
}


void BallPrediction::AddBallToArray()
{
  if (vo_ball_ == NULL) {
    framesBallSeen_ = 0;
    hadOutlier = false;
    return;
  } 
  double ballDistance = vo_ball_->distance_;
  double ballHeading = vo_ball_->heading_;
  
  // outlier stuff
  LocomotionData ld = lcq_.GetLocomotionData();
//	double changeX=-ld.deltaLeft;
	double changeY=ld.deltaForward;
	
  double tempX = ABS(vo_ball_->distance_) * sin(vo_ball_->heading_);
  double tempY = ABS(vo_ball_->distance_) * cos(vo_ball_->heading_);
  //cout << "(" << tempX << "," << tempY << ")\n" << flush;
  double dist = sqrt((prevBall_[framesBallSeen_-1].x-tempX)*(prevBall_[framesBallSeen_-1].x-tempX)+(prevBall_[framesBallSeen_-1].y-tempY)*(prevBall_[framesBallSeen_-1].y-tempY));
  if (framesBallSeen_ > 1 && dist > 25) { // 5cm in 1/30 of a sec
    if (hadOutlier) {
      framesBallSeen_ = 0;
     // cout << "outlier" << endl << flush;
      return;
    } else {
      ballDistance = prevBall_[framesBallSeen_-1].distance;
      ballHeading = prevBall_[framesBallSeen_-1].heading;
    }
    hadOutlier = true;
  } else hadOutlier = false;
  
  isMovingAway_ = false;
 
  framesBallSeen_++;
  framesBallSeen_=(int) CROP(framesBallSeen_,0,5);
  if (framesBallSeen_ == 5){ 
    for (int i=1; i < 5; i++) {
      prevBall_[i-1].x = prevBall_[i].x; 
      prevBall_[i-1].y = prevBall_[i].y;
      prevBall_[i-1].distance = prevBall_[i].distance;
      prevBall_[i-1].heading = prevBall_[i].heading;
    }
    if ( ( (prevBall_[0].y - tempY) < ABS(10*changeY) ) ) isMovingAway_ =true;
  }
  prevBall_[framesBallSeen_-1].x = ABS(ballDistance) * sin(ballHeading);
  prevBall_[framesBallSeen_-1].y = ABS(ballDistance) * cos(ballHeading);  
  prevBall_[framesBallSeen_-1].distance = ABS(ballDistance);
  prevBall_[framesBallSeen_-1].heading = ballHeading;
}

bool BallPrediction::DoPrediction(int framesAhead) {
  if (framesBallSeen_ == 5) {
    predictedBall_.x = (0.6+0.2*framesAhead)*prevBall_[4].x + (0.4+0.1*framesAhead)*prevBall_[3].x  + 0.2*prevBall_[2].x - (0.1*framesAhead)*prevBall_[1].x - (0.2+0.2*framesAhead)*prevBall_[0].x;
    predictedBall_.y = (0.6+0.2*framesAhead)*prevBall_[4].y + (0.4+0.1*framesAhead)*prevBall_[3].y  + 0.2*prevBall_[2].y - (0.1*framesAhead)*prevBall_[1].y - (0.2+0.2*framesAhead)*prevBall_[0].y;  
    predictedBall_.distance = sqrt(predictedBall_.x*predictedBall_.x + predictedBall_.y*predictedBall_.y);
    predictedBall_.heading = atan2(predictedBall_.x,predictedBall_.y);  

//    cout << "Vision (" << ABS(vo_ball_->distance_) << "," << vo_ball_->heading_ << ")  ";
//    cout << "Predicated " << framesAhead << "  (" << predictedBall_.x << "," << predictedBall_.y << ") (" << predictedBall_.distance << "," << predictedBall_.heading << ")" << endl << flush;
  
    return true; 
  }
  // Something is wrong make up some values -- these should really not be used by anyone !
  if (vo_ball_ != NULL) {
    predictedBall_.distance = vo_ball_->distance_;
    predictedBall_.heading = vo_ball_->heading_;
  } else {
    predictedBall_.distance = -500;
    predictedBall_.heading = 0.0;
  }
  return false;
}

void BallPrediction::DetermineAction()
{
  shouldDive = false;
  //return;
  // ball not moving fast enough
  double yChange = (predictedBall_.y - prevBall_[framesBallSeen_-1].y) / 3;
  double xChange = (predictedBall_.x - prevBall_[framesBallSeen_-1].x) / 3;


  // calculate when the ball will cross my x axis and also at which point
  // We don't care if it will take more then 20 frames -- this is too long in advance;
 
  if (yChange < -1.5) {
    double time = prevBall_[framesBallSeen_-1].y / ABS(yChange);
    double xIntercept = prevBall_[framesBallSeen_-1].x + (xChange * time);

    double x = fabs(xIntercept);
    if (xIntercept != 0.0 && time != 0.0 && time < 23 && x < 30) {
      // Try to ensure that the robot does not dive too soon by adding a delay
      if (diveDelay < 5) {
       // shouldDive = true;
        diveDelay = 0;
      } else diveDelay++;
    }
  } else diveDelay=0;
}

BallPrediction::Graham::Graham()
{
  doBallPrediction = false;
  currentIndex = 0;
  firstArray = true;
  debug = false;
  ballSpeed = 0.0;
  time = 0.0;
  xIntercept = 0.0;
  shouldDive = false;
  diveDelayCount = 0;
  firstCount = 0;
  currentBall = NULL;
  deltaX = 0.0;
  
  diveBound = 10.0;
  diveAtTime = 20.0;
  diveDelay = 0;
  normalBallSpeed = -2.5;
  fastBallSpeed = -4.0;
  ignoreFrames = 4;
  diveTime = 20;
  minDiveBound = 0.0;
}

void BallPrediction::Graham::LoadParameters(char* filename)
{
  configuration_.ParseFile(filename);
  doBallPrediction = configuration_.GetAsBool("DoBallPrediction");
  diveBound = configuration_.GetAsDouble("DiveBound");
  minDiveBound = configuration_.GetAsDouble("MinDiveBound");
  diveAtTime = configuration_.GetAsDouble("DiveAtTime");
  normalBallSpeed = configuration_.GetAsDouble("NormalBallSpeed");
  fastBallSpeed = configuration_.GetAsDouble("FastBallSpeed");
  ignoreFrames = configuration_.GetAsInt("IgnoreFrames");
  diveTime = diveAtTime - diveDelay;
}

void BallPrediction::Graham::ProcessFrame()
{
  if (doBallPrediction == false) return;

  if (!IsBallValid()) {
    currentIndex = 0;
    ballSpeed = 0.0;
    diveDelayCount = 0;
    firstArray = true;
    shouldDive = false;
    return;
  }

  AddBallToArray();
  DetermineAction();
}

bool BallPrediction::Graham::ShouldIDive()
{
  return shouldDive;
}

void BallPrediction::Graham::Clear()
{
  currentIndex = 0;
  firstArray = true;
  ballSpeed = 0.0;
  time = 0.0;
  xIntercept = 0.0;
  shouldDive = false;
  diveDelayCount = 0;
  firstCount = 0;
  currentBall = NULL;
  deltaX = 0.0;
}

bool BallPrediction::Graham::IsBallValid()
{
  if (vo_ball_ == NULL) {
    return false;
  }

  if (vo_ball_->distance_ < 0) {
    if (debug) cout << "ABORTING BP! negative ball distance" << endl;
    return false;
  }

  if (firstArray) {
    firstCount++;
    if (firstCount > 4) {
      firstCount = 0;
      firstArray = false;
    } else {
      return false;
    }
  }
  return true;
}

void BallPrediction::Graham::AddBallToArray()
{
  // Add the latest ball info to the array
  current[currentIndex].x = vo_ball_->distance_ * sin(vo_ball_->heading_);
  current[currentIndex].y = vo_ball_->distance_ * cos(vo_ball_->heading_);
  currentBall = &current[currentIndex];
  currentIndex++;

  if (currentIndex >= MAX_FRAMES) {
    ballSpeed = (current[MAX_FRAMES - 1].y - current[0].y) / (MAX_FRAMES - 1); // There are (MAX_FRAMES - 1) time instances in MAX_FRAMES frames        
    deltaX = (current[MAX_FRAMES - 1].x - current[0].x) / (MAX_FRAMES - 1);
    currentIndex = 0;
  }
}

void BallPrediction::Graham::DetermineAction()
{
  time = 0.0; // the number of frames until the ball reaches the robot
  xIntercept = 0.0; // the x-coordinate of the ball when it reaches the robot
  shouldDive = false;
  if (ballSpeed < normalBallSpeed) {
    time = vo_ball_->distance_ / ABS(ballSpeed);
    xIntercept = currentBall->x + (deltaX * time);

    double x = fabs(xIntercept);
    if (xIntercept != 0.0 && time != 0.0 && time < diveAtTime && x > minDiveBound && x < diveBound) { //ABS(xIntercept) < diveBound) {
      // Try to ensure that the robot does not dive too soon by adding a delay
      diveDelayCount++;
      if (diveDelayCount >= diveDelay) {
        shouldDive = true;
        diveDelayCount = 0;     
      }
    }
  } else {
    diveDelayCount = 0;
  }
}