ArRangeDevice.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 "ArRangeDevice.h"
#include "ArRobot.h"

ArRangeDevice::ArRangeDevice(size_t currentBufferSize,
                                      size_t cumulativeBufferSize, 
                                      const char *name, 
                                      unsigned int maxRange,
                                      int maxSecondsToKeepCurrent, 
                                      int maxSecondsToKeepCumulative,
                                      double maxDistToKeepCumulative) :
  myCurrentBuffer(currentBufferSize),
  myCumulativeBuffer(cumulativeBufferSize),
  myFilterCB(this, &ArRangeDevice::filterCallback)
{
  myRobot = NULL;
  myName = name;
  myMaxRange = maxRange;
  myRawReadings = NULL;
  setMaxSecondsToKeepCurrent(maxSecondsToKeepCurrent);
  setMaxSecondsToKeepCumulative(maxSecondsToKeepCumulative);
  setMaxDistToKeepCumulative(maxDistToKeepCumulative);
  myCurrentDrawingData = NULL;
  myOwnCurrentDrawingData = false;
  myCumulativeDrawingData = NULL;
  myOwnCumulativeDrawingData = false;
}

ArRangeDevice::~ArRangeDevice()
{
  if (myRobot != NULL)
    myRobot->remSensorInterpTask(&myFilterCB);
  if (myCurrentDrawingData != NULL && myOwnCurrentDrawingData)
  {
    delete myCurrentDrawingData;
    myCurrentDrawingData = NULL;
    myOwnCurrentDrawingData = false;
  }
  if (myCumulativeDrawingData != NULL && myOwnCumulativeDrawingData)
  {
    delete myCumulativeDrawingData;
    myCumulativeDrawingData = NULL;
    myOwnCumulativeDrawingData = false;
  }
}


const char * ArRangeDevice::getName(void) const
{ 
  return myName.c_str(); 
}

void ArRangeDevice::setRobot(ArRobot *robot) 
{ 
  char buf[512];
  sprintf(buf, "filter %s", getName());

  if (myRobot != NULL)
    myRobot->remSensorInterpTask(&myFilterCB);

  myRobot = robot;

  if (myRobot != NULL)
    myRobot->addSensorInterpTask(buf, 100, &myFilterCB);
}

ArRobot *ArRangeDevice::getRobot(void) 
{
  return myRobot; 
}

void ArRangeDevice::filterCallback(void)
{
  std::list<ArPoseWithTime *>::iterator it;

  lockDevice();
  // first filter the current readings based on time
  if (myMaxSecondsToKeepCurrent > 0 && 
      myCurrentBuffer.getSize() > 0)
  {
    // just walk through and make sure nothings too far away
    myCurrentBuffer.beginInvalidationSweep();
    for (it = getCurrentBuffer()->begin(); 
         it != getCurrentBuffer()->end(); 
         ++it)
    {
      if ((*it)->getTime().secSince() >= myMaxSecondsToKeepCurrent)
        myCurrentBuffer.invalidateReading(it);
    }
    myCurrentBuffer.endInvalidationSweep();
  }
  
  // okay done with current, now do the cumulative
  bool doingDist = true;
  bool doingAge = true;

  if (myCumulativeBuffer.getSize() == 0)
  {
    unlockDevice();
    return;
  }

  double squaredFarDist = (myMaxDistToKeepCumulative * 
                           myMaxDistToKeepCumulative);

  if (squaredFarDist < 1)
    doingDist = false;
  if (myMaxSecondsToKeepCumulative <= 0)
    doingAge = false;
                    
  if (!doingDist && !doingAge)
  {
    unlockDevice();
    return;
  }

  // just walk through and make sure nothings too far away
  myCumulativeBuffer.beginInvalidationSweep();
  for (it = getCumulativeBuffer()->begin(); 
       it != getCumulativeBuffer()->end(); 
       ++it)
  {
    // if its closer to a reading than the filter near dist, just return
    if (doingDist && 
        myRobot->getPose().squaredFindDistanceTo(*(*it)) > squaredFarDist)
      myCumulativeBuffer.invalidateReading(it);
    else if (doingAge && 
             (*it)->getTime().secSince() >= myMaxSecondsToKeepCumulative)
      myCumulativeBuffer.invalidateReading(it);
  }
  myCumulativeBuffer.endInvalidationSweep();
  unlockDevice();
}

void ArRangeDevice::setCurrentBufferSize(size_t size)
{
  myCurrentBuffer.setSize(size);
}

void ArRangeDevice::setCumulativeBufferSize(size_t size)
{
  myCumulativeBuffer.setSize(size);
}

void ArRangeDevice::addReading(double x, double y)
{
  myCurrentBuffer.addReading(x, y);
  myCumulativeBuffer.addReading(x, y);
}

double ArRangeDevice::currentReadingPolar(double startAngle,
                                                   double endAngle,
                                                   double *angle) const
{
  ArPose pose;
  if (myRobot != NULL)
    pose = myRobot->getPose();
  else
    {
      ArLog::log(ArLog::Normal, "ArRangeDevice %s: NULL robot, won't get polar reading correctly", getName());
      pose.setPose(0, 0);
    }
  return myCurrentBuffer.getClosestPolar(startAngle, endAngle, 
                                         pose,
                                         myMaxRange,
                                         angle);
}

double ArRangeDevice::cumulativeReadingPolar(double startAngle,
                                                      double endAngle,
                                                      double *angle) const
{
  ArPose pose;
  if (myRobot != NULL)
    pose = myRobot->getPose();
  else
    {
      ArLog::log(ArLog::Normal, "ArRangeDevice %s: NULL robot, won't get polar reading correctly", getName());
      pose.setPose(0, 0);
    }
  return myCumulativeBuffer.getClosestPolar(startAngle, endAngle, 
                                            pose,
                                            myMaxRange,
                                            angle);
}

double ArRangeDevice::currentReadingBox(double x1, double y1, 
                                                 double x2, double y2,
                                                 ArPose *pose) const
{
  ArPose robotPose;
  if (myRobot != NULL)
      robotPose = myRobot->getPose();
  else
    {
      ArLog::log(ArLog::Normal, "ArRangeDevice %s: NULL robot, won't get reading box correctly", getName());
      robotPose.setPose(0, 0);
    }
  return myCurrentBuffer.getClosestBox(x1, y1, x2, y2, robotPose,
                                       myMaxRange, pose);
}

double ArRangeDevice::cumulativeReadingBox(double x1, double y1, 
                                                 double x2, double y2,
                                                 ArPose *pose) const
{
  ArPose robotPose;
  if (myRobot != NULL)
    robotPose = myRobot->getPose();
  else
    {
      ArLog::log(ArLog::Normal, "ArRangeDevice %s: NULL robot, won't get reading box correctly", getName());
      robotPose.setPose(0, 0);
    }
  return myCumulativeBuffer.getClosestBox(x1, y1, x2, y2, 
                                          robotPose,
                                          myMaxRange, pose);
}

void ArRangeDevice::applyTransform(ArTransform trans, 
                                            bool doCumulative)
{
  myCurrentBuffer.applyTransform(trans);
  if (doCumulative)
    myCumulativeBuffer.applyTransform(trans);
}

std::vector<ArSensorReading> *ArRangeDevice::getRawReadingsAsVector(void)
{
  
  std::list<ArSensorReading *>::const_iterator it;
  myRawReadingsVector.clear();
  // if we don't have any return an empty list
  if (myRawReadings == NULL)
    return &myRawReadingsVector;
  myRawReadingsVector.reserve(myRawReadings->size());
  for (it = myRawReadings->begin(); it != myRawReadings->end(); it++)
    myRawReadingsVector.insert(myRawReadingsVector.begin(), *(*it));
  return &myRawReadingsVector;
}


void ArRangeDevice::setCurrentDrawingData(ArDrawingData *data, 
                                                   bool takeOwnershipOfData)
{
  if (myCurrentDrawingData != NULL && myOwnCurrentDrawingData)
  {
    delete myCurrentDrawingData;
    myCurrentDrawingData = NULL;
    myOwnCurrentDrawingData = false;
  }
  myCurrentDrawingData = data; 
  myOwnCurrentDrawingData = takeOwnershipOfData; 
}

 void ArRangeDevice::setCumulativeDrawingData(ArDrawingData *data, 
                                                      bool takeOwnershipOfData)
{
  if (myCumulativeDrawingData != NULL && myOwnCumulativeDrawingData)
  {
    delete myCumulativeDrawingData;
    myCumulativeDrawingData = NULL;
    myOwnCumulativeDrawingData = false;
  }
  myCumulativeDrawingData = data; 
  myOwnCumulativeDrawingData = takeOwnershipOfData; 
}

---