ArRangeBuffer.cpp

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     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 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/

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

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

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

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

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

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

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

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

AREXPORT 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;
    }
  }
}

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

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


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

AREXPORT 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;  
}

AREXPORT 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);
}

AREXPORT 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;
}

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

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

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

AREXPORT 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();
}

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

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

AREXPORT 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++;
}

AREXPORT 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();
  } 
}

AREXPORT 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();
}

AREXPORT 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());
  }
}

AREXPORT 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;
}

---