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

50
nav/mesh/NavControllerMesh.cpp
View File

@@ -213,10 +213,8 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
static float avgSum = 0; static float avgSum = 0;
static int avgCount = 0; static int avgCount = 0;
// fixed update rate based on incoming sensor data const Timestamp diff = curObs.currentTime - lastTransition;
// allows working with live data and faster for offline data if (curCtrl.numStepsSinceLastTransition > 0){
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 // 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; const Timestamp err = diff - Settings::Filter::updateEvery;
@@ -245,31 +243,33 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
/** perform a filter-update (called from a background-loop) */ /** perform a filter-update (called from a background-loop) */
void MeshBased::NavControllerMesh::filterUpdate() { void MeshBased::NavControllerMesh::filterUpdate() {
// //lastEst = curEst; //lastEst = curEst;
// curEst = pf->update(&curCtrl, curObs); MyState sCurEst = pf->update(&curCtrl, curObs);
// //Log::add("Nav", "cur est: " + curEst.position.asString()); curEst.pos_m = sCurEst.loc.pos;
curEst.head = sCurEst.heading;
// // inform listeners about the new estimation // inform listeners about the new estimation
// for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.position.inMeter());} 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->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->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
// // update estimated path // update estimated path
// estPath.push_back(curEst.position.inMeter()); 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->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"); Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
// PFTrans* trans = (PFTrans*)pf->getTransition(); //TODO: Implement destination for Mesh
// const MyGridNode* node = grid->getNodePtrFor(curEst.position); // PFTrans* trans = (PFTrans*)pf->getTransition();
// if (node) { // const MyGridNode* node = grid->getNodePtrFor(curEst.position);
// try { // if (node) {
// pathToDest = trans->modDestination.getShortestPath(*node); // try {
// Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed"); // pathToDest = trans->modDestination.getShortestPath(*node);
// Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed"); // Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
// } catch (...) {;} // Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
// } // } catch (...) {;}
// // mainController->getMapView()->showGridImportance(); // }
// mainController->getMapView()->showGridImportance();
} }