FHWS/YASMIN
Archived
3
0
Fork 0
Code Issues 12 Pull Requests Releases Wiki Activity
This repository has been archived on 2020-04-08. You can view files and clone it, but cannot push or open issues or pull requests.
Files
0bd9a5b5ce87fdffc8b4e31dfb15d32f69adc39b
YASMIN/nav/NavController.h
kazu 44f9b6ac80 performance enhancements 
memory enhancements
prevent starting sensors more than once
fix for wifi lag issues
map scaling for huge buildings
2016-10-01 10:21:15 +02:00

394 lines
13 KiB
C++
Raw Blame History

#ifndef NAVCONTROLLER_H
#define NAVCONTROLLER_H
#include "../sensors/AccelerometerSensor.h"
#include "../sensors/GyroscopeSensor.h"
#include "../sensors/BarometerSensor.h"
#include "../sensors/WiFiSensor.h"
#include "../sensors/SensorFactory.h"
#include "../sensors/StepSensor.h"
#include "../sensors/TurnSensor.h"
#include "../ui/debug/SensorDataWidget.h"
#include "../ui/map/3D/MapView3D.h"
#include "../ui/debug/InfoWidget.h"
#include <Indoor/Assertions.h>
#include <thread>
#include "State.h"
#include "Filter.h"
#include "Controller.h"
#include "NavControllerListener.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#ifndef ANDROID
#include <valgrind/callgrind.h>
#endif
#include "Settings.h"
#include "RegionalResampling.h"
#include "NodeResampling.h"
Q_DECLARE_METATYPE(const void*)
class NavController :
public SensorListener<AccelerometerData>,
public SensorListener<GyroscopeData>,
public SensorListener<BarometerData>,
public SensorListener<WiFiMeasurements>,
public SensorListener<GPSData>,
public SensorListener<StepData>,
public SensorListener<TurnData>,
public SensorListener<ActivityData> {
private:
Controller* mainController;
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling wifiModel;
Floorplan::IndoorMap* im;
MyObservation curObs;
MyControl curCtrl;
bool running = false;
std::thread tFilter;
std::thread tDisplay;
std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf;
/** the estimated path */
std::vector<Point3> estPath;
/** all listeners */
std::vector<NavControllerListener*> listeners;
public:
virtual ~NavController() {
if (running) {stop();}
}
/** ctor */
NavController(Controller* mainController, Grid<MyGridNode>* grid, Floorplan::IndoorMap* im) : mainController(mainController), grid(grid), wifiModel(im), im(im) {
// filter init
std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
// estimation
//std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>());
std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// resampling
std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
//std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.05));
//std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
// eval and transition
wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
// setup the filter
pf = std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>>(new K::ParticleFilter<MyState, MyControl, MyObservation>(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.75);
//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);
// hacky.. but we need to call this one from the main thread!
//mainController->getMapView()->showParticles(pf->getParticles());
qRegisterMetaType<const void*>();
}
/** attach a new event listener */
void addListener(NavControllerListener* l) {
listeners.push_back(l);
}
void start() {
Assert::isFalse(running, "already started!");
running = true;
curCtrl.resetAfterTransition(); // ensure we start empty ;)
tFilter = std::thread(&NavController::filterUpdateLoop, this);
tDisplay = std::thread(&NavController::updateMapViewLoop, this);
// start all sensors
SensorFactory::get().getAccelerometer().start();
SensorFactory::get().getGyroscope().start();
SensorFactory::get().getBarometer().start();
SensorFactory::get().getWiFi().start();
#ifndef ANDROID
// #include <valgrind/callgrind.h>
// run with
// valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin
// show with
// kcachegrind callgrind.out.xxxx
CALLGRIND_START_INSTRUMENTATION;
#endif
}
void stop() {
Assert::isTrue(running, "not started!");
running = false;
tFilter.join();
tDisplay.join();
}
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override {
(void) sensor;
(void) data;
(void) ts;
gotSensorData(ts);
}
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override {
(void) sensor;
(void) ts;
curObs.wifi = data;
gotSensorData(ts);
}
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override {
(void) sensor;
(void) ts;
curObs.gps = data;
gotSensorData(ts);
}
void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override {
(void) sensor;
(void) ts;
curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
}
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override {
(void) sensor;
(void) ts;
curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
}
void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override {
(void) sensor;
(void) ts;
curCtrl.activity = data.curActivity;
curObs.activity = data.curActivity;
debugActivity(data.curActivity);
gotSensorData(ts);
}
int cameraMode = 0;
void toggleCamera() {
cameraMode = (cameraMode + 1) % 3;
}
private:
/** called when any sensor has received new data */
void 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 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)); }
}
}
Timestamp lastTransition;
/** check whether its time for a filter update, and if so, execute the update and return true */
bool 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;
}
}
MyState curEst;
DijkstraPath<MyGridNode> pathToDest;
/** perform a filter-update (called from a background-loop) */
void filterUpdate() {
//lastEst = curEst;
curEst = pf->update(&curCtrl, curObs);
Log::add("Nav", "cur est: " + curEst.position.asString());
// inform listeners about the new estimation
for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.position.inMeter());}
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.position.inMeter());
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(curEst.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();
}
const int display_ms = Settings::MapView3D::msPerFrame.ms();
/** UI update loop */
void 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));
}
}
Point3 curPosFast;
Point3 curPosSlow;
/** update the map-view (called from within a background-loop) */
void updateMapView() {
const float kappa1 = display_ms / 1000.0f;
const float kappa2 = kappa1 * 0.7;
const float myHeight_m = 1.80;
curPosFast = curPosFast * (1-kappa1) + curEst.position.inMeter() * (kappa1);
curPosSlow = curPosSlow * (1-kappa2) + curEst.position.inMeter() * (kappa2);
const Point3 dir = (curPosFast - curPosSlow).normalized();
const Point3 dir2 = Point3(dir.x, dir.y, -0.2).normalized();
// how to update the camera
if (cameraMode == 0) {
mainController->getMapView3D()->setLookAt(curPosFast + Point3(0,0,myHeight_m), dir);
} else if (cameraMode == 1) {
mainController->getMapView3D()->setLookAt(curPosFast + Point3(0,0,myHeight_m) - dir2*4, dir2);
} else if (cameraMode == 2) {
const Point3 spectator = curPosFast + Point3(0,0,25) - dir*15;
const Point3 spectatorDir = (curPosFast - spectator).normalized();
mainController->getMapView3D()->setLookEye(spectator);
mainController->getMapView3D()->setLookDir(spectatorDir);
}
mainController->getMapView3D()->setClipAbove(curEst.position.inMeter().z + 2);
mainController->getMapView3D()->setCurrentEstimation(curEst.position.inMeter(), dir);
mainController->getMapView2D()->setCurrentEstimation(curEst.position.inMeter(), dir);
}
};
#endif // NAVCONTROLLER_H
Reference in New Issue View Git Blame Copy Permalink