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