obstructionmodel.cc

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

#include "../Common/Common.h"
#include "../Globals.h"
//#include "ObjectRecognition.h"
#include "ObstructionModel.h"


ObstructionModel::ObstructionModel() {
  gridSizeX = 10;
  gridSizeY = 20;
  obstructionGrid = new double[gridSizeX*gridSizeY];
  for (int x = 0; x < gridSizeX; x++) {
    for (int y = 0; y < gridSizeY; y++) {
      obstructionGrid[y*gridSizeX+x] = 0.0;
    }
  }
  rayStartOffset = 10.0;
  rayLength = 100.0;
  maxLeftAngle = 0;
  maxRightAngle = 58;

  decayRate = 0.1;

}

void ObstructionModel::LoadParameters() {
  delete obstructionGrid;
  gridSizeX = configuration_.GetAsInt("Ob_GridSizeX");
  gridSizeY = configuration_.GetAsInt("Ob_GridSizeY");
  obstructionGrid = new double[gridSizeX*gridSizeY];
  for (int x = 0; x < gridSizeX; x++) {
    for (int y = 0; y < gridSizeY; y++) {
      obstructionGrid[y*gridSizeX+x] = 0.0;
    }
  }
  rayStartOffset = configuration_.GetAsDouble("Ob_RayStartOffset");
  rayLength = configuration_.GetAsDouble("Ob_RayLength");
  maxLeftAngle = configuration_.GetAsInt("Ob_MaxLeftAngle");
  maxRightAngle = configuration_.GetAsInt("Ob_MaxRightAngle");

  decayRate = configuration_.GetAsDouble("Ob_DecayRate");
}

void ObstructionModel::DoInit() {
  for (int i = 0; i < 58; i++) {
    visionArc[i] = 0;
  }
  // decay values in grid.
  for (int x = 0; x < gridSizeX; x++) {
    for (int y = 0; y < gridSizeY; y++) {
      int index = y*gridSizeX+x;
      obstructionGrid[index] -= decayRate;
      if (obstructionGrid[index] < 0) obstructionGrid[index] = 0;
      if (obstructionGrid[index] > 200) obstructionGrid[index] = 200;
    }
  }
}



void ObstructionModel::DoBlobSearch() {
/*Run** runs_ = visionData->runs_;
  Blob** blobs_ = visionData->blobs_;
  int* numRuns_ = visionData->numRuns_;
  int* numBlobs_ = visionData->numBlobs_;*/


 // account for visible robots
  for (int i = 0; i < visionData->numBlobs_[c_ROBOT_RED]; i++) {
    Blob* currentBlob = &visionData->blobs_[c_ROBOT_RED][i];
    if (currentBlob->subsumed) continue;
    int rightMostAngle = (int)RAD_TO_DEG(visionData->CalculateHeading(currentBlob->maxX));
    int leftMostAngle = (int)RAD_TO_DEG(visionData->CalculateHeading(currentBlob->minX));

    // need a bounds check here for some reason.
    if (leftMostAngle < -29) leftMostAngle = -29;
    if (leftMostAngle > 28) leftMostAngle = 28;
    if (rightMostAngle < -29) rightMostAngle = -29;
    if (rightMostAngle > 28) rightMostAngle = 28;
     
    for (int k = leftMostAngle; k <= rightMostAngle; k++) {
      visionArc[k+29] = 1;
      if ((k % 2) == 0) {
        ProjectRay(20, k, rayLength);
      }
    }
  }
  for (int i = 0; i < visionData->numBlobs_[c_ROBOT_BLUE]; i++) {
    Blob* currentBlob = &visionData->blobs_[c_ROBOT_BLUE][i];
    if (currentBlob->subsumed) continue;
    int rightMostAngle = (int)RAD_TO_DEG(visionData->CalculateHeading(currentBlob->maxX));
    int leftMostAngle = (int)RAD_TO_DEG(visionData->CalculateHeading(currentBlob->minX));

    // need a bounds check here for some reason.
    if (leftMostAngle < -29) leftMostAngle = -29;
    if (leftMostAngle > 28) leftMostAngle = 28;
    if (rightMostAngle < -29) rightMostAngle = -29;
    if (rightMostAngle > 28) rightMostAngle = 28;

    for (int k = leftMostAngle; k <= rightMostAngle; k++) {
      visionArc[k+29] = 2;
      if ((k % 2) == 0) {
        ProjectRay(20, k, rayLength);
      }
    }
  }
}

void ObstructionModel::ProjectRays() {
  // account for clear space
  for (int i = maxLeftAngle; i < maxRightAngle; i++) {
    if (visionArc[i] == 0 && (i % 2 == 0)) {
      // clear space ! inform of this.
      ProjectRay(-1, i-29, rayLength);
    }
  }
}

void ObstructionModel::ProcessBlobs(VisionData* vd) {
  visionData = vd;
  DoInit();
  DoBlobSearch();
  ProjectRays();
}

void ObstructionModel::ProjectRay(double value, int angle, double distance) {
  if (wo_self_ == NULL) return;
  double radAngle = DEG_TO_RAD(angle);
  TransformPosition(&radAngle, 0.0);
  double vecX = sin(wo_self_->heading_+radAngle);
  double vecY = cos(wo_self_->heading_+radAngle);
  double t = rayStartOffset;
  while (t < rayLength) {
    double fieldX = (-wo_self_->x_) + vecX*t;
    double fieldY = wo_self_->y_ + vecY*t;
    int quantizedX = (int)(((fieldX+135.0)/270.0 * gridSizeX));
    int quantizedY = (int)(((fieldY+210.0)/420.0 * gridSizeY));
    if (quantizedX >= gridSizeX || quantizedY >= gridSizeY || quantizedX < 0 || quantizedY < 0) {
      return;
    } else {
      obstructionGrid[quantizedY*gridSizeX+quantizedX] += value;
    }
    t+= 10;
  }
}

void ObstructionModel::TransformPosition(double* heading_, double elevation_) {
/*  double distance_ = 10000;
  double x = 0;
  double y = 0;
  double z = distance_;

  // image heading
  double x2 = x*cos(*heading_) + z*sin(*heading_); // As x is 0 this is effectivly z*sin(heading_)
  double y2 = y;
  double z2 = -x*sin(*heading_) + z*cos(*heading_); // As x is 0 this is effectivly z*cos(heading_)

  // image elevation
  double x3 = x2;
  double y3 = y2*cos(elevation_) + z2*sin(elevation_);
  double z3 = -y2*sin(elevation_) + z2*cos(elevation_);

  // robot HEAD length
  double x4 = x3;
  double y4 = y3;
  double z4 = z3 + HEADLENGTH;

  // robot head pan - fixed in the y direction
  double x5 = x4 * visionData->cosHeadPan_ + z4 * visionData->sinHeadPan_;
  double y5 = y4;
  double z5 = -x4 * visionData->sinHeadPan_ + z4 * visionData->cosHeadPan_;

  // robot neck length and distance from ground to neck
  double x6 = x5;
  double y6 = y5 + NECKLENGTH;
  double z6 = z5;

  // rotation for head tilt - fixed in the x direction
  double x7 = x6;
  double y7 = y6 * visionData->cosHeadTilt_ - z6 * visionData->sinHeadTilt_;
  double z7 = y6 * visionData->sinHeadTilt_ + z6 * visionData->cosHeadTilt_;

  // calculate actual distance and heading to robot
  distance_ = sqrt(x7*x7 + y7*y7 + z7*z7);
  if (z7 == 0.0) {
    *heading_ = PI/2;
  } else {
    *heading_ = atan2(x7, z7);
  }*/
}