FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

huge commit

Browse Source 
- worked on about everything
- grid walker using plugable modules
- wifi models
- new distributions
- worked on geometric data-structures
- added typesafe timestamps
- worked on grid-building
- added sensor-classes
- added sensor analysis (step-detection, turn-detection)
- offline data reader
- many test-cases
This commit is contained in:
FrankE
2016-08-29 08:18:44 +02:00
parent 99ee95ce7b
commit a2c9e575a2
94 changed files with 8298 additions and 257 deletions
Download Patch File Download Diff File Expand all files Collapse all files

133
tests/grid/GridWalk2RelPressure.cpp Normal file
View File

@@ -0,0 +1,133 @@
#ifdef WITH_TESTS
#include <fstream>
#include "../Tests.h"
#include "Plot.h"
#include "../../grid/factory/v2/GridFactory.h"
#include "../../grid/factory/v2/GridNodeImportance.h"
#include "../../grid/walk/v2/GridWalker.h"
#include "../../grid/walk/v2/modules/WalkModuleHeading.h"
#include "../../grid/walk/v2/modules/WalkModuleHeadingControl.h"
#include "../../grid/walk/v2/modules/WalkModuleFollowDestination.h"
#include "../../grid/walk/v2/modules/WalkModuleRelativePressureControl.h"
#include "../../sensors/radio/WiFiGridNode.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
struct MyNode : public GridPoint, public GridNode {
MyNode() {;}
MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
};
struct MyState : public WalkState, public WalkStateRelativePressure {
MyState(const GridPoint& pos, const float hpa) : WalkState(pos), WalkStateRelativePressure(hpa) {;}
};
TEST(GridWalk2RelPressure, walkHeading) {
Grid<MyNode> grid(20);
int sxy = 300;
int sz = 600;
for (int x = 0; x < sxy; x += 40) {
for (int y = 0; y < sxy; y+= 40) {
for (int z = 0; z < sz; z += 20) {
grid.add(MyNode(x,y,z));
}
}
}
for (int x = 0; x < sxy; x += 40) {
for (int y = 0; y < sxy; y+= 40) {
for (int z = 0; z < sz; z += 20) {
// connect vertically
for (int dz = -20; dz <= +20; dz += 40) {
MyNode* n1 = (MyNode*)grid.getNodePtrFor(GridPoint(x,y,z));
MyNode* n2 = (MyNode*)grid.getNodePtrFor(GridPoint(x,y,z+dz));
if (n1&&n2) {
grid.connectUniDir(*n1, *n2);
}
}
// connect horizontally
for (int dx = -40; dx <= +40; dx += 40) {
for (int dy = -40; dy <= +40; dy += 40) {
if (dx == 0 && dy == 0) {continue;}
MyNode* n1 = (MyNode*)grid.getNodePtrFor(GridPoint(x,y,z));
MyNode* n2 = (MyNode*)grid.getNodePtrFor(GridPoint(x+dx,y+dy,z));
if (n1&&n2) {
grid.connectUniDir(*n1, *n2);
}
}
}
}
}
}
struct MyControl {
struct Barometer {
float hPaRelativeToT0 = 0.0;
float estimatedSigma = 0.05;
} barometer;
} ctrl;
GridPoint start(120,120,sz/2);
const MyNode* n = grid.getNodePtrFor(start);
MyState ms(start, 0);
GridWalker<MyNode, MyState> walker;
WalkModuleRelativePressureControl<MyNode, MyState, MyControl> modPres(&ctrl, 0.1);
walker.addModule(&modPres);
Plot p;
p.addEdges(grid);
p.addNodes(grid);
for (int i = 0; i < 1000; ++i) {
p.clearParticles();
ctrl.barometer.hPaRelativeToT0 = (std::sin(i/25.0)) * 0.3;
ms = walker.getDestination(grid, ms, 0.3);
p.addParticle(ms.startPos.inCentimeter());
usleep(1000*50);
p.gp << "set label 97 at screen 0.05, 0.95 ' baro: " << ctrl.barometer.hPaRelativeToT0 << "'\n";
p.gp << "set label 98 at screen 0.05, 0.90 ' state:" << ms.pressureRelToT0 << "'\n";
p.gp << "set label 99 at screen 0.05, 0.85 ' diff: " << std::abs(ctrl.barometer.hPaRelativeToT0-ms.pressureRelToT0) << "'\n";
p.fire();
}
p.fire();
sleep(1000);
}
#endif

23
tests/grid/Plot.h
View File

@@ -33,6 +33,7 @@ public:
K::GnuplotSplotElementColorPoints nodes;
K::GnuplotSplotElementLines lines;
K::GnuplotSplotElementLines floors;
K::GnuplotSplotElementPoints particles;
/** ctor */
Plot() {
@@ -44,14 +45,24 @@ public:
//gp << "set hidden3d front\n";
splot.add(&nodes);
splot.add(&nodes); nodes.setPointSize(0.5);
splot.add(&lines); lines.setColorHex("#999999");
splot.add(&floors);
splot.add(&particles); particles.setColorHex("#0000ff"); particles.setPointSize(0.5);
floors.setLineWidth(2);
floors.setColorHex("#008800");
}
void clearParticles() {
particles.clear();;
}
void addParticle(const Point3 p) {
particles.add(K::GnuplotPoint3(p.x, p.y, p.z));
}
template <typename T> Plot& showGrid(Grid<T>& g) {
addEdges(g);
addNodes(g);
@@ -63,9 +74,11 @@ public:
// prevent adding edges twice
std::set<size_t> done;
for (GP& n1 : g) {
for (T& n1 : g) {
for (const T& n2 : g.neighbors(n1)) {
size_t edge = g.getUID(n1) ^ g.getUID(n2);
const size_t uid1 = g.getUID(n1);
const size_t uid2 = g.getUID(n2);
size_t edge = uid1+uid2;//std::max(uid1,uid2) << 32 | std::min(uid1,uid2);
if (done.find(edge) == done.end()) {
K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
@@ -81,7 +94,7 @@ public:
template <typename T> Plot& addNodes(Grid<T>& g) {
for (GP& n1 : g) {
for (T& n1 : g) {
K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
nodes.add(p1, 1.0);
}

163
tests/grid/TestGridWalkV2.cpp Normal file
View File

@@ -0,0 +1,163 @@
#ifdef WITH_TESTS
#include <fstream>
#include "../Tests.h"
#include "../../grid/factory/v2/GridFactory.h"
#include "../../grid/factory/v2/GridNodeImportance.h"
#include "../../grid/walk/v2/GridWalker.h"
#include "../../grid/walk/v2/modules/WalkModuleHeading.h"
#include "../../grid/walk/v2/modules/WalkModuleHeadingControl.h"
#include "../../grid/walk/v2/modules/WalkModuleFollowDestination.h"
#include "../../sensors/radio/WiFiGridNode.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
struct MyNode : public GridPoint, public GridNode, public GridNodeImportance, public WiFiGridNode<10> {
MyNode() {;}
MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
static void staticDeserialize(std::istream& inp) {
WiFiGridNode::staticDeserialize(inp);
}
static void staticSerialize(std::ostream& out) {
WiFiGridNode::staticSerialize(out);
}
};
struct MyState : public WalkState, public WalkStateHeading {
MyState(const GridPoint& pos, const Heading head) : WalkState(pos), WalkStateHeading(head) {;}
};
TEST(GridWalk2, error) {
Grid<MyNode> grid(20);
GridFactory<MyNode> gf(grid);
Floorplan::Floor floor;
Floorplan::FloorOutlinePolygon poly;
poly.poly.points.push_back(Point2( 0, 0));
poly.poly.points.push_back(Point2( 0,50));
poly.poly.points.push_back(Point2(50,50));
poly.poly.points.push_back(Point2(50, 0));
floor.outline.push_back(&poly);
gf.addFloor(&floor);
struct Control {
float turnAngle = 0; // keep the angle as-is!
} ctrl;
GridWalker<MyNode, MyState> walker;
WalkModuleHeadingControl<MyNode, MyState, Control> modHead(&ctrl);
walker.addModule(&modHead);
MyState state(GridPoint(800,800,0), Heading(3.1415/2));
K::Gnuplot gp;
gp << "set xrange[0:5000]\n";
gp << "set yrange[0:5000]\n";
K::GnuplotPlot plot;
K::GnuplotPlotElementLines lines; plot.add(&lines);
for (int j = 0; j < 100; ++j) {
state = walker.getDestination(grid, state, 0.4);
gp << "set label 1 at screen 0.5,0.5 '" << state.startHeading.getRAD() << "'\n";
lines.add(K::GnuplotPoint2(state.startPos.x_cm, state.startPos.y_cm));
gp.draw(plot);
gp.flush();
usleep(1000*50);
}
}
TEST(GgridWalk2, walkHeading) {
Grid<MyNode> grid(20);
std::ifstream inp("/tmp/grid.dat");
grid.read(inp);
// Floorplan::FloorOutlinePolygon poly;
// poly.method =Floorplan::OutlineMethod::ADD;
// poly.poly.points.push_back(Point2(0,0));
// poly.poly.points.push_back(Point2(0,20));
// poly.poly.points.push_back(Point2(20,20));
// poly.poly.points.push_back(Point2(20,0));
// Floorplan::Floor floor;
// floor.outline.push_back(&poly);
// GridFactory<MyNode> fac(grid);
// fac.addFloor(&floor);
GridWalker<MyNode, MyState> walker;
WalkModuleHeading<MyNode, MyState> wmHead;
WalkModuleFollowDestination<MyNode, MyState> wmDest(grid, grid.getNodeFor(GridPoint(7200,4800,400+340+340)));
//walker.addModule(&wmHead);
walker.addModule(&wmDest);
std::vector<GridPoint> points;
for (int i = 0; i < 100; ++i) {points.push_back(GridPoint(200,200,0));}
K::Gnuplot gp;
K::GnuplotSplot splot;
K::GnuplotSplotElementPoints nodes; nodes.setPointSize(0.1); nodes.setColorHex("#888888"); splot.add(&nodes);
K::GnuplotSplotElementPoints states; states.setPointSize(0.5); states.setColorHex("#0000ff"); splot.add(&states);
for (const MyNode& n : grid) {
static int cnt = 0;
if (++cnt % 5 == 0) {
nodes.add(K::GnuplotPoint3(n.x_cm, n.y_cm, n.z_cm));
}
}
for (int j = 0; j < 500; ++j) {
states.clear();
for (int i = 0; i < points.size(); ++i) {
MyState start(points[i], Heading(0.0));
MyState next = walker.getDestination(grid, start, 2.0);
points[i] = next.startPos;
states.add(K::GnuplotPoint3(points[i].x_cm, points[i].y_cm, points[i].z_cm));
}
gp.draw(splot);
gp.flush();
usleep(1000*50);
}
int i = 0; (void) i;
}
#endif