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