huge commit

- 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

tests/grid/TestGridWalkV2.cpp

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