geometry changes/fixes/new features

new grid walkers + fixes
new test-cases
worked on step/and turn detection
android offline-data-reader
worked on vap-grouping

tests/grid/TestGridWalk2HeadingControl.cpp

@@ -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