Modules/SelfLocator/DistanceToBorderEstimator.cpp

Go to the documentation of this file.

/**
 * 
 * This file contains a class that estimates the distance to the border.
 *
 * @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
 */

#include "DistanceToBorderEstimator.h"

DistanceToBorderEstimator::DistanceToBorderEstimator(const SelfLocatorInterfaces& interfaces) :
SelfLocatorInterfaces(interfaces)
{
  numberOfLinesPerceptsWithoutBorderPoints = 100;
  lastDistanceToBorder = 1000;
}

void DistanceToBorderEstimator::execute()
{
  int currentDistanceToBorder = 0;
  for(int i = 0; i < linesPercept.numberOfPoints[LinesPercept::border]; i++)
  {
    currentDistanceToBorder += linesPercept.points[LinesPercept::border][i].abs();
  }
  if(linesPercept.numberOfPoints[LinesPercept::border] > 0)
  {
    currentDistanceToBorder /= linesPercept.numberOfPoints[LinesPercept::border];
    numberOfLinesPerceptsWithoutBorderPoints = 0;
  }
  else
  {
    numberOfLinesPerceptsWithoutBorderPoints++;
    if(numberOfLinesPerceptsWithoutBorderPoints > 30) currentDistanceToBorder = 1000;
    else currentDistanceToBorder = lastDistanceToBorder;
  }
  distanceToBorderBuffer.add(currentDistanceToBorder);

  robotPose.distanceToBorder = distanceToBorderBuffer.getSum() / distanceToBorderBuffer.getNumberOfEntries();
  robotPose.angleToBorder = 0.777;
}


/*
 * Change log :
 * 
 * $Log: DistanceToBorderEstimator.cpp,v $
 * Revision 1.1.1.1  2004/05/22 17:20:41  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.1  2004/03/27 16:17:24  juengel
 * Added DistanceToBorderEstimator.
 *
 */

---