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

AREXPORT ArForbiddenRangeDevice::ArForbiddenRangeDevice(
    ArMap *armap, double distanceIncrement, unsigned int maxRange,
    const char *name) :
  ArRangeDevice(32000, 0, name, maxRange, 0, 0, 0, true),
  myProcessCB(this, &ArForbiddenRangeDevice::processReadings),
  myMapChangedCB(this, &ArForbiddenRangeDevice::processMap)  ,
  myEnableCB(this, &ArForbiddenRangeDevice::enable),
  myDisableCB(this, &ArForbiddenRangeDevice::disable)
{
  myIsEnabled = true;
  myMap = armap;
  myDistanceIncrement = distanceIncrement;
  myMapChangedCB.setName("ArForbiddenRangeDevice");
}

AREXPORT ArForbiddenRangeDevice::~ArForbiddenRangeDevice()
{

}

AREXPORT void ArForbiddenRangeDevice::processMap(void)
{
  std::list<ArMapObject *>::const_iterator it;
  ArMapObject *obj;

  myDataMutex.lock();
  ArUtil::deleteSet(mySegments.begin(), mySegments.end());
  mySegments.clear();

  for (it = myMap->getMapObjects()->begin();
       it != myMap->getMapObjects()->end();
       it++)
  {
    obj = (*it);
    if (strcmp(obj->getType(), "ForbiddenLine") == 0 &&
    obj->hasFromTo())
    {
      mySegments.push_back(new ArLineSegment(obj->getFromPose(), 
                         obj->getToPose()));
    }
    if (strcmp(obj->getType(), "ForbiddenArea") == 0 &&
    obj->hasFromTo())
    {
      double angle = obj->getPose().getTh();
      double sa = ArMath::sin(angle);
      double ca = ArMath::cos(angle);
      double fx = obj->getFromPose().getX();
      double fy = obj->getFromPose().getY();
      double tx = obj->getToPose().getX();
      double ty = obj->getToPose().getY();
      ArPose P0((fx*ca - fy*sa), (fx*sa + fy*ca));
      ArPose P1((tx*ca - fy*sa), (tx*sa + fy*ca));
      ArPose P2((tx*ca - ty*sa), (tx*sa + ty*ca));
      ArPose P3((fx*ca - ty*sa), (fx*sa + ty*ca));
      mySegments.push_back(new ArLineSegment(P0, P1));
      mySegments.push_back(new ArLineSegment(P1, P2));
      mySegments.push_back(new ArLineSegment(P2, P3));
      mySegments.push_back(new ArLineSegment(P3, P0));
    }
  }
  myDataMutex.unlock();
}

AREXPORT void ArForbiddenRangeDevice::processReadings(void)
{
  ArPose intersection;
  std::list<ArLineSegment *>::iterator it;
  
  lockDevice();
  myDataMutex.lock();

  myCurrentBuffer.beginRedoBuffer();

  if (!myIsEnabled)
  {
    myCurrentBuffer.endRedoBuffer();
    myDataMutex.unlock();
    unlockDevice();
    return;
  }

  ArLineSegment *segment;
  ArPose start;
  double startX;
  double startY;
  ArPose end;
  double angle;
  double length;
  double gone;
  double sin;
  double cos;
  double atX;
  double atY;
  double robotX = myRobot->getX();
  double robotY = myRobot->getY();
  double max = (double) myMaxRange;
  double maxSquared = (double) myMaxRange * (double) myMaxRange;
  ArTime startingTime;
  //startingTime.setToNow();
  // now see if the end points of the segments are too close to us
  for (it = mySegments.begin(); it != mySegments.end(); it++)
  {
    segment = (*it);
    // if either end point or some perpindicular point is close to us
    // add the line's data
    if (ArMath::squaredDistanceBetween(
        segment->getX1(), segment->getY1(), 
        myRobot->getX(), myRobot->getY()) < maxSquared ||
    ArMath::squaredDistanceBetween(
        segment->getX2(), segment->getY2(), 
        myRobot->getX(), myRobot->getY()) < maxSquared ||
    segment->getPerpDist(myRobot->getPose()) < max)
    {
      start.setPose(segment->getX1(), segment->getY1());
      end.setPose(segment->getX2(), segment->getY2());
      angle = start.findAngleTo(end);
      cos = ArMath::cos(angle);
      sin = ArMath::sin(angle);
      startX = start.getX();
      startY = start.getY();
      length = start.findDistanceTo(end);
      // first put in the start point if we should
      if (ArMath::squaredDistanceBetween(
          startX, startY, robotX, robotY) < maxSquared)
    myCurrentBuffer.redoReading(start.getX(), start.getY());
      // now walk the length of the line and see if we should put the points in
      for (gone = 0; gone < length; gone += myDistanceIncrement)
      {
    atX = startX + gone * cos;
    atY = startY + gone * sin;
    if (ArMath::squaredDistanceBetween(
        atX, atY, robotX, robotY) < maxSquared)
      myCurrentBuffer.redoReading(atX, atY);
      }
      // now check the end point
      if (end.squaredFindDistanceTo(myRobot->getPose()) < maxSquared)
    myCurrentBuffer.redoReading(end.getX(), end.getY());
    }
  }
  myDataMutex.unlock();
  // and we're done
  myCurrentBuffer.endRedoBuffer();
  unlockDevice();
  //printf("%d\n", startingTime.mSecSince());
}

AREXPORT void ArForbiddenRangeDevice::setRobot(ArRobot *robot)
{
  myRobot = robot;
  if (myRobot != NULL)
    myRobot->addSensorInterpTask(myName.c_str(), 20, &myProcessCB);
  ArRangeDevice::setRobot(robot);
  myMap->lock();
  myMap->addMapChangedCB(&myMapChangedCB);
  processMap();
  myMap->unlock();
}

AREXPORT void ArForbiddenRangeDevice::enable(void)
{
  myDataMutex.lock();
  myIsEnabled = true;
  myDataMutex.unlock();
}

AREXPORT void ArForbiddenRangeDevice::disable(void)
{
  myDataMutex.lock();
  myIsEnabled = false;
  myDataMutex.unlock();
}

---