AREXPORT ArLaserFilter::ArLaserFilter( ArLaser *laser, const char *name) : ArLaser(laser->getLaserNumber(), name != NULL && name[0] != '\0' ? name : laser->getName(), laser->getAbsoluteMaxRange(), laser->isLocationDependent(), false), myProcessCB(this, &ArLaserFilter::processReadings) { myLaser = laser; if (name == NULL || name[0] == '\0') { std::string filteredName; filteredName = "filtered_"; filteredName += laser->getName(); laserSetName(filteredName.c_str()); } myRawReadings = new std::list<ArSensorReading *>; char buf[1024]; sprintf(buf, "%sProcessCB", getName()); myProcessCB.setName(buf); myAngleToCheck = 1; myAnyFactor = -1; myAllFactor = -1; myMaxRange = -1; setCurrentDrawingData( new ArDrawingData(*(myLaser->getCurrentDrawingData())), true); setCumulativeDrawingData( new ArDrawingData(*(myLaser->getCumulativeDrawingData())), true); // laser parameters setInfoLogLevel(myLaser->getInfoLogLevel()); setConnectionTimeoutSeconds(myLaser->getConnectionTimeoutSeconds()); setCumulativeCleanDist(myLaser->getCumulativeCleanDist()); setCumulativeCleanInterval(myLaser->getCumulativeCleanInterval()); setCumulativeCleanOffset(myLaser->getCumulativeCleanOffset()); setSensorPosition(myLaser->getSensorPosition()); laserSetAbsoluteMaxRange(myLaser->getAbsoluteMaxRange()); setMaxRange(myLaser->getMaxRange()); // base range device parameters setMaxSecondsToKeepCurrent(myLaser->getMaxSecondsToKeepCurrent()); setMinDistBetweenCurrent(getMinDistBetweenCurrent()); setMaxSecondsToKeepCumulative(myLaser->getMaxSecondsToKeepCumulative()); setMaxDistToKeepCumulative(myLaser->getMaxDistToKeepCumulative()); setMinDistBetweenCumulative(myLaser->getMinDistBetweenCumulative()); setMaxInsertDistCumulative(myLaser->getMaxInsertDistCumulative()); setCurrentDrawingData(myLaser->getCurrentDrawingData(), false); setCumulativeDrawingData(myLaser->getCumulativeDrawingData(), false); // now all the specific laser settings (this should already be taken // care of when this is created, but the code existed for the // simulated laser so I put it here too) if (myLaser->canSetDegrees()) laserAllowSetDegrees( myLaser->getStartDegrees(), myLaser->getStartDegreesMin(), myLaser->getStartDegreesMax(), myLaser->getEndDegrees(), myLaser->getEndDegreesMin(), myLaser->getEndDegreesMax()); if (myLaser->canChooseDegrees()) laserAllowDegreesChoices(myLaser->getDegreesChoice(), myLaser->getDegreesChoicesMap()); if (myLaser->canSetIncrement()) laserAllowSetIncrement(myLaser->getIncrement(), myLaser->getIncrementMin(), myLaser->getIncrementMax()); if (myLaser->canChooseIncrement()) laserAllowIncrementChoices(myLaser->getIncrementChoice(), myLaser->getIncrementChoicesMap()); if (myLaser->canChooseUnits()) laserAllowUnitsChoices(myLaser->getUnitsChoice(), myLaser->getUnitsChoices()); if (myLaser->canChooseReflectorBits()) laserAllowReflectorBitsChoices(myLaser->getReflectorBitsChoice(), myLaser->getReflectorBitsChoices()); if (canSetPowerControlled()) laserAllowSetPowerControlled(myLaser->getPowerControlled()); if (myLaser->canChooseStartingBaud()) laserAllowStartingBaudChoices(myLaser->getStartingBaudChoice(), myLaser->getStartingBaudChoices()); if (myLaser->canChooseAutoBaud()) laserAllowAutoBaudChoices(myLaser->getAutoBaudChoice(), myLaser->getAutoBaudChoices()); laserSetDefaultTcpPort(myLaser->getDefaultTcpPort()); laserSetDefaultPortType(myLaser->getDefaultPortType()); }
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; }