ArRangeBuffer.cpp

/*
ActivMedia Robotics Interface for Applications (ARIA)
Copyright (C) 2004,2005 ActivMedia Robotics, LLC


     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
ActivMedia Robotics for information about a commercial version of ARIA at 
robots@activmedia.com or 
ActivMedia Robotics, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481

*/

#include "ArExport.h"
#include "ariaOSDef.h"
#include "ArRangeBuffer.h"
#include "ArLog.h"

ArRangeBuffer::ArRangeBuffer(int size)
{
  mySize = size;
  myVector.reserve(mySize);
}

ArRangeBuffer::~ArRangeBuffer()
{
  ArUtil::deleteSet(myBuffer.begin(), myBuffer.end());
  ArUtil::deleteSet(myInvalidBuffer.begin(), myInvalidBuffer.end());
}

size_t ArRangeBuffer::getSize(void) const
{
  return mySize;
}

ArPose ArRangeBuffer::getPoseTaken() const
{
  return myBufferPose;
}

void ArRangeBuffer::setPoseTaken(ArPose p)
{
  myBufferPose = p;
}

ArPose ArRangeBuffer::getEncoderPoseTaken() const
{
  return myEncoderBufferPose;
}

void ArRangeBuffer::setEncoderPoseTaken(ArPose p)
{
  myEncoderBufferPose = p;
}

void ArRangeBuffer::setSize(size_t size) 
{
  mySize = size;
  myVector.reserve(mySize);
  // if its smaller then chop the lists down to size
  while (myInvalidBuffer.size() + myBuffer.size() > mySize)
  {
    if ((myRevIterator = myInvalidBuffer.rbegin()) != myInvalidBuffer.rend())
    {
      myReading = (*myRevIterator);
      myInvalidBuffer.pop_back();
      delete myReading;
    }
    else if ((myRevIterator = myBuffer.rbegin()) != myBuffer.rend())
    {
      myReading = (*myRevIterator);
      myBuffer.pop_back();
      delete myReading;
    }
  }
}

const std::list<ArPoseWithTime *> *ArRangeBuffer::getBuffer(void) const
{ 
  return &myBuffer; 
}

std::list<ArPoseWithTime *> *ArRangeBuffer::getBuffer(void)
{ 
  return &myBuffer; 
}


double ArRangeBuffer::getClosestPolar(double startAngle, 
                                               double endAngle, 
                                               ArPose startPos, 
                                               unsigned int maxRange,
                                               double *angle) const
{
  return getClosestPolarInList(startAngle, endAngle, 
                               startPos, maxRange, angle, &myBuffer);
}

double ArRangeBuffer::getClosestPolarInList(
        double startAngle, double endAngle, ArPose startPos, 
        unsigned int maxRange, double *angle, 
        const std::list<ArPoseWithTime *> *buffer)
{
  double closest;
  bool foundOne = false;
  std::list<ArPoseWithTime *>::const_iterator it;
  ArPoseWithTime *reading;
  double th;
  double closeTh;
  double dist;
  double angle1, angle2;

  startAngle = ArMath::fixAngle(startAngle);
  endAngle = ArMath::fixAngle(endAngle);

  for (it = buffer->begin(); it != buffer->end(); ++it)
  {
    reading = (*it);

    angle1=startPos.findAngleTo(*reading);
    angle2=startPos.getTh();
    th = ArMath::subAngle(angle1, angle2);
    if (ArMath::angleBetween(th, startAngle, endAngle))
    {
      if (!foundOne || (dist = reading->findDistanceTo(startPos)) < closest)
      {
        closeTh = th;   
        if (!foundOne)
          closest = reading->findDistanceTo(startPos);
        else
          closest = dist;
        foundOne = true;
      }
    }
  }
  if (!foundOne)
    return maxRange;
  if (angle != NULL)
    *angle = closeTh;
  if (closest > maxRange)
    return maxRange;
  else
    return closest;  
}

double ArRangeBuffer::getClosestBox(double x1, double y1, double x2,
                                             double y2, ArPose startPos,
                                             unsigned int maxRange, 
                                             ArPose *readingPos,
                                             ArPose targetPose) const
{
  return getClosestBoxInList(x1, y1, x2, y2, startPos, maxRange, readingPos, 
                             targetPose, &myBuffer);
}

double ArRangeBuffer::getClosestBoxInList(
        double x1, double y1, double x2, double y2, ArPose startPos,
        unsigned int maxRange, ArPose *readingPos, ArPose targetPose,
        const std::list<ArPoseWithTime *> *buffer)

{
  double closest;
  double dist;
  ArPose closestPos;
  std::list<ArPoseWithTime *>::const_iterator it;
  ArTransform trans;
  ArPoseWithTime pose;
  ArPose zeroPos;
  
  double temp;

  closest = maxRange;
  zeroPos.setPose(0, 0, 0);
  trans.setTransform(startPos, zeroPos);

  if (x1 >= x2)
  {
    temp = x1, 
    x1 = x2;
    x2 = temp;
  }
  if (y1 >= y2)
  {
    temp = y1, 
    y1 = y2;
    y2 = temp;
  }
  
  for (it = buffer->begin(); it != buffer->end(); ++it)
  {
    pose = trans.doTransform(*(*it));

    // see if its in the box
    if (pose.getX() >= x1 && pose.getX() <= x2 &&
        pose.getY() >= y1 && pose.getY() <= y2)
    {
      dist = pose.findDistanceTo(targetPose);
      //pose.log();
      if (dist < closest)
      {
        closest = dist;
        closestPos = pose;
      }
    }
  }
  if (readingPos != NULL)
    *readingPos = closestPos;
  if (closest > maxRange)
    return maxRange;
  else
    return closest;
}

void ArRangeBuffer::applyTransform(ArTransform trans)
{
  trans.doTransform(&myBuffer);
}

void ArRangeBuffer::clear(void)
{
  beginRedoBuffer();
  endRedoBuffer();
}

void ArRangeBuffer::reset(void)
{
  clear();
}

void ArRangeBuffer::clearOlderThan(int milliSeconds)
{
  std::list<ArPoseWithTime *>::iterator it;

  beginInvalidationSweep();
  for (it = myBuffer.begin(); it != myBuffer.end(); ++it)
  {
    if ((*it)->getTime().mSecSince() > milliSeconds)
      invalidateReading(it);
  }
  endInvalidationSweep();
}

void ArRangeBuffer::clearOlderThanSeconds(int seconds)
{
  clearOlderThan(seconds*1000);
}

void ArRangeBuffer::beginRedoBuffer(void)
{
  myRedoIt = myBuffer.begin();
  myHitEnd = false;
  myNumRedone = 0;
}

void ArRangeBuffer::redoReading(double x, double y)
{
  if (myRedoIt != myBuffer.end() && !myHitEnd)
  {
    (*myRedoIt)->setPose(x, y);
    myRedoIt++;
  }
  // if we don't, add more (its just moving from buffers here, 
  //but let the class for this do the work
  else
  {
    addReading(x,y);
    myHitEnd = true;
  }
  myNumRedone++;
}

void ArRangeBuffer::endRedoBuffer(void)
{
  if (!myHitEnd)
  {
    // now we get rid of the extra readings on the end
    beginInvalidationSweep();
    while (myRedoIt != myBuffer.end())
    {
      invalidateReading(myRedoIt);
      myRedoIt++;
    }
    endInvalidationSweep();
  } 
}

void ArRangeBuffer::addReading(double x, double y) 
{
  if (myBuffer.size() < mySize)
  {
    if ((myIterator = myInvalidBuffer.begin()) != myInvalidBuffer.end())
    {
      myReading = (*myIterator);
      myReading->setPose(x, y);
      myReading->setTimeToNow();
      myBuffer.push_front(myReading);
      myInvalidBuffer.pop_front();
    }
    else
      myBuffer.push_front(new ArPoseWithTime(x, y));
  }
  else if ((myRevIterator = myBuffer.rbegin()) != myBuffer.rend())
  {
    myReading = (*myRevIterator);
    myReading->setPose(x, y);
    myReading->setTimeToNow();
    myBuffer.pop_back();
    myBuffer.push_front(myReading);
  }
}

void ArRangeBuffer::beginInvalidationSweep(void)
{
  myInvalidSweepList.clear();
}

void ArRangeBuffer::invalidateReading(
        std::list<ArPoseWithTime*>::iterator readingIt)
{
  myInvalidSweepList.push_front(readingIt);
}

void ArRangeBuffer::endInvalidationSweep(void)
{
  while ((myInvalidIt = myInvalidSweepList.begin()) != 
         myInvalidSweepList.end())
  {
    //printf("nuked one before %d %d\n", myBuffer.size(), myInvalidBuffer.size());
    myReading = (*(*myInvalidIt));
    myInvalidBuffer.push_front(myReading);
    myBuffer.erase((*myInvalidIt));
    myInvalidSweepList.pop_front();
    //printf("after %d %d\n", myBuffer.size(), myInvalidBuffer.size());
  }
}

std::vector<ArPoseWithTime> *ArRangeBuffer::getBufferAsVector(void)
{
  std::list<ArPoseWithTime *>::iterator it;

  myVector.clear();
  // start filling the array with the buffer until we run out of
  // readings or its full
  for (it = myBuffer.begin(); it != myBuffer.end(); it++)
  {
    myVector.insert(myVector.begin(), *(*it));
  }
  return &myVector;
}

---