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)); }