AREXPORT ArSick::ArSick(
size_t currentBufferSize, size_t cumulativeBufferSize,
const char *name, bool addAriaExitCB, bool isSecondLaser) :
ArLMS2xx(!isSecondLaser ? 1 : 2, name, !isSecondLaser ? false : true)
{
setCurrentBufferSize(currentBufferSize);
setCumulativeBufferSize(cumulativeBufferSize);
}
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 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;
myAnyMinRange = -1;
myAnyMinRangeLessThanAngle = -180;
myAnyMinRangeGreaterThanAngle = 180;
setCurrentDrawingData(
new ArDrawingData(*(myLaser->getCurrentDrawingData())),
true);
setCumulativeDrawingData(
new ArDrawingData(*(myLaser->getCumulativeDrawingData())),
true);
// laser parameters
setInfoLogLevel(myLaser->getInfoLogLevel());
setConnectionTimeoutSeconds(myLaser->getConnectionTimeoutSeconds());
setCurrentBufferSize(myLaser->getCurrentBufferSize());
setCumulativeBufferSize(myLaser->getCumulativeBufferSize());
setCumulativeCleanDist(myLaser->getCumulativeCleanDist());
setCumulativeCleanInterval(myLaser->getCumulativeCleanInterval());
setCumulativeCleanOffset(myLaser->getCumulativeCleanOffset());
setSensorPosition(myLaser->getSensorPosition(),
myLaser->getSensorPositionZ());
laserSetAbsoluteMaxRange(myLaser->getAbsoluteMaxRange());
// set our max range to the laser we're filtering... then set the
// max range on the laser we're filtering to 0, so that it's over
// max range values don't get set to ignore, since then we can't
// clear cumulatives beyond that value
setMaxRange(myLaser->getMaxRange());
myLaser->setMaxRange(0);
// 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);
// turn off the cumulative buffer on the original to save CPU
myLaser->setCumulativeBufferSize(0);
// 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());
}
