rosaria/Legacy/Aria/src/ArLaser.cpp

/*
Adept MobileRobots Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc.
Copyright (C) 2011, 2012, 2013 Adept Technology

     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 
Adept MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960
*/
#include "ArExport.h"
#include "ariaOSDef.h"
#include "ArLaser.h"
#include "ArRobot.h"
#include "ArDeviceConnection.h"

bool ArLaser::ourUseSimpleNaming = false;

AREXPORT ArLaser::ArLaser(
	int laserNumber, const char *name, 
	unsigned int absoluteMaxRange, bool locationDependent, 
	bool appendLaserNumberToName) :
  ArRangeDeviceThreaded(
	  361, 200, name, absoluteMaxRange,
	  0, 0, 0, locationDependent)
{
  myLaserNumber = laserNumber;

  if (appendLaserNumberToName)
  {
    char buf[1024];
    snprintf(buf, sizeof(buf) - 20, "%s", name);
    sprintf(buf, "%s_%d", buf, myLaserNumber);
    myName = buf;
  }
  else
  {
    if (laserNumber != 1)
      ArLog::log(ArLog::Verbose, "ArLaser::%s: Laser created with number %d, but the number is not appended to the name which may break things (especially since this number is greater than 1_", name, laserNumber);

    myName = name;
  }


  laserSetName(myName.c_str());

  myAbsoluteMaxRange = absoluteMaxRange;
  myMaxRangeSet = false;

  setSensorPosition(0, 0, 0, 0);
  myTimeoutSeconds = 8;

  myHaveSensorPose = false;

  myFlipped = false;
  myFlippedSet = false;

  myCanSetDegrees = false;
  myStartDegreesMin = HUGE_VAL;
  myStartDegreesMax = -HUGE_VAL;
  myStartDegreesSet = false;
  myStartDegrees = 0;
  myEndDegreesMin = HUGE_VAL;
  myEndDegreesMax = -HUGE_VAL;
  myEndDegreesSet = false;
  myEndDegrees = 0;

  myCanChooseDegrees = false;
  myDegreesChoiceDouble = -HUGE_VAL;

  myCanSetIncrement = 0;
  myIncrementMin = HUGE_VAL; 
  myIncrementMax = -HUGE_VAL; 
  myIncrementSet = false;
  myIncrement = 0;

  myCanChooseIncrement = false; 
  myIncrementChoiceDouble = -HUGE_VAL;

  myCanChooseUnits = false;

  myCanChooseReflectorBits = false;

  myCanSetPowerControlled = false;
  myPowerControlled = true;
  myPowerControlledSet = false;

  myCanChooseStartingBaud = false;

  myCanChooseAutoBaud = false;
  
  myDefaultTcpPort = 8102;

  myInfoLogLevel = ArLog::Verbose;
  myRobotRunningAndConnected = false;
}

AREXPORT ArLaser::~ArLaser()
{
}

/**
   This can be used to set the name on mutexes and such to match the
   laser's new name.
**/
AREXPORT void ArLaser::laserSetName(const char *name)
{
  if (ourUseSimpleNaming)
  {
    myName = "Laser_";
    char buf[1024];
    sprintf(buf, "%d", myLaserNumber);
    myName += buf;
  }    
  else
  {
    myName = name;
  }


  myTask.setThreadName(myName.c_str());

  myConnectCBList.setNameVar("%s::myConnectCBList", myName.c_str());
  myFailedConnectCBList.setNameVar("%s::myFailedConnectCBList", myName.c_str());
  myDisconnectOnErrorCBList.setNameVar(
	  "%s::myDisconnectOnErrorCBList", myName.c_str());
  myDisconnectNormallyCBList.setNameVar(
	  "%s::myDisconnectNormallyCBList", myName.c_str());
  myDataCBList.setNameVar("%s::myDataCBList", myName.c_str());
  //myDataCBList.setLogging(false); // supress debug logging since it drowns out all other logging
}

AREXPORT void ArLaser::setMaxRange(unsigned int maxRange)
{
  if (maxRange > myAbsoluteMaxRange)
  {
    ArLog::log(ArLog::Terse, "%s::setMaxRange: Tried to set the max range to %u which is above the absoluteMaxRange on the device of %d, capping it", 
	       getName(), maxRange, getAbsoluteMaxRange());
    ArRangeDevice::setMaxRange(myAbsoluteMaxRange);
  }
  else
    ArRangeDevice::setMaxRange(maxRange);

  myMaxRangeSet = true;
}

AREXPORT void ArLaser::setCumulativeBufferSize(size_t size)
{
  ArRangeDevice::setCumulativeBufferSize(size);
  myCumulativeBufferSizeSet = true;
}


AREXPORT void ArLaser::laserSetAbsoluteMaxRange(unsigned int absoluteMaxRange)
{ 
  ArLog::log(myInfoLogLevel, "%s: Setting absolute max range to %u", 
	     getName(), absoluteMaxRange);
  myAbsoluteMaxRange = absoluteMaxRange; 
  setMaxRange(getMaxRange());
}

/**
   Filter readings, moving them from the raw current buffer to
   filtered current buffer (see ArRangeDevice), and then also to the
   cumulative buffer.

   This must be called for the laser subclass to work right.

   This also calls the reading callbacks.
**/

void ArLaser::laserProcessReadings(void)
{
  // if we have no readings... don't do anything
  if (myRawReadings == NULL || myRawReadings->begin() == myRawReadings->end())
    return;

  std::list<ArSensorReading *>::iterator sensIt;
  ArSensorReading *sReading;
  double x, y;
  double lastX = 0.0, lastY = 0.0;
  //unsigned int i = 0;
  ArTime len;
  len.setToNow();

  bool clean;
  if (myCumulativeCleanInterval <= 0 ||
      (myCumulativeLastClean.mSecSince() > 
       myCumulativeCleanInterval))
  {
    myCumulativeLastClean.setToNow();
    clean = true;
  }
  else
  {
    clean = false;
  }
  
  myCurrentBuffer.setPoseTaken(myRawReadings->front()->getPoseTaken());
  myCurrentBuffer.setEncoderPoseTaken(
	  myRawReadings->front()->getEncoderPoseTaken());
  myCurrentBuffer.beginRedoBuffer();	  

  // walk the buffer of all the readings and see if we want to add them
  for (sensIt = myRawReadings->begin(); 
       sensIt != myRawReadings->end(); 
       ++sensIt)
  {
    sReading = (*sensIt);

    // if we have ignore readings then check them here
    if (!myIgnoreReadings.empty() && 
	(myIgnoreReadings.find(
		(int) ceil(sReading->getSensorTh())) != 
	 myIgnoreReadings.end()) || 
	myIgnoreReadings.find(
		(int) floor(sReading->getSensorTh())) != 
	myIgnoreReadings.end())
      sReading->setIgnoreThisReading(true);

    // see if the reading is valid
    if (sReading->getIgnoreThisReading())
      continue;

    // if we have a max range then check it here... 
    if (myMaxRange != 0 && 
	sReading->getRange() > myMaxRange)
    {
      sReading->setIgnoreThisReading(true);
    }

    // see if the reading is valid... this is set up this way so that
    // max range readings can cancel out other readings, but will
    // still be ignored other than that... ones ignored for other
    // reasons were skipped above
    if (sReading->getIgnoreThisReading())
    {
      internalProcessReading(sReading->getX(), sReading->getY(), 
			     sReading->getRange(), clean, true);
      continue;
    }

    // get our coords
    x = sReading->getX();
    y = sReading->getY();
    
    // see if we're checking on the filter near dist... if we are
    // and the reading is a good one we'll check the cumulative
    // buffer
    if (myMinDistBetweenCurrentSquared > 0.0000001)
    {
      // see where the last reading was
      //squaredDist = (x-lastX)*(x-lastX) + (y-lastY)*(y-lastY);
      // see if the reading is far enough from the last reading
      if (ArMath::squaredDistanceBetween(x, y, lastX, lastY) > 
	  myMinDistBetweenCurrentSquared)
      {
	lastX = x;
	lastY = y;
	// since it was a good reading, see if we should toss it in
	// the cumulative buffer... 
	internalProcessReading(x, y, sReading->getRange(), clean, false);
	
	/* we don't do this part anymore since it wound up leaving
	// too many things not really tehre... if its outside of our
	// sensor angle to use to filter then don't let this one
	// clean  (ArMath::fabs(sReading->getSensorTh()) > 50)
	// filterAddAndCleanCumulative(x, y, false); else*/
      }
      // it wasn't far enough, skip this one and go to the next one
      else
      {
	continue;		
      }
    }
    // we weren't filtering the readings, but see if it goes in the
    // cumulative buffer anyways
    else
    {
      internalProcessReading(x, y, sReading->getRange(), clean, false);
    }
    // now drop the reading into the current buffer
    myCurrentBuffer.redoReading(x, y);
    //i++;
  }
  myCurrentBuffer.endRedoBuffer();
  /*  Put this in to see how long the cumulative filtering is taking  
  if (clean)
    printf("### %ld %d\n", len.mSecSince(), myCumulativeBuffer.getBuffer()->size());
    */
  internalGotReading();
}


void ArLaser::internalProcessReading(double x, double y, 
				     unsigned int range, bool clean,
				     bool onlyClean)
{
  if (myCumulativeBuffer.getSize() == 0)
    return;

  // make sure we really want to clean
  if (clean && myCumulativeCleanDistSquared < 1)
    clean = false;


  std::list<ArPoseWithTime *>::iterator cit;
  bool addReading = true;

  //double squaredDist;

  ArLineSegment line;
  double xTaken = myCurrentBuffer.getPoseTaken().getX();
  double yTaken = myCurrentBuffer.getPoseTaken().getY();
  ArPose intersection;
  ArPoseWithTime reading(x, y);

  // if we're not cleaning and its further than we're keeping track of
  // readings ignore it... replaced with the part thats 'until here'
  /*
  if (!clean && 
      myMaxInsertDistCumulative > 1 && 
      range > myMaxInsertDistCumulative)
    return;
  */

  if (onlyClean)
    addReading = false;

  if (myMaxInsertDistCumulative > 1 && 
      range > myMaxInsertDistCumulative)
    addReading = false;

  if (!clean && !addReading)
    return;
  // until here
  
  // if we're cleaning we start our sweep
  if (clean)
    myCumulativeBuffer.beginInvalidationSweep();
  // run through all the readings
  for (cit = getCumulativeBuffer()->begin(); 
       cit != getCumulativeBuffer()->end(); 
       ++cit)
  {
    // if its closer to a reading than the filter near dist, just return
    if (addReading && myMinDistBetweenCumulativeSquared < .0000001 ||
	(ArMath::squaredDistanceBetween(x, y, (*cit)->getX(), (*cit)->getY()) <
	 myMinDistBetweenCumulativeSquared))
    {
      // if we're not cleaning it and its too close just return,
      // otherwise keep going (to clear out invalid readings)
      if (!clean)
	return;
      addReading = false;
    }
    // see if this reading invalidates some other readings by coming too close
    if (clean)
    {
      // set up our line
      line.newEndPoints(x, y, xTaken, yTaken);
      // see if the cumulative buffer reading perpindicular intersects
      // this line segment, and then see if its too close if it does,
      // but if the intersection is very near the endpoint then leave it
      if (line.getPerpPoint((*cit), &intersection) &&
	  (intersection.squaredFindDistanceTo(*(*cit)) < 
	   myCumulativeCleanDistSquared) &&
	  (intersection.squaredFindDistanceTo(reading) > 
	   50 * 50))
      {
	//printf("Found one too close to the line\n");
	myCumulativeBuffer.invalidateReading(cit);
      }
    }
  }
  // if we're cleaning finish the sweep
  if (clean)
    myCumulativeBuffer.endInvalidationSweep();
  // toss the reading in
  if (addReading)
    myCumulativeBuffer.addReading(x, y);

}

AREXPORT bool ArLaser::laserPullUnsetParamsFromRobot(void)
{
  if (myRobot == NULL)
  {
    ArLog::log(ArLog::Normal, "%s: Trying to connect, but have no robot, continuing under the assumption this is intentional", getName());
    return true;
  }

  const ArRobotParams *params = myRobot->getRobotParams();
  if (params == NULL)
  {
    ArLog::log(ArLog::Terse, 
	       "%s: Robot has no params, cannot pull unset params from robot",
	       getName());
    return false;
  }

  const char *paramStr;
  char *endPtr;
  double paramDouble;
  int paramInt;
  bool paramBool;

  paramBool = params->getLaserFlipped(getLaserNumber());
  if (!myFlippedSet)
  {
    if (paramBool)
    {
      ArLog::log(myInfoLogLevel, 
		 "%s: Setting flipped to true from robot params",
		 getName());
      setFlipped(true);
    }
    else if (!paramBool)
    {
      ArLog::log(myInfoLogLevel, 
		 "%s: Setting flipped to false from robot params",
		 getName());
      setFlipped(false);
    }
  }

  paramInt = params->getLaserMaxRange(getLaserNumber());
  if (!myMaxRangeSet)
  {
    if(paramInt < 0)
    {
      ArLog::log(ArLog::Terse, "%s: LaserMaxRange in robot param file was negative but shouldn't be (it was '%d'), failing", getName(), paramInt);
      return false;
    }
    if (paramInt > 0)
    {
      ArLog::log(myInfoLogLevel, 
		 "%s: Setting max range to %d from robot params",
		 getName(), paramInt);
      setMaxRange(paramInt);
    }
  }

  paramInt = params->getLaserCumulativeBufferSize(getLaserNumber());
  if (!myCumulativeBufferSizeSet)
  {
    if(paramInt < 0)
    {
      ArLog::log(ArLog::Terse, "%s: LaserCumulativeBufferSize in robot param file was negative but shouldn't be (it was '%d'), failing", getName(), paramInt);
      return false;
    }
    if (paramInt > 0)
    {
      ArLog::log(myInfoLogLevel, 
		 "%s: Setting cumulative buffer size to %d from robot params",
		 getName(), paramInt);
      setCumulativeBufferSize(paramInt);
    }
  }

  paramStr = params->getLaserStartDegrees(getLaserNumber());
  if (canSetDegrees() && !myStartDegreesSet && 
      paramStr != NULL && paramStr[0] != '\0')
  {
    paramDouble = strtod(paramStr, &endPtr);
    if(endPtr == paramStr)
    {
      ArLog::log(ArLog::Terse, "%s: LaserStartDegrees in robot param file was not a double (it was '%s'), failing", getName(), paramStr);
      return false;
    }
    ArLog::log(myInfoLogLevel, 
	       "%s: Setting start degrees to %g from robot params",
	       getName(), paramDouble);
    setStartDegrees(paramDouble);
  }

  paramStr = params->getLaserEndDegrees(getLaserNumber());
  if (canSetDegrees() && !myEndDegreesSet && 
      paramStr != NULL && paramStr[0] != '\0')
  {
    paramDouble = strtod(paramStr, &endPtr);
    if(endPtr == paramStr)
    {
      ArLog::log(ArLog::Terse, 
		 "%s: LaserEndDegrees in robot param file was not a double (it was '%s'), failing", 
		 getName(), paramStr);
      return false;
    }
    ArLog::log(myInfoLogLevel, 
	       "%s: Setting end degrees to %g from robot params",
	       getName(), paramDouble);
    setEndDegrees(paramDouble);
  }

  paramStr = params->getLaserDegreesChoice(getLaserNumber());
  if (canChooseDegrees() && !myDegreesChoiceSet && 
      paramStr != NULL && paramStr[0] != '\0')
  {
    ArLog::log(myInfoLogLevel, 
	       "%s: Setting degrees choice to %s from robot params",
	       getName(), paramStr);
    chooseDegrees(paramStr);
  }

  paramStr = params->getLaserIncrement(getLaserNumber());
  if (canSetDegrees() && !myIncrementSet && 
      paramStr != NULL && paramStr[0] != '\0')
  {
    paramDouble = strtod(paramStr, &endPtr);
    if(endPtr == paramStr)
    {
      ArLog::log(ArLog::Terse, 
		 "%s: LaserIncrement in robot param file was not a double (it was '%s'), failing", 
		 getName(), paramStr);
      return false;
    }
    ArLog::log(myInfoLogLevel, 
	       "%s: Setting increment to %g from robot params",
	       getName(), paramDouble);
    setIncrement(paramDouble);
  }

  paramStr = params->getLaserIncrementChoice(getLaserNumber());
  if (canChooseIncrement() && !myIncrementChoiceSet && 
      paramStr != NULL && paramStr[0] != '\0')
  {
    ArLog::log(myInfoLogLevel, 
	       "%s: Setting increment choice to %s from robot params",
	       getName(), paramStr);
    chooseIncrement(paramStr);
  }

  paramStr = params->getLaserUnitsChoice(getLaserNumber());
  if (canChooseUnits() && !myUnitsChoiceSet && 
      paramStr != NULL && paramStr[0] != '\0')
  {
    ArLog::log(myInfoLogLevel, 
	       "%s: Setting units choice to %s from robot params",
	       getName(), paramStr);
    chooseUnits(paramStr);
  }

  paramStr = params->getLaserReflectorBitsChoice(getLaserNumber());
  if (canChooseReflectorBits() && !myReflectorBitsChoiceSet && 
      paramStr != NULL && paramStr[0] != '\0')
  {
    ArLog::log(myInfoLogLevel, 
	       "%s: Setting reflectorBits choice to %s from robot params",
	       getName(), paramStr);
    chooseReflectorBits(paramStr);
  }

  paramBool = params->getLaserPowerControlled(getLaserNumber());
  if (canSetPowerControlled() && !myPowerControlledSet)
  {
    if (paramBool)
    {
      ArLog::log(myInfoLogLevel, 
		 "%s: Setting powerControlled to true from robot params",
		 getName());
      setPowerControlled(true);
    }
    else if (!paramBool)
    {
      ArLog::log(myInfoLogLevel, 
		 "%s: Setting powerControlled to false from robot params",
		 getName());
      setPowerControlled(false);
    }
  }

  paramStr = params->getLaserStartingBaudChoice(getLaserNumber());
  if (canChooseStartingBaud() && !myStartingBaudChoiceSet && 
      paramStr != NULL && paramStr[0] != '\0')
  {
    ArLog::log(myInfoLogLevel,
	       "%s: Setting startingBaud choice to %s from robot params",
	       getName(), paramStr);
    chooseStartingBaud(paramStr);
  }

  paramStr = params->getLaserAutoBaudChoice(getLaserNumber());
  if (canChooseAutoBaud() && !myAutoBaudChoiceSet && 
      paramStr != NULL && paramStr[0] != '\0')
  {
    ArLog::log(myInfoLogLevel, 
	       "%s: Setting autoBaud choice to %s from robot params",
	       getName(), paramStr);
    chooseAutoBaud(paramStr);
  }

  if (!addIgnoreReadings(params->getLaserIgnore(getLaserNumber())))
    return false;

  setSensorPosition(params->getLaserX(getLaserNumber()), 
		    params->getLaserY(getLaserNumber()), 
		    params->getLaserTh(getLaserNumber()),
		    params->getLaserZ(getLaserNumber()));

  return true;
}

AREXPORT void ArLaser::setDeviceConnection(ArDeviceConnection *conn)
{
  myConnMutex.lock();
  myConn = conn; 
  myConn->setDeviceName(getName());
  myConnMutex.unlock();
}

AREXPORT ArDeviceConnection *ArLaser::getDeviceConnection(void)
{
  return myConn;
}

/**
   Sets the time to go without a response from the laser
   until it is assumed that the connection with the laser has been
   broken and the disconnect on error events will happen.

   If there is no robot then there is a straightforward check of last
   reading time against this value.  If there is a robot then it will
   not start the check until the laser is running and connected.

   @param seconds if 0 or less then the connection timeout feature
   will be disabled, otherwise disconnect on error will be triggered
   after this number of miliseconds...  
**/
AREXPORT void ArLaser::setConnectionTimeoutSeconds(double seconds)
{
  ArLog::log(ArLog::Normal, 
	     "%s::setConnectionTimeoutSeconds: Setting timeout to %g secs", 
	     getName(), seconds);
  myLastReading.setToNow();

  if (seconds > 0)
    myTimeoutSeconds = seconds;
  else
    myTimeoutSeconds = 0;
}

/**
   Gets the time (miliseconds) to go without response from the laser
   until it is assumed that the connection with the laser has been
   broken and the disconnect on error events will happen.
   If 0, then the timeout is disabled.

   If there is no robot then there is a straightforward check of last
   reading time against this value.  If there is a robot then it will
   not start the check until the laser is running and connected.
**/
double ArLaser::getConnectionTimeoutSeconds(void)
{
  return myTimeoutSeconds;
}

AREXPORT void ArLaser::laserConnect(void)
{
  // figure out how many readings we can have and set the current
  // buffer size to that
  double degrees;

  myLastReading.setToNow();

  if (canSetDegrees())
  {
    //degrees = fabs(ArMath::subAngle(getStartDegrees(), getEndDegrees()));
    degrees = fabs(getStartDegrees() - getEndDegrees());
    ArLog::log(myInfoLogLevel, 
	       "%s: Using degrees settings of %g to %g for %g degrees",
	       getName(), getStartDegrees(), getEndDegrees(), 
	       degrees);
  }
  else if (canChooseDegrees())
  {
    degrees = getDegreesChoiceDouble();
    ArLog::log(myInfoLogLevel, "%s: Using choice of %g degrees",
	       getName(), degrees);
  }
  else
  {
    degrees = 360;
    ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 360", getName());
  }

  double increment;
  if (canSetIncrement())
  {
    increment = getIncrement();
    ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
	       getName(), increment);
  }
  else if (canChooseIncrement())
  {
    increment = getIncrementChoiceDouble();
    ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
	       getName(), increment);
  }
  else
  {
	// PS 10/20/11 - This was missing causing buffer size to be very large
	// set this to the lowest, note both the SZ and S3 are setting the buffer
	// size but it's being overriden by this procedure - do we want to fix
	// this or just leave it at the max value 360/.25=1440???
	increment = .25;
    ArLog::log(ArLog::Terse, "%s: Don't have any settings for increment, arbitrarily using .25", getName());
  }
  
  int size = (int)ceil(degrees / increment) + 1;
  ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d", 
	     getName(), size);
  setCurrentBufferSize(size);
  
  ArLog::log(myInfoLogLevel, "%s: Connected", getName());
  myConnectCBList.invoke();
}

AREXPORT void ArLaser::laserFailedConnect(void)
{
  ArLog::log(myInfoLogLevel, "%s: Failed to connect", getName());
  myFailedConnectCBList.invoke();
}

AREXPORT void ArLaser::laserDisconnectNormally(void)
{
  ArLog::log(myInfoLogLevel, "%s: Disconnected normally", getName());
  myDisconnectNormallyCBList.invoke();
}

AREXPORT void ArLaser::laserDisconnectOnError(void)
{
  ArLog::log(ArLog::Normal, "%s: Disconnected because of error", getName());
  myDisconnectOnErrorCBList.invoke();
}

AREXPORT void ArLaser::internalGotReading(void)
{
  if (myTimeLastReading != time(NULL)) 
  {
    myTimeLastReading = time(NULL);
    myReadingCount = myReadingCurrentCount;
    myReadingCurrentCount = 0;
  }
  myReadingCurrentCount++;

  myLastReading.setToNow();
  
  myDataCBList.invoke();
}

AREXPORT int ArLaser::getReadingCount()
{
  if (myTimeLastReading == time(NULL))
    return myReadingCount;
  if (myTimeLastReading == time(NULL) - 1)
    return myReadingCurrentCount;
  return 0;
}





AREXPORT void ArLaser::setSensorPosition(
	double x, double y, double th, double z)
{
  setSensorPosition(ArPose(x, y, th), z);
}

AREXPORT void ArLaser::setSensorPosition(ArPose pose, double z)
{
  myHaveSensorPose = true;
  mySensorPose.setPose(pose);
  mySensorZ = z;
}

bool ArLaser::internalCheckChoice(const char *check, const char *choice, 
				  std::list<std::string> *choices,
				  const char *choicesStr)
{
  if (check == NULL || choices == NULL || choice == NULL || choice[0] == '\0')
  {
    ArLog::log(ArLog::Terse, "%s::%s: Internal error in setup");
    return false;
  }
  
  std::list<std::string>::iterator it;
  std::string str;
  for (it = choices->begin(); it != choices->end(); it++)
  {
    str = (*it);
    if (ArUtil::strcasecmp(choice, str) == 0)
      return true;
  }
  
  ArLog::log(ArLog::Terse, "%s::%s: Invalid choice, choices are <%s>.",
	     myName.c_str(), check, choicesStr);
  return false;
}

bool ArLaser::internalCheckChoice(const char *check, const char *choice, 
				  std::map<std::string, double> *choices,
				  const char *choicesStr, 
				  double *choiceDouble)
{
  if (check == NULL || choices == NULL || choice == NULL || choice[0] == '\0')
  {
    ArLog::log(ArLog::Terse, "%s::%s: Internal error in setup");
    return false;
  }
  
  std::map<std::string, double>::iterator it;
  std::string str;
  for (it = choices->begin(); it != choices->end(); it++)
  {
    str = (*it).first;
    if (ArUtil::strcasecmp(choice, str) == 0)
    {
      *choiceDouble = (*it).second;
      return true;
    }
  }

  ArLog::log(ArLog::Terse, "%s::%s: Invalid choice, choices are <%s>.",
	     myName.c_str(), check, choicesStr);
  return false;
}

void ArLaser::internalBuildChoicesString(
	std::list<std::string> *choices, std::string *str)
{
  std::list<std::string>::iterator it;
  bool first;
  std::string choiceStr;

  for (it = choices->begin(), first = true; it != choices->end(); it++)
  {
    choiceStr = (*it);
    if (!first)
      (*str) += "|";
    first = false;
    (*str) += choiceStr;
  }
}

void ArLaser::internalBuildChoices(
	std::map<std::string, double> *choices, std::string *str,
	std::list<std::string> *choicesList)
{
  std::map<std::string, double>::iterator it;
  bool first;
  std::string choiceStr;

  for (it = choices->begin(), first = true; it != choices->end(); it++)
  {
    choiceStr = (*it).first;
    choicesList->push_back(choiceStr);
    if (!first)
      (*str) += "|";
    first = false;
    (*str) += choiceStr;
  }
}

/**
   This allows the setting of the degrees the laser will use from a
   range for both starting and ending degrees.  Only one of this and
   laserAllowDegreesChoices should be used.

   @param defaultStartDegrees The default start degrees to use, this
   default should probably be for the max range.

   @param startDegreesMin The minimum value for start degrees

   @param startDegreesMax The maximum value for start degrees

   @param defaultEndDegrees The default end degrees to use, this
   default should probably be for the max range.

   @param endDegreesMin The minimum value for end degrees

   @param endDegreesMax The maximum value for end degrees
**/
AREXPORT void ArLaser::laserAllowSetDegrees(double defaultStartDegrees, double startDegreesMin, double startDegreesMax, double defaultEndDegrees, double endDegreesMin, double endDegreesMax)
{
  myCanSetDegrees = true;
  myStartDegreesMin = startDegreesMin;
  myStartDegreesMax = startDegreesMax;
  setStartDegrees(defaultStartDegrees);
  myStartDegreesSet = false;

  myEndDegreesMin = endDegreesMin;
  myEndDegreesMax = endDegreesMax;
  setEndDegrees(defaultEndDegrees);
  myEndDegreesSet = false;

}

AREXPORT bool ArLaser::setStartDegrees(double startDegrees)
{
  if (!myCanSetDegrees)
  {
    ArLog::log(ArLog::Terse, "%s::setStartDegrees: Cannot set angles on this laser", myName.c_str());
    return false;
  }
  if (startDegrees < myStartDegreesMin)
  {
    ArLog::log(ArLog::Terse, "%s::setStartDegrees: Start degrees (%g) tried to be set to less than the minimum (%g))", myName.c_str(), startDegrees, myStartDegreesMin);
    return false;
  }
  if (startDegrees > myStartDegreesMax)
  {
    ArLog::log(ArLog::Terse, "%s::setStartDegrees: Start degrees (%g) tried to be set to greater than the minimum (%g))", myName.c_str(), startDegrees, myStartDegreesMax);
    return false;
  }
  if (myEndDegreesSet && startDegrees >= myEndDegrees)
  {
    ArLog::log(ArLog::Terse, "%s::setStartDegrees: Start degrees (%g) tried to be set to greater than or equal to end degrees %g)", myName.c_str(), startDegrees, myEndDegrees);
    return false;
  }
  myStartDegreesSet = true;
  myStartDegrees = startDegrees;
  return true;
}
    
AREXPORT bool ArLaser::setEndDegrees(double endDegrees)
{
  if (!myCanSetDegrees)
  {
    ArLog::log(ArLog::Terse, "%s::setEndDegrees: Cannot set angles on this laser", myName.c_str());
    return false;
  }
  if (endDegrees < myEndDegreesMin)
  {
    ArLog::log(ArLog::Terse, "%s::setEndDegrees: End degrees (%g) tried to be set to less than the minimum (%g))", myName.c_str(), endDegrees, myEndDegreesMin);
    return false;
  }
  if (endDegrees > myEndDegreesMax)
  {
    ArLog::log(ArLog::Terse, "%s::setEndDegrees: End degrees (%g) tried to be set to greater than the minimum (%g))", myName.c_str(), endDegrees, myEndDegreesMax);
    return false;
  }
  if (myStartDegreesSet && endDegrees <= myStartDegrees)
  {
    ArLog::log(ArLog::Terse, "%s::setEndDegrees: End degrees (%g) tried to be set to less than or equal to end degrees %g)", myName.c_str(), endDegrees, myStartDegrees);
    return false;
  }
  myEndDegreesSet = true;
  myEndDegrees = endDegrees;
  return true;
}

/**
   Allows the choice of the laser degrees from one of a number of
   choices, only one of this and laserAllowSetDegrees should be used.

   @param defaultDegreesChoice The default degrees, this should be
   the largest value.

   @param degreesChoices this is a mapping of std::strings to
   doubles, the strings should be the actual available choices, and
   the doubles should be the numerical representation... this is so
   the simulated laser can behave more easily like the real
   lasers... and because the original sick driver used words typed out
   (to make problems more obvious).
**/
AREXPORT void ArLaser::laserAllowDegreesChoices(
	const char *defaultDegreesChoice, 
	std::map<std::string, double> degreesChoices)
{
  myCanChooseDegrees = true;
  myDegreesChoices = degreesChoices;
  internalBuildChoices(&myDegreesChoices, &myDegreesChoicesString, &myDegreesChoicesList);
  chooseDegrees(defaultDegreesChoice);
  myDegreesChoiceSet = false;
}

AREXPORT bool ArLaser::chooseDegrees(
	const char *degreesChoice)
{
  if (!myCanChooseDegrees)
  {
    ArLog::log(ArLog::Terse, "%s::chooseDegrees: Cannot choose degrees on this laser", myName.c_str());
    return false;
  }

  double degreesChoiceDouble;
  if (!internalCheckChoice("chooseDegrees", degreesChoice, &myDegreesChoices, 
		   myDegreesChoicesString.c_str(), &degreesChoiceDouble))
    return false;

  myDegreesChoice = degreesChoice;
  myDegreesChoiceDouble = degreesChoiceDouble;
  return true;      
}

/**
   Allows the choice of increment from a range, only one of this and
   laserAllowIncrementChoices should be used.

   @param defaultIncrement The default increment to use, this
   default should be a reasonable value.

   @param incrementMin The minimum value for the increment

   @param incrementMax The maximum value for the increment
**/
AREXPORT void ArLaser::laserAllowSetIncrement(
	double defaultIncrement, double incrementMin, double incrementMax)
{
  myCanSetIncrement = true;
  myIncrementMin = incrementMin;
  myIncrementMax = incrementMax;
  setIncrement(defaultIncrement);
  myIncrementSet = false;

}

AREXPORT bool ArLaser::setIncrement(double increment)
{
  if (!myCanSetIncrement)
  {
    ArLog::log(ArLog::Terse, "%s::setIncrement: Cannot set increment on this laser", myName.c_str());
    return false;
  }

  if (increment < myIncrementMin)
  {
    ArLog::log(ArLog::Terse, "%s::setIncrement: Increment (%g) tried to be set to less than the minimum (%g))", myName.c_str(), increment, myIncrementMin);
    return false;
  }
  if (increment > myIncrementMax)
  {
    ArLog::log(ArLog::Terse, "%s::setIncrement: End degrees (%g) tried to be set to greater than the maximum (%g))", myName.c_str(), increment, myIncrementMax);
    return false;
  }
  myIncrementSet = true;
  myIncrement = increment;
  return true;
}

/**
   Allows the choice of increment from a limited set of values, only
   one of this and laserAllowSetIncrement should be used.

   @param defaultIncrementChoice The default increment, this should be
   a reasonable value.

   @param incrementChoices this is a mapping of std::strings to
   doubles, the strings should be the actual available choices, and
   the doubles should be the numerical representation... this is so
   the simulated laser can behave more easily like the real
   lasers... and because the original sick driver used words typed out
   (to make problems more obvious).
**/
AREXPORT void ArLaser::laserAllowIncrementChoices(
	const char *defaultIncrementChoice, 
	std::map<std::string, double> incrementChoices)
{
  myCanChooseIncrement = true;
  myIncrementChoices = incrementChoices;
  internalBuildChoices(&myIncrementChoices, &myIncrementChoicesString, &myIncrementChoicesList);
  chooseIncrement(defaultIncrementChoice);
  myIncrementChoiceSet = false;
}

AREXPORT bool ArLaser::chooseIncrement(const char *incrementChoice)
{
  if (!myCanChooseIncrement)
  {
    ArLog::log(ArLog::Terse, "%s::chooseIncrement: Cannot choose increment on this laser", myName.c_str());
    return false;
  }

  double incrementChoiceDouble;
  if (!internalCheckChoice("chooseIncrement", incrementChoice, 
		   &myIncrementChoices, myIncrementChoicesString.c_str(), 
		   &incrementChoiceDouble))
    return false;
  myIncrementChoice = incrementChoice;
  myIncrementChoiceDouble = incrementChoiceDouble;
  return true;      
}

/**
   @param defaultUnitsChoice This is the default units choice, it
   should be a reasonable value.

   @param unitsChoices The possible choices for units.
**/
AREXPORT void ArLaser::laserAllowUnitsChoices(
	const char *defaultUnitsChoice, 
	std::list<std::string> unitsChoices)
{
  myCanChooseUnits = true;
  myUnitsChoices = unitsChoices;
  internalBuildChoicesString(&myUnitsChoices, &myUnitsChoicesString);
  chooseUnits(defaultUnitsChoice);
  myUnitsChoiceSet = false;

}

AREXPORT bool ArLaser::chooseUnits(const char *unitsChoice)
{
  if (!myCanChooseUnits)
  {
    ArLog::log(ArLog::Terse, "%s::chooseUnits: Cannot choose units on this laser", myName.c_str());
    return false;
  }

  if (!internalCheckChoice("chooseUnits", unitsChoice, &myUnitsChoices,
		   myUnitsChoicesString.c_str()))
    return false;

  myUnitsChoice = unitsChoice;
  return true;      
}

/**
   @param defaultReflectorBitsChoice The default choice for reflector
   bits, should be a reasonable value.
   
   @param reflectorBitsChoices The possible choices for reflector bits
**/
AREXPORT void ArLaser::laserAllowReflectorBitsChoices(
	const char *defaultReflectorBitsChoice, 
	std::list<std::string> reflectorBitsChoices)
{
  myCanChooseReflectorBits = true;
  myReflectorBitsChoices = reflectorBitsChoices;
  internalBuildChoicesString(&myReflectorBitsChoices, &myReflectorBitsChoicesString);
  chooseReflectorBits(defaultReflectorBitsChoice);
  myReflectorBitsChoiceSet = false;
}

AREXPORT bool ArLaser::chooseReflectorBits(const char *reflectorBitsChoice)
{
  if (!myCanChooseReflectorBits)
  {
    ArLog::log(ArLog::Terse, "%s::chooseReflectorBits: Cannot choose reflectorBits on this laser", myName.c_str());
    return false;
  }

  if (!internalCheckChoice("chooseReflectorBits", reflectorBitsChoice, 
		   &myReflectorBitsChoices, 
		   myReflectorBitsChoicesString.c_str()))
    return false;

  myReflectorBitsChoice = reflectorBitsChoice;
  return true;      
}

/**
   Allows settings of whether the power can be controlled or not.
   This is mostly for devices that respond differently at power up
   than they do if they are already on (ie the lms2xx where it doesn't
   respond at all while powering up).  If the communication is the
   same either way, you can just not set this.

   @param defaultPowerControlled The default value for power
   controlled.
**/
AREXPORT void ArLaser::laserAllowSetPowerControlled(bool defaultPowerControlled)
{
  myCanSetPowerControlled = true;
  setPowerControlled(defaultPowerControlled);
  myPowerControlledSet = false;
}

AREXPORT bool ArLaser::setPowerControlled(
	bool powerControlled)
{
  if (!myCanSetPowerControlled)
  {
    ArLog::log(ArLog::Terse, "%s::setPowerControlled: Cannot set if the laser power is controlled on this laser", myName.c_str());
    return false;
  }

  myPowerControlledSet = true;
  myPowerControlled = powerControlled;
  return true;      
}

/**
   @param defaultStartingBaudChoice Default starting baud choice.
   This should probably stay the same as what the sensor ships with.

   @param startingBaudChoices The available choices for starting baud
**/
AREXPORT void ArLaser::laserAllowStartingBaudChoices(
	const char *defaultStartingBaudChoice, 
	std::list<std::string> startingBaudChoices)
{
  myCanChooseStartingBaud = true;
  myStartingBaudChoices = startingBaudChoices;
  internalBuildChoicesString(&myStartingBaudChoices, &myStartingBaudChoicesString);
  chooseStartingBaud(defaultStartingBaudChoice);
  myStartingBaudChoiceSet = false;
}

AREXPORT bool ArLaser::chooseStartingBaud(const char *startingBaudChoice)
{
  if (!myCanChooseStartingBaud)
  {
    ArLog::log(ArLog::Terse, "%s::chooseStartingBaud: Cannot choose startingBaud on this laser", myName.c_str());
    return false;
  }

  if (!internalCheckChoice("chooseStartingBaud", startingBaudChoice, 
		   &myStartingBaudChoices, 
		   myStartingBaudChoicesString.c_str()))
    return false;

  myStartingBaudChoice = startingBaudChoice;
  return true;      
}

/**
   @param defaultAutoBaudChoice Default auto baud choice.  This should
   probably be the maximum reasonable reliable robust rate that the
   laser supports.  The laser should autobaud up to this choice after
   it connects.

   @param autoBaudChoices The available choices for auto baud
**/
AREXPORT void ArLaser::laserAllowAutoBaudChoices(
	const char *defaultAutoBaudChoice, 
	std::list<std::string> autoBaudChoices)
{
  myCanChooseAutoBaud = true;
  myAutoBaudChoices = autoBaudChoices;
  internalBuildChoicesString(&myAutoBaudChoices, &myAutoBaudChoicesString);
  chooseAutoBaud(defaultAutoBaudChoice);
  myAutoBaudChoiceSet = false;
}

AREXPORT bool ArLaser::chooseAutoBaud(const char *autoBaudChoice)
{
  if (!myCanChooseAutoBaud)
  {
    ArLog::log(ArLog::Terse, "%s::chooseAutoBaud: Cannot choose autoBaud on this laser", myName.c_str());
    return false;
  }

  if (!internalCheckChoice("chooseAutoBaud", autoBaudChoice, 
		   &myAutoBaudChoices, myAutoBaudChoicesString.c_str()))
    return false;

  myAutoBaudChoice = autoBaudChoice;
  return true;      
}

AREXPORT void ArLaser::laserSetDefaultTcpPort(int defaultTcpPort)
{
  myDefaultTcpPort = defaultTcpPort;
}

AREXPORT void ArLaser::laserSetDefaultPortType(const char *defaultPortType)
{
  myDefaultPortType = defaultPortType;
}

AREXPORT bool ArLaser::addIgnoreReadings(const char *ignoreReadings)
{
  // if we have , then use it as the separator, otherwise use space
  // like normal
  char separator = ' ';
  if (strstr(ignoreReadings, ",") != NULL)
    separator = ',';

  ArArgumentBuilder args(1024, separator);
  
  args.add(ignoreReadings);
  if (args.getArgc() == 0)
    return true;

  size_t i;
  const char *str;
  float begin, end;
  float ignore;
  for (i = 0; i < args.getArgc(); i++)
  {
    if (args.isArgDouble(i))
    {
      ignore = args.getArgDouble(i);
      addIgnoreReading(ignore);
      ArLog::log(ArLog::Verbose, "%s: Added ignore reading %g", 
		 getName(), ignore);
    }
    else
    {
      str = args.getArg(i);
      if (sscanf(str, "%f:%f", &begin, &end) == 2 || 
	  sscanf(str, "%f-%f", &begin, &end) == 2)
      {
	ArLog::log(ArLog::Verbose, "%s: Adding ignore reading from %g to %g", 
		   getName(), begin, end);
	// reorder them for easier looping
	if (begin > end)
	{
	  ignore = begin;
	  begin = end;
	  end = ignore;
	}

	ArLog::log(ArLog::Verbose, "%s: Added ignore reading (beginning) %g", 
		   getName(), begin);
	addIgnoreReading(begin);
	for (ignore = begin; ignore <= end; ignore += 1.0)
	{
	  ArLog::log(ArLog::Verbose, "%s: Added ignore reading %g", 
		     getName(), ignore);
	  addIgnoreReading(ignore);
	}
	ArLog::log(ArLog::Verbose, "%s: Added ignore reading (ending) %g", 
		   getName(), end);
	addIgnoreReading(end);
      }
      else
      {
	ArLog::log(ArLog::Terse, "%s: Bad syntax for ignore readings, had string '%s' as one of the arguments (the values need to either be individual doubles, or begin:end (75:77) or begin-end (75-77))", getName(), str);
	return false;
      }
    }
  }

  return true;
}

/** 
    Applies a transform to the buffers. this is mostly useful for translating
    to/from local/global coordinates, but may have other uses.  
    This is different from
    the class because it also transforms the raw readings.
    @param trans the transform to apply to the data
    @param doCumulative whether to transform the cumulative buffer or not
*/
AREXPORT void ArLaser::applyTransform(ArTransform trans,
				       bool doCumulative)
{
  myCurrentBuffer.applyTransform(trans);
  std::list<ArSensorReading *>::iterator it;

  for (it = myRawReadings->begin(); it != myRawReadings->end(); ++it)
    (*it)->applyTransform(trans);

  if (doCumulative)
    myCumulativeBuffer.applyTransform(trans);
}

/**
   This will check if the laser has lost connection.  If there is no
   robot it is a straightforward check of last reading time against
   getConnectionTimeoutSeconds.  If there is a robot then it will not
   start the check until the laser is running and connected.
**/
AREXPORT bool ArLaser::laserCheckLostConnection(void)
{
	
  if ((myRobot == NULL || myRobotRunningAndConnected) && 
      getConnectionTimeoutSeconds() > 0 && 
      myLastReading.mSecSince() >  getConnectionTimeoutSeconds() * 1000)
    return true;

  if (!myRobotRunningAndConnected && myRobot != NULL && 
      myRobot->isRunning() && myRobot->isConnected())
  {
    myRobotRunningAndConnected = true;
    myLastReading.setToNow();
  }

  return false;
}

AREXPORT void ArLaser::copyReadingCount(const ArLaser* laser)
{
  myTimeLastReading = laser->myTimeLastReading;
  myReadingCurrentCount = laser->myReadingCurrentCount;
  myReadingCount = laser->myReadingCount;
}

AREXPORT void ArLaser::useSimpleNamingForAllLasers(void)
{
  ArLog::log(ArLog::Normal, "ArLaser: Will use simple naming for all lasers");
  ourUseSimpleNaming = true;
}