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

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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


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

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

AREXPORT void ArRangeDevice::adjustRawReadings(bool interlaced)
{
  std::list<ArSensorReading *>::iterator rawIt;

  // make sure we have raw readings and a robot, and a delay to
  // correct for (note that if we don't have a delay to correct for
  // but have already been adjusting (ie someone changed the delay)
  // we'll just keep adjusting)
  if (myRawReadings == NULL || myRobot == NULL || 
      (myAdjustedRawReadings == NULL && myRobot->getOdometryDelay() == 0))
    return;
  

  // if we don't already have a list then make one
  if (myAdjustedRawReadings == NULL)
    myAdjustedRawReadings = new std::list<ArSensorReading *>;
  
  // if we've already adjusted these readings then don't do it again
  if (myRawReadings->begin() != myRawReadings->end() &&
      myRawReadings->front()->getAdjusted())
    return;

  std::list<ArSensorReading *>::iterator adjIt;
  ArSensorReading *adjReading;
  ArSensorReading *rawReading;

  ArTransform trans;
  ArTransform encTrans;
  ArTransform interlacedTrans;
  ArTransform interlacedEncTrans;

  bool first = true;
  bool second = true;

  int onReading;
  for (rawIt = myRawReadings->begin(), adjIt = myAdjustedRawReadings->begin(), 
       onReading = 0; 
       rawIt != myRawReadings->end(); 
       rawIt++, onReading++)
  {
    rawReading = (*rawIt);
    if (adjIt != myAdjustedRawReadings->end())
    {
      adjReading = (*adjIt);
      adjIt++;
    }
    else
    {
      adjReading = new ArSensorReading;
      myAdjustedRawReadings->push_back(adjReading);
    }
    (*adjReading) = (*rawReading);
    if (first || (interlaced && second))
    {
      ArPose origPose;
      ArPose corPose;
      ArPose origEncPose;
      ArPose corEncPose;
      ArTime corTime;


      corTime = rawReading->getTimeTaken();
      corTime.addMSec(-myRobot->getOdometryDelay());
      if (myRobot->getPoseInterpPosition(corTime, 
                     &corPose) == 1 && 
      myRobot->getEncoderPoseInterpPosition(corTime, 
                        &corEncPose) == 1)
      {
    origPose = rawReading->getPoseTaken();
    origEncPose = rawReading->getEncoderPoseTaken();
    /*
    printf("Difference was %g %g %g (rotVel %.0f, rotvel/40 %g)\n", 
           origEncPose.getX() - corEncPose.getX(),
           origEncPose.getY() - corEncPose.getY(),
           origEncPose.getTh() - corEncPose.getTh(),
           myRobot->getRotVel(), myRobot->getRotVel() / 40);
    */
    if (first)
    {
      trans.setTransform(origPose, corPose);
      encTrans.setTransform(origEncPose, corEncPose);
    }
    else if (interlaced && second)
    {
      interlacedTrans.setTransform(origPose, corPose);
      interlacedEncTrans.setTransform(origEncPose, corEncPose);
    }
      }
      else
      {
    //printf("Couldn't correct\n");
      }

      if (first)
    first = false;
      else if (interlaced && second)
    second = false;

    }
    if (!interlaced && (onReading % 2) == 0)
    {
      adjReading->applyTransform(trans);
      adjReading->applyEncoderTransform(encTrans);
    }
    else
    {
      adjReading->applyTransform(interlacedTrans);
      adjReading->applyEncoderTransform(interlacedEncTrans);
    }
    /*
    if (fabs(adjReading->getEncoderPoseTaken().getX() - 
         corEncPose.getX()) > 1 ||
    fabs(adjReading->getEncoderPoseTaken().getY() - 
         corEncPose.getY()) > 1 || 
    fabs(ArMath::subAngle(adjReading->getEncoderPoseTaken().getTh(), 
                  corEncPose.getTh())) > .2)
      printf("(%.0f %.0f %.0f) should be (%.0f %.0f %.0f)\n", 
         adjReading->getEncoderPoseTaken().getX(),
         adjReading->getEncoderPoseTaken().getY(),
         adjReading->getEncoderPoseTaken().getTh(),
         corEncPose.getX(), corEncPose.getY(),  corEncPose.getTh());
    */
    adjReading->setAdjusted(true);
    rawReading->setAdjusted(true);
  }  
}

---