AREXPORT ArUrg::ArUrg(int laserNumber, const char *name) :
ArLaser(laserNumber, name, 4095),
mySensorInterpTask(this, &ArUrg::sensorInterp),
myAriaExitCB(this, &ArUrg::disconnect)
{
clear();
myRawReadings = NULL;
Aria::addExitCallback(&myAriaExitCB, -10);
laserSetName(getName());
setParams();
setSensorPosition(0, 0, 0);
laserAllowSetDegrees(-120, -135, 135, // default, min and max start degrees
120, -135, 135); // default, min and max end degrees
laserAllowSetIncrement(1, 0, 135);
std::list<std::string> baudChoices;
baudChoices.push_back("019200");
baudChoices.push_back("057600");
baudChoices.push_back("115200");
baudChoices.push_back("250000");
baudChoices.push_back("500000");
baudChoices.push_back("750000");
laserAllowStartingBaudChoices("019200", baudChoices);
laserAllowAutoBaudChoices("057600", baudChoices);
setMinDistBetweenCurrent(0);
setMaxDistToKeepCumulative(4000);
setMinDistBetweenCumulative(200);
setMaxSecondsToKeepCumulative(30);
setMaxInsertDistCumulative(3000);
setCumulativeCleanDist(75);
setCumulativeCleanInterval(1000);
setCumulativeCleanOffset(600);
resetLastCumulativeCleanTime();
setCurrentDrawingData(
new ArDrawingData("polyDots",
ArColor(0, 0xaa, 0xaa),
70, // mm diameter of dots
77), true);
setCumulativeDrawingData(
new ArDrawingData("polyDots",
ArColor(0, 0x55, 0x55),
100, // mm diameter of dots
62), true);
//myLogMore = false;
myLogMore = 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;
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;
}
AREXPORT void ArLaser::setSensorPosition(
double x, double y, double th, double z)
{
setSensorPosition(ArPose(x, y, th), z);
}
AREXPORT ArLaser::ArLaser(
int laserNumber, const char *name,
unsigned int absoluteMaxRange, bool locationDependent,
bool appendLaserNumberToName) :
ArRangeDeviceThreaded(
361, 200, name, absoluteMaxRange,
0, 0, 0, locationDependent)
{
myLaserNumber = laserNumber;
if (appendLaserNumberToName)
{
char buf[1024];
snprintf(buf, sizeof(buf) - 20, "%s", name);
sprintf(buf, "%s_%d", buf, myLaserNumber);
myName = buf;
}
else
{
if (laserNumber != 1)
ArLog::log(ArLog::Verbose, "ArLaser::%s: Laser created with number %d, but the number is not appended to the name which may break things (especially since this number is greater than 1_", name, laserNumber);
myName = name;
}
laserSetName(myName.c_str());
myAbsoluteMaxRange = absoluteMaxRange;
myMaxRangeSet = false;
setSensorPosition(0, 0, 0, 0);
myTimeoutSeconds = 8;
myHaveSensorPose = false;
myFlipped = false;
myFlippedSet = false;
myCanSetDegrees = false;
myStartDegreesMin = HUGE_VAL;
myStartDegreesMax = -HUGE_VAL;
myStartDegreesSet = false;
myStartDegrees = 0;
myEndDegreesMin = HUGE_VAL;
myEndDegreesMax = -HUGE_VAL;
myEndDegreesSet = false;
myEndDegrees = 0;
myCanChooseDegrees = false;
myDegreesChoiceDouble = -HUGE_VAL;
myCanSetIncrement = 0;
myIncrementMin = HUGE_VAL;
myIncrementMax = -HUGE_VAL;
myIncrementSet = false;
myIncrement = 0;
myCanChooseIncrement = false;
myIncrementChoiceDouble = -HUGE_VAL;
myCanChooseUnits = false;
myCanChooseReflectorBits = false;
myCanSetPowerControlled = false;
myPowerControlled = true;
myPowerControlledSet = false;
myCanChooseStartingBaud = false;
myCanChooseAutoBaud = false;
myDefaultTcpPort = 8102;
myInfoLogLevel = ArLog::Verbose;
myRobotRunningAndConnected = false;
}
