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

geometry changes/fixes/new features

Browse Source 
new grid walkers + fixes
new test-cases
worked on step/and turn detection
android offline-data-reader
worked on vap-grouping
This commit is contained in:
FrankE
2016-09-07 10:16:51 +02:00
parent a203305628
commit d283d9b326
27 changed files with 976 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
tests/data/WalkHeadingMap.xml Normal file
View File

@@ -0,0 +1,30 @@
<map width="70" depth="50">
<floors>
<floor atHeight="0" height="4" name="0. Stock">
<outline>
<polygon name="new" method="0">
<point x="0" y="0"/>
<point x="20" y="0"/>
<point x="20" y="20"/>
<point x="-0" y="20"/>
</polygon>
</outline>
<obstacles>
<line material="3" type="1" x1="9" y1="15" x2="11" y2="15"/>
<line material="3" type="1" x1="15" y1="11" x2="15" y2="9"/>
<line material="3" type="1" x1="9" y1="5" x2="11" y2="5"/>
<line material="3" type="1" x1="5" y1="11" x2="5" y2="9"/>
<line material="3" type="1" x1="14" y1="15" x2="15" y2="14"/>
<line material="3" type="1" x1="15" y1="6" x2="14" y2="5"/>
<line material="3" type="1" x1="5" y1="6" x2="6" y2="5"/>
<line material="3" type="1" x1="5" y1="14" x2="6" y2="15"/>
<line material="3" type="1" x1="21" y1="-1" x2="21" y2="-5"/>
</obstacles>
<underlays/>
<pois/>
<accesspoints/>
<beacons/>
<stairs/>
</floor>
</floors>
</map>

8
tests/grid/Plot.h
View File

@@ -43,6 +43,9 @@ public:
gp << "set cbrange[0.5:1.5]\n";
gp << "set palette gray negative\n";
gp << "set xlabel 'x'\n";
gp << "set ylabel 'y'\n";
//gp << "set hidden3d front\n";
splot.add(&nodes); nodes.setPointSize(0.5);
@@ -72,13 +75,14 @@ public:
template <typename T> Plot& addEdges(Grid<T>& g) {
// prevent adding edges twice
std::set<size_t> done;
std::set<std::string> done;
for (T& n1 : g) {
for (const T& n2 : g.neighbors(n1)) {
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);
//size_t edge = uid1+uid2;
std::string edge = std::to_string(std::max(uid1,uid2)) + std::to_string(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);

177
tests/grid/TestGridWalk2HeadingControl.cpp Normal file
View File

@@ -0,0 +1,177 @@
#ifdef WITH_TESTS
#include <fstream>
#include "../Tests.h"
#include "Plot.h"
#include "../../floorplan/v2/FloorplanReader.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 MyNode12456012 : public GridPoint, public GridNode {
MyNode12456012() {;}
MyNode12456012(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
};
struct MyState0012345 : public WalkState, public WalkStateHeading {
MyState0012345(const GridPoint& pos, const Heading& heading, const float headingError) : WalkState(pos), WalkStateHeading(heading, headingError) {;}
};
TEST(GridWalk2HeadingControl, Heading) {
const Heading head0(0, 0, 0, +20);
const Heading head1(0, 0, 20, +20);
const Heading head2(0, 0, 20, 0);
const Heading head3(0, 0, 20, -20);
const Heading head4(0, 0, 0, -20);
const Heading baseHead = 0;
const float diff0 = head0.getDiffHalfRAD(baseHead);
const float diff1 = head1.getDiffHalfRAD(baseHead);
const float diff2 = head2.getDiffHalfRAD(baseHead);
const float diff3 = head3.getDiffHalfRAD(baseHead);
const float diff4 = head4.getDiffHalfRAD(baseHead);
const float d = 0.0001;
ASSERT_NEAR(0, diff2, d);
ASSERT_NEAR(diff1, diff3, d);
ASSERT_NEAR(diff0, diff4, d);
}
TEST(GridWalk2HeadingControl, LIVE_walkHeading) {
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(getDataFile("WalkHeadingMap.xml"));
Grid<MyNode12456012> grid(25);
GridFactory<MyNode12456012> fac(grid);
fac.build(map);
const int sxy = 2000;
// int sxy = 2000;
// for (int x = 0; x < sxy; x += 20) {
// for (int y = 0; y < sxy; y+= 20) {
// grid.add(MyNode12456012(x,y,0));
// }
// }
// for (int x = 0; x < sxy; x += 20) {
// for (int y = 0; y < sxy; y+= 20) {
// MyNode12456012* n1 = (MyNode12456012*)grid.getNodePtrFor(GridPoint(x,y,0));
// // connect horizontally
// for (int dx = -20; dx <= +20; dx += 20) {
// for (int dy = -20; dy <= +20; dy += 20) {
// if (dx == 0 && dy == 0) {continue;}
// MyNode12456012* n2 = (MyNode12456012*)grid.getNodePtrFor(GridPoint(x+dx,y+dy,0));
// if (n2) { grid.connectUniDir(*n1, *n2); }
// }
// }
// }
// }
struct MyControl {
float turnAngle = 0;
} ctrl;
// one particle
struct Particle {
GridPoint pos;
Heading head;
float headErr = 0;
Particle(const GridPoint pos, const Heading head) : pos(pos), head(head) {;}
};
GridWalker<MyNode12456012, MyState0012345> walker;
WalkModuleHeadingControl<MyNode12456012, MyState0012345, MyControl> modPres(&ctrl, 0.0);
walker.addModule(&modPres);
Plot p;
//p.addEdges(grid);
p.addNodes(grid);
// noisy step size
std::minstd_rand gen;
std::normal_distribution<float> dist(0.75, 0.10);
for (float rad = 0; rad < 6*M_PI; rad += M_PI*0.125) {
// setup particles
std::vector<Particle> particles;
for (int i = 0; i < 75; ++i) {
particles.push_back( Particle(GridPoint(sxy/2, sxy/2, 0), 0.0f) );
}
// run
for (int i = 0; i < 30; ++i) {
ctrl.turnAngle = (i == 0) ? (rad) : (0);
p.clearParticles();
for (Particle& particle : particles) {
// particle -> state
MyState0012345 state(particle.pos, particle.head, particle.headErr);
// process state
const float dist_m = dist(gen);
state = walker.getDestination(grid, state, dist_m);
// state -> particle
particle.head = state.heading.direction;
particle.pos = state.position;
particle.headErr = state.heading.error;
p.addParticle(Point3(particle.pos.x_cm, particle.pos.y_cm, particle.pos.z_cm));
}
Point2 p1(sxy/2, sxy/2);
Point2 p2 = p1 + (Point2(1,0).rotated(rad) * sxy/2);
p.gp << "set arrow 1 from " << p1.x << "," << p1.y << "," << 0 << " to " << p2.x << "," << p2.y << "," << 0 << " front \n";
p.fire();
usleep(1000*33);
}
}
}
#endif

32
tests/grid/GridWalk2RelPressure.cpp → tests/grid/TestGridWalk2RelPressure.cpp
View File

@@ -27,13 +27,13 @@
#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 MyNode124234 : public GridPoint, public GridNode {
MyNode124234() {;}
MyNode124234(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) {;}
struct MyState316123 : public WalkState, public WalkStateRelativePressure {
MyState316123(const GridPoint& pos, const float hpa) : WalkState(pos), WalkStateRelativePressure(hpa) {;}
};
@@ -41,7 +41,7 @@ struct MyState : public WalkState, public WalkStateRelativePressure {
TEST(GridWalk2RelPressure, LIVE_walkHeading) {
Grid<MyNode> grid(20);
Grid<MyNode124234> grid(20);
int sxy = 300;
int sz = 600;
@@ -49,7 +49,7 @@ TEST(GridWalk2RelPressure, LIVE_walkHeading) {
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));
grid.add(MyNode124234(x,y,z));
}
}
}
@@ -60,8 +60,8 @@ TEST(GridWalk2RelPressure, LIVE_walkHeading) {
// 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));
MyNode124234* n1 = (MyNode124234*)grid.getNodePtrFor(GridPoint(x,y,z));
MyNode124234* n2 = (MyNode124234*)grid.getNodePtrFor(GridPoint(x,y,z+dz));
if (n1&&n2) {
grid.connectUniDir(*n1, *n2);
}
@@ -71,8 +71,8 @@ TEST(GridWalk2RelPressure, LIVE_walkHeading) {
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));
MyNode124234* n1 = (MyNode124234*)grid.getNodePtrFor(GridPoint(x,y,z));
MyNode124234* n2 = (MyNode124234*)grid.getNodePtrFor(GridPoint(x+dx,y+dy,z));
if (n1&&n2) {
grid.connectUniDir(*n1, *n2);
}
@@ -92,11 +92,11 @@ TEST(GridWalk2RelPressure, LIVE_walkHeading) {
} ctrl;
GridPoint start(120,120,sz/2);
const MyNode* n = grid.getNodePtrFor(start);
const MyNode124234* n = grid.getNodePtrFor(start);
MyState ms(start, 0);
GridWalker<MyNode, MyState> walker;
WalkModuleRelativePressureControl<MyNode, MyState, MyControl> modPres(&ctrl, 0.1);
MyState316123 ms(start, 0);
GridWalker<MyNode124234, MyState316123> walker;
WalkModuleRelativePressureControl<MyNode124234, MyState316123, MyControl> modPres(&ctrl, 0.1);
walker.addModule(&modPres);
Plot p;
@@ -109,7 +109,7 @@ TEST(GridWalk2RelPressure, LIVE_walkHeading) {
ctrl.barometer.hPaRelativeToT0 = (std::sin(i/25.0)) * 0.3;
ms = walker.getDestination(grid, ms, 0.3);
p.addParticle(ms.startPos.inCentimeter());
p.addParticle(ms.position.inCentimeter());
usleep(1000*50);
p.gp << "set label 97 at screen 0.05, 0.95 ' baro: " << ctrl.barometer.hPaRelativeToT0 << "'\n";

10
tests/grid/TestGridWalkV2.cpp
View File

@@ -39,7 +39,7 @@ struct MyNode1239 : public GridPoint, public GridNode, public GridNodeImportance
// ENSURE UNIQUE CLASS NAME
struct MyState23452 : public WalkState, public WalkStateHeading {
MyState23452(const GridPoint& pos, const Heading head) : WalkState(pos), WalkStateHeading(head) {;}
MyState23452(const GridPoint& pos, const Heading head) : WalkState(pos), WalkStateHeading(head, 0) {;}
};
@@ -64,7 +64,7 @@ TEST(GridWalk2, LIVE_error) {
} ctrl;
GridWalker<MyNode1239, MyState23452> walker;
WalkModuleHeadingControl<MyNode1239, MyState23452, Control> modHead(&ctrl);
WalkModuleHeadingControl<MyNode1239, MyState23452, Control> modHead(&ctrl, 3.0f);
walker.addModule(&modHead);
@@ -81,9 +81,9 @@ TEST(GridWalk2, LIVE_error) {
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";
gp << "set label 1 at screen 0.5,0.5 '" << state.heading.direction.getRAD() << "'\n";
lines.add(K::GnuplotPoint2(state.startPos.x_cm, state.startPos.y_cm));
lines.add(K::GnuplotPoint2(state.position.x_cm, state.position.y_cm));
gp.draw(plot);
gp.flush();
@@ -147,7 +147,7 @@ TEST(GgridWalk2, LIVE_walkHeading) {
for (int i = 0; i < points.size(); ++i) {
MyState23452 start(points[i], Heading(0.0));
MyState23452 next = walker.getDestination(grid, start, 2.0);
points[i] = next.startPos;
points[i] = next.position;
states.add(K::GnuplotPoint3(points[i].x_cm, points[i].y_cm, points[i].z_cm));
}