AREXPORT void ArSonarDevice::processReadings(void)
{
int i;
ArSensorReading *reading;
lockDevice();
for (i = 0; i < myRobot->getNumSonar(); i++)
{
reading = myRobot->getSonarReading(i);
if (reading == NULL || !reading->isNew(myRobot->getCounter()))
continue;
addReading(reading->getX(), reading->getY());
}
// delete too-far readings
std::list<ArPoseWithTime *> *readingList;
std::list<ArPoseWithTime *>::iterator it;
double dx, dy, rx, ry;
myCumulativeBuffer.beginInvalidationSweep();
readingList = myCumulativeBuffer.getBuffer();
rx = myRobot->getX();
ry = myRobot->getY();
// walk through the list and see if this makes any old readings bad
if (readingList != NULL)
{
for (it = readingList->begin(); it != readingList->end(); ++it)
{
dx = (*it)->getX() - rx;
dy = (*it)->getY() - ry;
if ((dx*dx + dy*dy) > (myFilterFarDist * myFilterFarDist))
myCumulativeBuffer.invalidateReading(it);
}
}
myCumulativeBuffer.endInvalidationSweep();
// leave this unlock here or the world WILL end
unlockDevice();
}
void IOPolly::obsFromTextFile (std::string filePath, int angle_unit_type)
{
// The input file
std::ifstream data_file (filePath.c_str());
// testa a abertura do ficheiro
if (!data_file.is_open())
{
std::cerr << "Error: Could not open " << filePath << "\n";
exit (8); //faz parte da cstdlib
}
std::string current_line; // alberga cada linha do ficheiro
double horizontal_dir, vertical_dir; // alberga os valores angulares convertidos para rad
SIUnits *ang_conv = new SIUnits(); // responsavel pelas conversoes
while(!data_file.eof())
{
std::getline(data_file, current_line); // recolhe a linha
if (!current_line.empty()) // se n leu uma linha em branco
{
std::vector<std::string> elements; // linha nos seus elementos
// faz o split ah string para elements
std::istringstream iss(current_line);
std::copy(std::istream_iterator<std::string>(iss),
std::istream_iterator<std::string>(),
std::back_inserter<std::vector<std::string> >(elements));
if(elements.size() == 2)
{
//cria uma estacao
addStation(TCStations->getEnzPoint(elements[0]),
std::atof(elements[1].c_str()));
}
else if(elements.size() == 5)
{
//cria um reading
switch(angle_unit_type) // conversao dos angulos
{
case 1:
horizontal_dir = std::atof(elements[1].c_str());
vertical_dir = std::atof(elements[2].c_str());
break;
case 2:
horizontal_dir = ang_conv->degree2rad (std::atof(elements[1].c_str()));
vertical_dir = ang_conv->degree2rad (std::atof(elements[2].c_str()));
break;
case 3:
horizontal_dir = ang_conv->gon2rad (std::atof(elements[1].c_str()));
vertical_dir = ang_conv->gon2rad (std::atof(elements[2].c_str()));
break;
}
addReading((*(--(obs->end()))).from,
TCStations->getEnzPoint(elements[0]), horizontal_dir,
vertical_dir, std::atof(elements[3].c_str()),
std::atof(elements[4].c_str()));
}
}
}
delete ang_conv;
data_file.close();
}
Daemon::Daemon(int argc, char *argv[])
: QCoreApplication(argc, argv)
{
initSettings();
char defaultLogFilename[] = DEFAULT_LOG_FILE;
char* logFilename = defaultLogFilename;
int port = DEFAULT_LOCAL_PORT;
bool isDaemon = true;
bool runWiFiScanner = true;
bool runMovementDetector = true;
bool recordScans = false;
bool runAllAlgorithms = false;
//////////////////////////////////////////////////////////
// Make sure no other arguments have been given
QStringList args = QCoreApplication::arguments();
QStringListIterator argsIter(args);
argsIter.next(); // moled
while (argsIter.hasNext()) {
QString arg = argsIter.next();
if (arg == "-h") {
usage();
} else if (arg == "-v") {
version();
} else if (arg == "-d") {
debug = true;
} else if (arg == "-n") {
isDaemon = false;
logFilename = NULL;
} else if (arg == "-l") {
logFilename = argsIter.next().toAscii().data();
} else if (arg == "-s") {
mapServerURL = argsIter.next();
} else if (arg == "-f") {
staticServerURL = argsIter.next();
} else if (arg == "-r") {
rootPathname = argsIter.next();
} else if (arg == "-p") {
port = argsIter.next().toInt();
} else if (arg == "--no-accelerometer") {
runMovementDetector = false;
} else if (arg == "--no-wifi") {
runWiFiScanner = false;
} else if (arg == "-S") {
recordScans = true;
} else if (arg == "-A") {
runAllAlgorithms = true;
} else if (arg == "-H") {
SpeedSensor::HibernateWhenInactive = true;
} else {
usage();
}
}
// Seed rng
QTime time = QTime::currentTime();
qsrand((uint)time.msec());
settings = new QSettings(QSettings::SystemScope, MOLE_ORGANIZATION,
MOLE_APPLICATION, this);
//////////////////////////////////////////////////////////
// Read default settings
if (settings->contains("wifi_scanning")) {
runWiFiScanner = settings->value("wifi_scanning").toBool();
}
if (settings->contains("movement_detector")) {
runMovementDetector = settings->value("movement_detector").toBool();
}
if (settings->contains("map_server_url")) {
mapServerURL = settings->value("map_server_url").toString();
}
if (settings->contains("fingerprint_server_url")) {
staticServerURL = settings->value("fingerprint_server_url").toString();
}
if (settings->contains("port")) {
port = settings->value("port").toInt();
}
if (settings->contains("root_path")) {
rootPathname = settings->value("root_path").toString();
}
if (isDaemon) {
daemonize();
} else {
qDebug() << "not daemonizing";
}
//////////////////////////////////////////////////////////
// check a few things before daemonizing
initCommon(this, logFilename);
qWarning() << "Starting mole daemon "
<< "debug=" << debug
<< "port=" << port
<< "wifi_scanning=" << runWiFiScanner
<< "movement_detector=" << runMovementDetector
<< "logFilename=" << logFilename
<< "map_server_url=" << mapServerURL
<< "fingerprint_server_url=" << staticServerURL
<< "rootPath=" << rootPathname;
// start create map directory
if (!rootDir.exists("map")) {
bool ret = rootDir.mkdir("map");
if (!ret) {
qFatal("Failed to create map directory. Exiting...");
exit(-1);
}
qDebug() << "created map dir";
}
// end create map directory
// reset session cookie on MOLEd restart
resetSessionCookie();
m_localizer = new Localizer(this, runAllAlgorithms);
if (runMovementDetector && SpeedSensor::haveAccelerometer()) {
m_scanQueue = new ScanQueue(this, m_localizer, 0, recordScans);
} else {
// since we are not detecting motion, only use this many scans
// for localization
const int maxActiveQueueLength = 12;
m_scanQueue = new ScanQueue(this, m_localizer, maxActiveQueueLength, recordScans);
}
m_binder = new Binder(this, m_localizer, m_scanQueue);
m_proximity = new Proximity(this, m_localizer);
m_localServer = new LocalServer(this, m_localizer, m_binder, port);
m_scanner = 0;
if (runWiFiScanner) {
m_scanner = new Scanner(this);
connect(m_scanner, SIGNAL(setWiFiDesc(QString)), m_binder, SLOT(setWiFiDesc(QString)));
connect(m_scanner, SIGNAL(addReading(QString,QString,qint16,qint8)),
m_scanQueue, SLOT(addReading(QString,QString,qint16,qint8)));
connect(m_scanner, SIGNAL(scanCompleted()), m_scanQueue, SLOT(scanCompleted()));
}
m_speedSensor = 0;
if (runMovementDetector && SpeedSensor::haveAccelerometer()) {
m_speedSensor = new SpeedSensor(this);
connect(m_speedSensor, SIGNAL(hibernate(bool)), m_scanner, SLOT(handleHibernate(bool)));
connect(m_speedSensor, SIGNAL(hibernate(bool)), m_localizer, SLOT(handleHibernate(bool)));
connect(m_speedSensor, SIGNAL(motionChange(Motion)), m_localizer, SLOT(handleMotionChange(Motion)));
connect(m_speedSensor, SIGNAL(motionChange(Motion)), m_scanQueue, SLOT(handleMotionChange(Motion)));
}
connect(this, SIGNAL(aboutToQuit()), SLOT(handle_quit()));
if (runWiFiScanner) {
m_scanner->start();
}
}
int log_device(struct sOneWireDevice *device_to_log) {
return (addReading(device_to_log));
}
