/**
@param currentBufferSize number of readings to store in the current
buffer
@param cumulativeBufferSize number of readings to store in the
cumulative buffer
@param name the name of this device
@param maxRange the maximum range of this device. If the device
can't find a reading in a specified section, it returns this
maxRange
@param maxSecondsToKeepCurrent this is the number of seconds to
keep current readings in the current buffer. If less than 0, then
readings are not automatically removed based on time (but can be
replaced or removed for other reasons).
@param maxSecondsToKeepCumulative this is the number of seconds to
keep cumulative readings in the cumulative buffer. If less than 0
then readings are not automatically based on time (but can be
replaced or removed for other reasons).
@param maxDistToKeepCumulative if cumulative readings are further
than this distance from the current robot pose, then they are
removed. If this is less than 0 they are not removed because of
this
@param locationDependent if the data in this range device is
dependent on the robot's location or not... For instance, a laser
would not be dependent on location, because it'll be correct in a
relative manner, whereas forbidden lines are dependent on location,
because if the robot isn't where it thinks it is then the forbidden
lines will be avoided in the wrong spots
**/
AREXPORT ArRangeDevice::ArRangeDevice(size_t currentBufferSize,
size_t cumulativeBufferSize,
const char *name,
unsigned int maxRange,
int maxSecondsToKeepCurrent,
int maxSecondsToKeepCumulative,
double maxDistToKeepCumulative,
bool locationDependent) :
myCurrentBuffer(currentBufferSize),
myCumulativeBuffer(cumulativeBufferSize),
myFilterCB(this, &ArRangeDevice::filterCallback)
{
myRobot = NULL;
myName = name;
myMaxRange = maxRange;
myRawReadings = NULL;
myAdjustedRawReadings = NULL;
setMaxSecondsToKeepCurrent(maxSecondsToKeepCurrent);
setMaxSecondsToKeepCumulative(maxSecondsToKeepCumulative);
setMaxDistToKeepCumulative(maxDistToKeepCumulative);
myCurrentDrawingData = NULL;
myOwnCurrentDrawingData = false;
myCumulativeDrawingData = NULL;
myOwnCumulativeDrawingData = false;
myIsLocationDependent = locationDependent;
}
AREXPORT ArSonarDevice::ArSonarDevice(size_t currentBufferSize,
size_t cumulativeBufferSize, const char *name) :
ArRangeDevice(currentBufferSize, cumulativeBufferSize, name, 5000),
myProcessCB(this, &ArSonarDevice::processReadings)
{
setMaxDistToKeepCumulative(3000);
myFilterNearDist = 50; // 50 mm between cumulative readings, at least
myFilterFarDist = 3000; // throw out cumulative readings this far
// from robot
setMaxSecondsToKeepCurrent(5);
setMaxSecondsToKeepCumulative(15);
setCurrentDrawingData(new ArDrawingData("polyArrows",
ArColor(0x33, 0xCC, 0xFF),
200, // mm length of arrow
70), // first sensor layer
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());
}
