FHWS/YASMIN
Archived
3
0
Fork 0
Code Issues 12 Pull Requests Releases Wiki Activity

activated filter. is building. however still crashes on loading map

Browse Source
This commit is contained in:
toni
2018-07-13 19:51:08 +02:00
parent 625f5fe04d
commit e6ede27212
1 changed files with 25 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
nav/mesh/NavControllerMesh.cpp
View File

@@ -213,10 +213,8 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
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) {
if (curCtrl.numStepsSinceLastTransition > 0){
// 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;
@@ -245,21 +243,23 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
/** perform a filter-update (called from a background-loop) */
void MeshBased::NavControllerMesh::filterUpdate() {
// //lastEst = curEst;
// curEst = pf->update(&curCtrl, curObs);
// //Log::add("Nav", "cur est: " + curEst.position.asString());
//lastEst = curEst;
MyState sCurEst = pf->update(&curCtrl, curObs);
curEst.pos_m = sCurEst.loc.pos;
curEst.head = sCurEst.heading;
// // inform listeners about the new estimation
// for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.position.inMeter());}
// 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");
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");
// 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");
//TODO: Implement destination for Mesh
// PFTrans* trans = (PFTrans*)pf->getTransition();
// const MyGridNode* node = grid->getNodePtrFor(curEst.position);
// if (node) {
@@ -269,7 +269,7 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
// Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
// } catch (...) {;}
// }
// // mainController->getMapView()->showGridImportance();
// mainController->getMapView()->showGridImportance();
}