int Sound::load(GFFFile &gff, GFFFile::Struct &top)
{
std::string tlabels[] =
{"Active", "Continuous", "Random", "RandomPosition", "Looping",
"Positional", "GeneratedType", "Priority", "Times", "Volume",
"VolumeVrtn", "PaletteID", "Hours", "Interval", "IntervalVrtn",
"ObjectId", "Elevation", "MaxDistance", "MinDistance", "XPosition",
"YPosition", "ZPosition", "PitchVariation", "RandomRangeX",
"RandomRangeY", "Tag", "Comment", "TemplateResRef", "LocName",
"Sounds", "ActionList", "VarTable"};
std::vector<uint32> n;
if(gff.getIdxByLabel(tlabels, sizeof(tlabels)/sizeof(std::string),
top, n, aderrstr))
return ERR(gff);
if(n[0] != NOTFOUND) active = (bool) top.values[n[0]];
if(n[1] != NOTFOUND) continuous = (bool) top.values[n[1]];
if(n[2] != NOTFOUND) random = (bool) top.values[n[2]];
if(n[3] != NOTFOUND) randomposition = (bool) top.values[n[3]];
if(n[4] != NOTFOUND) looping = (bool) top.values[n[4]];
if(n[5] != NOTFOUND) positional = (bool) top.values[n[5]];
if(n[6] != NOTFOUND) generatedtype = (bool) top.values[n[6]];
if(n[7] != NOTFOUND) priority = top.values[n[7]];
if(n[8] != NOTFOUND) times = top.values[n[8]];
if(n[9] != NOTFOUND) volume = top.values[n[9]];
if(n[10] != NOTFOUND) volumevrtn = top.values[n[10]];
if(n[11] != NOTFOUND) paletteid = top.values[n[11]];
if(n[12] != NOTFOUND) hours = top.values[n[12]];
if(n[13] != NOTFOUND) interval = top.values[n[13]];
if(n[14] != NOTFOUND) intervalvrtn = top.values[n[14]];
if(n[15] != NOTFOUND) objectid = top.values[n[15]];
if(n[16] != NOTFOUND) elevation = *((float32 *) &top.values[n[16]]);
if(n[17] != NOTFOUND) maxdistance = *((float32 *) &top.values[n[17]]);
if(n[18] != NOTFOUND) mindistance = *((float32 *) &top.values[n[18]]);
if(n[19] != NOTFOUND) x = *((float32 *) &top.values[n[19]]);
if(n[20] != NOTFOUND) y = *((float32 *) &top.values[n[20]]);
if(n[21] != NOTFOUND) z = *((float32 *) &top.values[n[21]]);
if(n[22] != NOTFOUND) pitchvariation = *((float32 *) &top.values[n[22]]);
if(n[23] != NOTFOUND) randomrangex = *((float32 *) &top.values[n[23]]);
if(n[24] != NOTFOUND) randomrangey = *((float32 *) &top.values[n[24]]);
if(n[25] != NOTFOUND)
if(gff.getExoString(n[25], top, tag)) return ERR(gff);
if(n[26] != NOTFOUND)
if(gff.getExoString(n[26], top, comment)) return ERR(gff);
if(n[27] != NOTFOUND)
if(gff.getResRef(n[27], top, templateresref)) return ERR(gff);
if(n[28] != NOTFOUND)
if(gff.getExoLocString(n[28], top, name)) return ERR(gff);
if(n[29] != NOTFOUND) if(getSounds(gff, n[29], top)) return errcode;
if(n[30] != NOTFOUND) if(getActions(gff, n[29], top)) return errcode;
if(n[31] != NOTFOUND)
if(variables.load(gff, n[32], top)) return ERR(variables);
checkRanges();
return errcode = 0;
}
void ArLaserFilter::processReadings(void)
{
myLaser->lockDevice();
selfLockDevice();
const std::list<ArSensorReading *> *rdRawReadings;
std::list<ArSensorReading *>::const_iterator rdIt;
if ((rdRawReadings = myLaser->getRawReadings()) == NULL)
{
selfUnlockDevice();
myLaser->unlockDevice();
return;
}
size_t rawSize = myRawReadings->size();
size_t rdRawSize = myLaser->getRawReadings()->size();
while (rawSize < rdRawSize)
{
myRawReadings->push_back(new ArSensorReading);
rawSize++;
}
// set where the pose was taken
myCurrentBuffer.setPoseTaken(
myLaser->getCurrentRangeBuffer()->getPoseTaken());
myCurrentBuffer.setEncoderPoseTaken(
myLaser->getCurrentRangeBuffer()->getEncoderPoseTaken());
std::list<ArSensorReading *>::iterator it;
ArSensorReading *rdReading;
ArSensorReading *reading;
#ifdef DEBUGRANGEFILTER
FILE *file = NULL;
file = ArUtil::fopen("/mnt/rdsys/tmp/filter", "w");
#endif
std::map<int, ArSensorReading *> readingMap;
int numReadings = 0;
// first pass to copy the readings and put them into a map
for (rdIt = rdRawReadings->begin(), it = myRawReadings->begin();
rdIt != rdRawReadings->end() && it != myRawReadings->end();
rdIt++, it++)
{
rdReading = (*rdIt);
reading = (*it);
*reading = *rdReading;
readingMap[numReadings] = reading;
numReadings++;
}
char buf[1024];
int i;
int j;
ArSensorReading *lastAddedReading = NULL;
// now walk through the readings to filter them
for (i = 0; i < numReadings; i++)
{
reading = readingMap[i];
// if we're ignoring this reading then just get on with life
if (reading->getIgnoreThisReading())
continue;
if (myMaxRange >= 0 && reading->getRange() > myMaxRange)
{
reading->setIgnoreThisReading(true);
continue;
}
if (lastAddedReading != NULL)
{
if (lastAddedReading->getPose().findDistanceTo(reading->getPose()) < 50)
{
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file, "%.1f too close from last %6.0f\n",
reading->getSensorTh(),
lastAddedReading->getPose().findDistanceTo(
reading->getPose()));
#endif
reading->setIgnoreThisReading(true);
continue;
}
#ifdef DEBUGRANGEFILTER
else if (file != NULL)
fprintf(file, "%.1f from last %6.0f\n",
reading->getSensorTh(),
lastAddedReading->getPose().findDistanceTo(
reading->getPose()));
#endif
}
buf[0] = '\0';
bool goodAll = true;
bool goodAny = false;
if (myAnyFactor <= 0)
goodAny = true;
for (j = i - 1;
(j >= 0 && //good &&
fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
reading->getSensorTh())) <= myAngleToCheck);
j--)
{
if (readingMap[j]->getIgnoreThisReading())
{
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6s", buf, "i");
#endif
continue;
}
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
if (myAllFactor > 0 &&
!checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAllFactor))
goodAll = false;
if (myAnyFactor > 0 &&
checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAnyFactor))
goodAny = true;
}
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d*", buf, reading->getRange());
#endif
for (j = i + 1;
(j < numReadings && //good &&
fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
reading->getSensorTh())) <= myAngleToCheck);
j++)
{
if (readingMap[j]->getIgnoreThisReading())
{
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6s", buf, "i");
#endif
continue;
}
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
if (myAllFactor > 0 &&
!checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAllFactor))
goodAll = false;
if (myAnyFactor > 0 &&
checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAnyFactor))
goodAny = true;
}
if (!goodAll || !goodAny)
reading->setIgnoreThisReading(true);
else
lastAddedReading = reading;
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file,
"%5.1f %6d %c\t%s\n", reading->getSensorTh(), reading->getRange(),
good ? 'g' : 'b', buf);
#endif
}
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fclose(file);
#endif
laserProcessReadings();
selfUnlockDevice();
myLaser->unlockDevice();
}
int Sound::save(GFFFile &gff, uint32 top)
{
uint32 list, sct;
checkRanges();
if(gff.writeVar(top, NWN_VAR_INT8, "Active", active)) return ERR(gff);
if(gff.writeVar(top, NWN_VAR_INT8, "Continuous", continuous))
return ERR(gff);
if(gff.writeVar(top, NWN_VAR_INT8, "Random", random)) return ERR(gff);
if(gff.writeVar(top, NWN_VAR_INT8, "RandomPosition", randomposition))
return ERR(gff);
if(gff.writeVar(top, NWN_VAR_INT8, "Looping", looping)) return ERR(gff);
if(gff.writeVar(top, NWN_VAR_INT8, "Positional", positional))
return ERR(gff);
if(gff.writeVar(top, NWN_VAR_INT8, "Priority", priority)) return ERR(gff);
if(gff.writeVar(top, NWN_VAR_INT8, "Times", times)) return ERR(gff);
if(gff.writeVar(top, NWN_VAR_INT8, "Volume", volume)) return ERR(gff);
if(gff.writeVar(top, NWN_VAR_INT8, "VolumeVrtn", volumevrtn))
return ERR(gff);
if(gff.writeVar(top, NWN_VAR_UINT32, "Hours", hours)) return ERR(gff);
if(gff.writeVar(top, NWN_VAR_UINT32, "Interval", interval)) return ERR(gff);
if(gff.writeVar(top, NWN_VAR_UINT32, "IntervalVrtn", intervalvrtn))
return ERR(gff);
if(gff.writeFloat(top, "PitchVariation", pitchvariation)) return ERR(gff);
if(gff.writeFloat(top, "Elevation", elevation)) return ERR(gff);
if(gff.writeFloat(top, "MinDistance", mindistance)) return ERR(gff);
if(gff.writeFloat(top, "MaxDistance", maxdistance)) return ERR(gff);
if(gff.writeFloat(top, "RandomRangeX", randomrangex)) return ERR(gff);
if(gff.writeFloat(top, "RandomRangeY", randomrangey)) return ERR(gff);
if(gff.writeExoString(top, "Tag", tag)) return ERR(gff);
if(gff.writeResRef(top, "TemplateResRef", templateresref)) return ERR(gff);
if(gff.writeExoLocString(top, "LocName", name)) return ERR(gff);
if(gff.writeList(list, top, "Sounds")) return ERR(gff);
for(uint32 i=0;i<sounds.size();i++)
{
if(gff.writeListStruct(sct, list)) return ERR(gff);
if(gff.writeSetId(sct, 0)) return ERR(gff);
if(gff.writeResRef(sct, "Sound", sounds[i])) return ERR(gff);
}
switch(objectstate)
{
case NWN_OBJECT_BLUEPRINT:
if(gff.writeVar(top, NWN_VAR_INT8, "PaletteID", paletteid))
return ERR(gff);
if(gff.writeExoString(top, "Comment", comment)) return ERR(gff);
break;
case NWN_OBJECT_SAVE:
if(gff.writeVar(top, NWN_VAR_UINT32, "ObjectId", objectid))
return ERR(gff);
if(writeActions(gff, top)) return errcode;
case NWN_OBJECT_INSTANCE:
if(gff.writeVar(top, NWN_VAR_UINT8, "GeneratedType", generatedtype))
return ERR(gff);
if(gff.writeFloat(top, "XPosition", x)) return ERR(gff);
if(gff.writeFloat(top, "YPosition", y)) return ERR(gff);
if(gff.writeFloat(top, "ZPosition", z)) return ERR(gff);
break;
}
if(!variables.empty())
{
if(gff.writeList(list, top, "VarTable")) return ERR(gff);
if(variables.save(gff, list)) return ERR(variables);
}
return errcode = 0;
}
void Novatech409B::parseDeviceEvents(const RawEventMap& eventsIn,
SynchronousEventVector& eventsOut) throw(std::exception)
{
//Make sure the current state of all channels is up to date.
refreshLocallyStoredFrequencyChannels();
cerr << endl << endl << "Table: " << endl;
RawEventMap::const_iterator events;
RawEventMap::const_iterator lastTableEvents;
double holdoff = 0;
double minimumEventSpacing = 0;
double minimumAbsoluteStartTime = 0;
double tableStartHoldoff_ns = 1000 * 1000; //1 ms
bool goodFormat = true;
bool holdEventFound = false;
bool channelZeroOrOne = false;
FrequencyChannel currentTriplet;
FrequencyChannel currentTriplet0 = frequencyChannels.at(0);
FrequencyChannel currentTriplet1 = frequencyChannels.at(1);
unsigned tableAddress = 0;
double tableDwellTime_ns = 0; //in ns
double lastTableDwellTime_ns = 0; //in ns
std::string errorMessage;
double eventTime;
double previousTime = minimumAbsoluteStartTime;
for(events = eventsIn.begin(); events != eventsIn.end(); events++)
{
eventTime = events->first - holdoff;
if( (events->first - minimumEventSpacing) < previousTime)
{
if(events != eventsIn.begin())
throw EventParsingException(events->second.at(0),
"The Novatech needs " +
valueToString(minimumEventSpacing) +
" ns between events.");
else
throw EventParsingException(events->second.at(0),
"The Novatech needs " +
valueToString(minimumAbsoluteStartTime) +
" ns at the beginning of the timing file.");
}
//Verify format. Parameter convention: (freq, amplitude, phase, [Hold])
for(unsigned i = 0; i < events->second.size(); i++)
{
goodFormat = events->second.at(i).value().getVector().size() == 3 ||
events->second.at(i).value().getVector().size() == 4;
for(unsigned k = 0; k < 3; k++)
{
goodFormat &= events->second.at(i).value().getVector().at(k).getType() == MixedValue::Double;
}
if(!goodFormat) {
throw EventParsingException(events->second.at(i),
"Bad format. Novatech commands must be of the form (frequency, amplitude, phase, [Hold]).");
}
currentTriplet.frequency = events->second.at(i).value().getVector().at(0).getDouble();
currentTriplet.amplitude = events->second.at(i).value().getVector().at(1).getDouble();
currentTriplet.phase = events->second.at(i).value().getVector().at(2).getDouble();
//Check parameter range limits
if( !checkRanges(currentTriplet, errorMessage) ) {
throw EventParsingException(events->second.at(i),
"Out of range. Allowed ranges are (0-171.1276031) MHz; (0-100) percent; (0-360) degrees.");
}
}
//Check for trigger hold command
if( isHoldEvent(events) ) {
holdEventFound = true; //always True after 1st hold event is found (switches to Table Mode).
}
//As long as their is no "Hold" event found, assume the events are soft timing and use PsuedoSynchronousEvents
if(!holdEventFound) {
eventsOut.push_back(
new STI_Device::PsuedoSynchronousEvent(events->first, events->second, this) );
//Update channel 0 and 1 triplets to most recent event. This allows for a dummy event at the start
//of table mode. The dummy event will be whatever value was last played on the channel.
for(unsigned j = 0; j < events->second.size(); j++) {
if(events->second.at(j).channel() == 0) {
getTriplet(events->second.at(j).value(), currentTriplet0);
}
if(events->second.at(j).channel() == 1) {
getTriplet(events->second.at(j).value(), currentTriplet1);
}
}
//Register any measurements
for(unsigned j = 0; j < events->second.size(); j++)
{
if( events->second.at(j).isMeasurementEvent() ) {
eventsOut.back().addMeasurement( events->second.at(j) );
}
}
}
else {
//A Hold event was found. Run the rest of the events in Table Mode.
/////// Table Mode ///////
//Only channels 0 and 1 can be set using table mode. Check for violations.
channelZeroOrOne = true;
for(unsigned j = 0; j < events->second.size(); j++)
{
channelZeroOrOne = (events->second.at(j).channel() == 0 || events->second.at(j).channel() == 1);
if( !channelZeroOrOne) {
throw EventParsingException(events->second.at(j),
std::string("Only Channels 0 and 1 can be changed after a 'Hold' command is given\n") +
"('Table Mode' only supports setting channels 0 and 1).");
}
}
//Now we can assume that the 'events' vector only contains events for channels 0 and/or 1.
tableDwellTime_ns = eventTime - previousTime;
if(tableAddress > 0) {
// New table point; add the previous point
if( tableDwellTime_ns > 25400000 ) {
throw EventParsingException(lastTableEvents->second.at(0),
std::string("Dwell time overflow.\n Maximum dwell time between events in a triggered table\n is 25.4 ms.\n")
+ "The computed dwell time was " + STI::Utils::valueToString(tableDwellTime_ns / 1000000) + " ms.");
}
//Get triplets for this event.
//Possibly only one channel will be explicitly set. Make a dummy event for the other
//because table commands must be made for both channels.
for(unsigned j = 0; j < lastTableEvents->second.size(); j++) {
if(lastTableEvents->second.at(j).channel() == 0) {
getTriplet(lastTableEvents->second.at(j).value(), currentTriplet0);
}
if(lastTableEvents->second.at(j).channel() == 1) {
getTriplet(lastTableEvents->second.at(j).value(), currentTriplet1);
}
}
// bool temp = isHoldEvent(lastTableEvents);
addTablePoint(currentTriplet0, currentTriplet1, tableAddress, tableDwellTime_ns,
isHoldEvent(lastTableEvents));
//When tableAddress==1 we ignore the explicit Hold event, since the dummy event at the
//0th table entry already has the hold. tableAddress != 1 &&
}
else {
//first table point
preTableCommands();
//currentTriplet0;
//First table point is a "dummy" point that maintains the current state. It asserts ff for the hold
//so that the next point in the table will be reached after the trigger.
// addTablePoint(currentTriplet0, currentTriplet1, 0, tableDwellTime_ns, true);
eventsOut.push_back( new NovatechTableEvent(events->first - tableStartHoldoff_ns, this) );
}
lastTableDwellTime_ns = tableDwellTime_ns;
lastTableEvents = events;
tableAddress += 1;
}
previousTime = eventTime;
}
if(tableAddress > 0) {
//Add final table point with dwell time of 0 us
for(unsigned j = 0; j < lastTableEvents->second.size(); j++) {
if(lastTableEvents->second.at(j).channel() == 0) {
getTriplet(lastTableEvents->second.at(j).value(), currentTriplet0);
}
if(lastTableEvents->second.at(j).channel() == 1) {
getTriplet(lastTableEvents->second.at(j).value(), currentTriplet1);
}
}
addTablePoint(currentTriplet0, currentTriplet1, tableAddress, 0, isHoldEvent(lastTableEvents));
addTablePoint(currentTriplet0, currentTriplet1, tableAddress, 0, false); //end table with dwell time 00.
postTableCommands();
}
}
bool Novatech409B::writeChannel(unsigned short channel, const MixedValue& valuet)
{
std::string queryResult;
MixedValueVector tempVec;
enum error {TYPE, CHANNEL, RANGE};
std::string errorMessage;
FrequencyChannel newFrequencyTriplet;
// double frequency;
// double amplitude;
// double phase;
int amplitudeN;
int phaseN;
try {
if (channel >= 0 && channel < 4)
{
if (valuet.getType() == MixedValue::Vector)
{
tempVec = valuet.getVector();
if (tempVec.size() != 3)
throw TYPE;
newFrequencyTriplet.frequency = tempVec.at(0).getDouble();
newFrequencyTriplet.amplitude = tempVec.at(1).getDouble();
newFrequencyTriplet.phase = tempVec.at(2).getDouble();
amplitudeN = static_cast<int>( (newFrequencyTriplet.amplitude / 100.0) * amplitudeMaxVal );
phaseN = static_cast<int>( (newFrequencyTriplet.phase / 360.0) * phaseMaxVal );
if( !checkRanges(newFrequencyTriplet, errorMessage) ) {
//Bad range found
throw RANGE;
}
}
else
throw TYPE;
}
else
throw CHANNEL;
}
catch (error e)
{
if (e == CHANNEL)
std::cerr << this->getDeviceName() << " expects channel 0, 1, 2,or 3 for a write command" << std::endl;
if (e == TYPE)
std::cerr << this->getDeviceName() << " requires a triplet of doubles: (frequency, amplitude, phase)" << std::endl;
if (e == RANGE)
std::cerr << this->getDeviceName() << " allows ranges of (0-171.1276031 MHz, 0-100 percent, 0-360 degrees)" << std::endl;
return false;
}
std::string frequencyString, amplitudeString, phaseString;
//Set frequency
if (frequencyChannels.at(channel).frequency != newFrequencyTriplet.frequency)
{
frequencyString = "f" + valueToString(channel) + " " +
valueToString(newFrequencyTriplet.frequency, "", ios::dec, 10);
queryResult = serialController->queryDevice(frequencyString, 50, 30);
if (queryResult.find("OK") == std::string::npos)
{
std::cerr << "Unable to set frequency of " << this->getDeviceName() << " channel " << channel << std::endl;
return false;
}
frequencyChannels.at(channel).frequency = newFrequencyTriplet.frequency;
}
//Set amplitude
if (frequencyChannels.at(channel).amplitude != newFrequencyTriplet.amplitude)
{
amplitudeString = "v" + valueToString(channel) + " " + valueToString(amplitudeN);
queryResult = serialController->queryDevice(amplitudeString);
if (queryResult.find("OK") == std::string::npos)
{
std::cerr << "Unable to set amplitude of " << this->getDeviceName() << " channel " << channel << std::endl;
return false;
}
frequencyChannels.at(channel).amplitude = newFrequencyTriplet.amplitude;
}
//Set phase
if (frequencyChannels.at(channel).phase != newFrequencyTriplet.phase)
{
phaseString = "p" + valueToString(channel) + " " + valueToString(phaseN);
queryResult = serialController->queryDevice(phaseString);
if (queryResult.find("OK") == std::string::npos)
{
std::cerr << "Unable to set phase of " << this->getDeviceName() << " channel " << channel << std::endl;
return false;
}
frequencyChannels.at(channel).phase = newFrequencyTriplet.phase;
}
return true;
}
//=========================== OPTIM CALIBRATION ============================
void optim_calibration(int *ind, int *n, double *x, double *f, double *g, opt_simul_data *optim_data)
{
double grad[5];
double xorig[5];
int i;
if( (TypeModel == 111)||(!ifChangeVar))
{
if(!checkRanges(*n, x)) {printf("Some of parameters are out of range!\n");}
for(i=0; i<5; i++) xorig[i]=x[i];
}
else if(ifChangeVar)
{ // if variables are changed
xorig[0] = 0.5 + atan(x[0])/M_PI;
xorig[1] = 30.0*(0.5 + atan(x[1])/M_PI);
xorig[2] = 0.5 + atan(x[2])/M_PI;
xorig[3] = 0.5 + atan(x[3])/M_PI;
xorig[4] = 2.0*atan(x[4])/M_PI;
}
printf("current params %d %f %f %f %f %f\n", TypeModel, xorig[0], xorig[1], xorig[2], xorig[3], xorig[4]);
if (*ind == 2 || *ind == 4 )
{
/* cost */
if(TypeModel==111) { *f = costFunctionVS(*n,x);}
else {
*f = costFunction(*n, xorig);
}
}
if (*ind == 3 || *ind == 4 )
{
/* gradient */
if(TypeModel==111) { gradFunctionVS(*n,x,grad);
g[0] = grad[0];
g[1] = grad[1];
g[2] = grad[2];
g[3] = grad[3];
g[4] = grad[4]; }
else {
gradFunction(*n,xorig,grad);
g[0] = grad[0];
g[1] = grad[1];
g[2] = grad[2];
g[3] = grad[3];
g[4] = grad[4];
if(ifChangeVar)
{
g[0] = g[0]/M_PI/(1.0+x[0]*x[0]);
g[1] = g[1]*30.0/M_PI/(1.0+x[1]*x[1]);
g[2] = g[2]/M_PI/(1.0+x[2]*x[2]);;
g[3] = g[3]/M_PI/(1.0+x[3]*x[3]);
g[4] = g[4]*2.0/M_PI/(1.0+x[4]*x[4]);
}
if(TypeModel==11) {// to fix parameters fitted by Variance Swaps
g[0] =0.0;
g[1] =0.0;
g[2] =0.0;
}
}
}
}
void ArLaserFilter::processReadings(void)
{
myLaser->lockDevice();
selfLockDevice();
const std::list<ArSensorReading *> *rdRawReadings;
std::list<ArSensorReading *>::const_iterator rdIt;
if ((rdRawReadings = myLaser->getRawReadings()) == NULL)
{
selfUnlockDevice();
myLaser->unlockDevice();
return;
}
size_t rawSize = myRawReadings->size();
size_t rdRawSize = myLaser->getRawReadings()->size();
while (rawSize < rdRawSize)
{
myRawReadings->push_back(new ArSensorReading);
rawSize++;
}
// set where the pose was taken
myCurrentBuffer.setPoseTaken(
myLaser->getCurrentRangeBuffer()->getPoseTaken());
myCurrentBuffer.setEncoderPoseTaken(
myLaser->getCurrentRangeBuffer()->getEncoderPoseTaken());
std::list<ArSensorReading *>::iterator it;
ArSensorReading *rdReading;
ArSensorReading *reading;
#ifdef DEBUGRANGEFILTER
FILE *file = NULL;
//file = ArUtil::fopen("/mnt/rdsys/tmp/filter", "w");
file = ArUtil::fopen("/tmp/filter", "w");
#endif
std::map<int, ArSensorReading *> readingMap;
int numReadings = 0;
// first pass to copy the readings and put them into a map
for (rdIt = rdRawReadings->begin(), it = myRawReadings->begin();
rdIt != rdRawReadings->end() && it != myRawReadings->end();
rdIt++, it++)
{
rdReading = (*rdIt);
reading = (*it);
*reading = *rdReading;
readingMap[numReadings] = reading;
numReadings++;
}
// if we're not doing any filtering, just short circuit out now
if (myAllFactor <= 0 && myAnyFactor <= 0 && myAnyMinRange <= 0)
{
laserProcessReadings();
copyReadingCount(myLaser);
selfUnlockDevice();
myLaser->unlockDevice();
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fclose(file);
#endif
return;
}
char buf[1024];
int i;
int j;
//ArSensorReading *lastAddedReading = NULL;
// now walk through the readings to filter them
for (i = 0; i < numReadings; i++)
{
reading = readingMap[i];
// if we're ignoring this reading then just get on with life
if (reading->getIgnoreThisReading())
continue;
/* Taking this check out since the base class does it now and if
* it gets marked ignore now it won't get used for clearing
* cumulative readings
if (myMaxRange >= 0 && reading->getRange() > myMaxRange)
{
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file, "%.1f beyond max range at %d\n",
reading->getSensorTh(), reading->getRange());
#endif
reading->setIgnoreThisReading(true);
continue;
}
*/
if (myAnyMinRange >= 0 && reading->getRange() < myAnyMinRange &&
(reading->getSensorTh() < myAnyMinRangeLessThanAngle ||
reading->getSensorTh() > myAnyMinRangeGreaterThanAngle))
{
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file, "%.1f within min range at %d\n",
reading->getSensorTh(), reading->getRange());
#endif
reading->setIgnoreThisReading(true);
continue;
}
/*
if (lastAddedReading != NULL)
{
if (lastAddedReading->getPose().findDistanceTo(reading->getPose()) < 50)
{
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file, "%.1f too close from last %6.0f\n",
reading->getSensorTh(),
lastAddedReading->getPose().findDistanceTo(
reading->getPose()));
#endif
reading->setIgnoreThisReading(true);
continue;
}
#ifdef DEBUGRANGEFILTER
else if (file != NULL)
fprintf(file, "%.1f from last %6.0f\n",
reading->getSensorTh(),
lastAddedReading->getPose().findDistanceTo(
reading->getPose()));
#endif
}
*/
buf[0] = '\0';
bool goodAll = true;
bool goodAny = false;
bool goodMinRange = true;
if (myAnyFactor <= 0)
goodAny = true;
for (j = i - 1;
(j >= 0 && //good &&
fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
reading->getSensorTh())) <= myAngleToCheck);
j--)
{
/* You can't skip these, or you get onesided filtering
if (readingMap[j]->getIgnoreThisReading())
{
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6s", buf, "i");
#endif
continue;
}
*/
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
if (myAllFactor > 0 &&
!checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAllFactor))
goodAll = false;
if (myAnyFactor > 0 &&
checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAnyFactor))
goodAny = true;
if (myAnyMinRange > 0 &&
(reading->getSensorTh() < myAnyMinRangeLessThanAngle ||
reading->getSensorTh() > myAnyMinRangeGreaterThanAngle) &&
readingMap[j]->getRange() <= myAnyMinRange)
goodMinRange = false;
}
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d*", buf, reading->getRange());
#endif
for (j = i + 1;
(j < numReadings && //good &&
fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
reading->getSensorTh())) <= myAngleToCheck);
j++)
{
// you can't ignore these or you get one sided filtering
/*
if (readingMap[j]->getIgnoreThisReading())
{
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6s", buf, "i");
#endif
continue;
}
*/
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
if (myAllFactor > 0 &&
!checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAllFactor))
goodAll = false;
if (myAnyFactor > 0 &&
checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAnyFactor))
goodAny = true;
if (myAnyMinRange > 0 &&
(reading->getSensorTh() < myAnyMinRangeLessThanAngle ||
reading->getSensorTh() > myAnyMinRangeGreaterThanAngle) &&
readingMap[j]->getRange() <= myAnyMinRange)
goodMinRange = false;
}
if (!goodAll || !goodAny || !goodMinRange)
reading->setIgnoreThisReading(true);
/*
else
lastAddedReading = reading;
*/
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file,
"%5.1f %6d %c\t%s\n", reading->getSensorTh(), reading->getRange(),
goodAll && goodAny && goodMinRange ? 'g' : 'b', buf);
#endif
}
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fclose(file);
#endif
laserProcessReadings();
copyReadingCount(myLaser);
selfUnlockDevice();
myLaser->unlockDevice();
}
