Exemplo n.º 1
0
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();
}
Exemplo n.º 2
0
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(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
  {
    ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 1000 for current buffer size", 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();
}
Exemplo n.º 3
0
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;
}