#include "NavControllerGrid.h" #include #include #include #include #include "Settings.h" #include "RegionalResampling.h" #include "NodeResampling.h" #include "../ui/debug/SensorDataWidget.h" #include "../ui/map/3D/MapView3D.h" #include "../ui/map/2D/MapView2D.h" #include "../ui/debug/InfoWidget.h" #include "../Controller.h" Q_DECLARE_METATYPE(const void*) GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid* grid) : NavController(mainController, im), grid(grid), wifiModel(im) { // filter init std::unique_ptr> init(new PFInit(grid)); // estimation //std::unique_ptr> estimation(new SMC::ParticleFilterEstimationWeightedAverage()); std::unique_ptr> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage(0.5)); // resampling std::unique_ptr> resample(new NodeResampling(*grid)); //std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimple()); //std::unique_ptr> resample(new SMC::ParticleFilterResamplingPercent(0.05)); //std::unique_ptr resample(new RegionalResampling()); // eval and transition wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); std::unique_ptr> eval(new PFEval(grid, wifiModel)); std::unique_ptr> transition(new PFTrans(grid)); // setup the filter pf = std::unique_ptr>(new SMC::ParticleFilter(Settings::numParticles, std::move(init))); pf->setTransition(std::move(transition)); pf->setEvaluation(std::move(eval)); pf->setEstimation(std::move(estimation)); pf->setResampling(std::move(resample)); pf->setNEffThreshold(0.85); //before 0.75, edit by toni //pf->setNEffThreshold(0.65); // still too low? //pf->setNEffThreshold(0.25); // too low // attach as listener to all sensors SensorFactory::get().getAccelerometer().addListener(this); SensorFactory::get().getGyroscope().addListener(this); SensorFactory::get().getBarometer().addListener(this); SensorFactory::get().getWiFi().addListener(this); SensorFactory::get().getSteps().addListener(this); SensorFactory::get().getTurns().addListener(this); SensorFactory::get().getActivity().addListener(this); } void GridBased::NavControllerGrid::start() { Assert::isFalse(running, "already started!"); running = true; curCtrl.resetAfterTransition(); // ensure we start empty ;) tFilter = std::thread(&NavControllerGrid::filterUpdateLoop, this); tDisplay = std::thread(&NavControllerGrid::updateMapViewLoop, this); // start all sensors SensorFactory::get().getAccelerometer().start(); SensorFactory::get().getGyroscope().start(); SensorFactory::get().getBarometer().start(); SensorFactory::get().getWiFi().start(); #ifndef ANDROID // #include // run with // valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin // show with // kcachegrind callgrind.out.xxxx CALLGRIND_START_INSTRUMENTATION; #endif } void GridBased::NavControllerGrid::stop() { Assert::isTrue(running, "not started!"); running = false; tFilter.join(); tDisplay.join(); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const AccelerometerData& data) { (void) sensor; (void) data; (void) ts; gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const GyroscopeData& data) { (void) sensor; (void) ts; (void) data; gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const BarometerData& data) { (void) sensor; (void) ts; (void) data; gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const WiFiMeasurements& data) { (void) sensor; (void) ts; curObs.wifi = data; gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const GPSData& data) { (void) sensor; (void) ts; curObs.gps = data; gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const StepData& data) { (void) sensor; (void) ts; curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const TurnData& data) { (void) sensor; (void) ts; curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition gotSensorData(ts); } void GridBased::NavControllerGrid::onSensorData(Sensor* sensor, const Timestamp ts, const ActivityData& data) { (void) sensor; (void) ts; curCtrl.activity = data.curActivity; curObs.activity = data.curActivity; //debugActivity(data.curActivity); gotSensorData(ts); } /** called when any sensor has received new data */ void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) { curObs.currentTime = ts; if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();} } // void debugActivity(const ActivityData& activity) { // QString act; // switch(activity.curActivity) { // case ActivityButterPressure::Activity::STAY: act = "STAY"; break; // case ActivityButterPressure::Activity::DOWN: act = "DOWN"; break; // case ActivityButterPressure::Activity::UP: act = "UP"; break; // default: act = "???"; break; // } // Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed"); // } /** particle-filter update loop */ void GridBased::NavControllerGrid::filterUpdateLoop() { while(running && !Settings::Filter::useMainThread) { // // fixed update rate based on the systems time -> LIVE! even for offline data // const Timestamp ts1 = Timestamp::fromUnixTime(); // doUpdate(); // const Timestamp ts2 = Timestamp::fromUnixTime(); // const Timestamp needed = ts2-ts1; // const Timestamp sleep = Timestamp::fromMS(500) - needed; // std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms())); const bool wasUpdated = filterUpdateIfNeeded(); if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); } } } /** check whether its time for a filter update, and if so, execute the update and return true */ bool GridBased::NavControllerGrid::filterUpdateIfNeeded() { static float avgSum = 0; static int avgCount = 0; // fixed update rate based on incoming sensor data // allows working with live data and faster for offline data const Timestamp diff = curObs.currentTime - lastTransition; if (diff >= Settings::Filter::updateEvery) { // as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one const Timestamp err = diff - Settings::Filter::updateEvery; lastTransition = curObs.currentTime - err; const Timestamp ts1 = Timestamp::fromUnixTime(); filterUpdate(); const Timestamp ts2 = Timestamp::fromUnixTime(); const Timestamp tsDiff = ts2-ts1; const QString filterTime = QString::number(tsDiff.ms()); avgSum += tsDiff.ms(); ++avgCount; //Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount)); QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime)); return true; } else { return false; } } /** perform a filter-update (called from a background-loop) */ void GridBased::NavControllerGrid::filterUpdate() { //lastEst = curEst; MyState sCurEst = pf->update(&curCtrl, curObs); curEst.pos_m = sCurEst.position.inMeter(); curEst.head = sCurEst.heading.direction; //Log::add("Nav", "cur est: " + curEst.position.asString()); // inform listeners about the new estimation for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.pos_m);} Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed"); Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed"); // update estimated path estPath.push_back(curEst.pos_m); Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed"); Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed"); PFTrans* trans = (PFTrans*)pf->getTransition(); const MyGridNode* node = grid->getNodePtrFor(sCurEst.position); if (node) { try { pathToDest = trans->modDestination.getShortestPath(*node); Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed"); Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed"); } catch (...) {;} } // mainController->getMapView()->showGridImportance(); } /** UI update loop */ void GridBased::NavControllerGrid::updateMapViewLoop() { while(running) { const Timestamp ts1 = Timestamp::fromUnixTime(); updateMapView(); const Timestamp ts2 = Timestamp::fromUnixTime(); const Timestamp tsDiff = ts2-ts1; const QString mapViewTime = QString::number(tsDiff.ms()); //QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime)); std::this_thread::sleep_for(std::chrono::milliseconds(display_ms)); } }