added new sanity checks and compile-time assertions to prevent errors

fixed stair-building issue
new test-cases
added elevator support
fixed/improved some walker modules

tests/Tests.h

@@ -6,7 +6,8 @@
 #include <gtest/gtest.h>
 
 static inline std::string getDataFile(const std::string& name) {
-    return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name;
+	return "/mnt/data/workspaces/Indoor/tests/data/" + name;
+	//return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name;
 }
 
 

tests/data/TestRingBuffer.cpp

@@ -60,6 +60,97 @@ TEST(RingBuffer, add) {
 
 }
 
+TEST(RingBuffer, peek) {
+
+	RingBuffer<int> buf(4);
+
+	buf.add(11);
+	ASSERT_EQ(11, buf.peek(0));
+
+	buf.add(13);
+	ASSERT_EQ(11, buf.peek(0));
+	ASSERT_EQ(13, buf.peek(1));
+
+	buf.add(10);
+	ASSERT_EQ(11, buf.peek(0));
+	ASSERT_EQ(13, buf.peek(1));
+	ASSERT_EQ(10, buf.peek(2));
+
+	buf.add(17);
+	ASSERT_EQ(11, buf.peek(0));
+	ASSERT_EQ(13, buf.peek(1));
+	ASSERT_EQ(10, buf.peek(2));
+	ASSERT_EQ(17, buf.peek(3));
+
+	buf.add(18);
+	ASSERT_EQ(18, buf.peek(0));
+	ASSERT_EQ(13, buf.peek(1));
+	ASSERT_EQ(10, buf.peek(2));
+	ASSERT_EQ(17, buf.peek(3));
+
+	buf.add(9);
+	ASSERT_EQ(18, buf.peek(0));
+	ASSERT_EQ( 9, buf.peek(1));
+	ASSERT_EQ(10, buf.peek(2));
+	ASSERT_EQ(17, buf.peek(3));
+
+	buf.add(88);
+	ASSERT_EQ(18, buf.peek(0));
+	ASSERT_EQ( 9, buf.peek(1));
+	ASSERT_EQ(88, buf.peek(2));
+	ASSERT_EQ(17, buf.peek(3));
+
+	buf.add(54);
+	ASSERT_EQ(18, buf.peek(0));
+	ASSERT_EQ( 9, buf.peek(1));
+	ASSERT_EQ(88, buf.peek(2));
+	ASSERT_EQ(54, buf.peek(3));
+
+	buf.add(1);
+	ASSERT_EQ( 1, buf.peek(0));
+	ASSERT_EQ( 9, buf.peek(1));
+	ASSERT_EQ(88, buf.peek(2));
+	ASSERT_EQ(54, buf.peek(3));
+
+}
+
+TEST(RingBuffer, contains) {
+
+	RingBuffer<int> buf(4);
+
+	ASSERT_FALSE(buf.contains(0));
+	buf.add(0);
+	ASSERT_TRUE(buf.contains(0));
+
+	ASSERT_FALSE(buf.contains(3));
+	buf.add(3);
+	ASSERT_TRUE(buf.contains(0));
+	ASSERT_TRUE(buf.contains(3));
+
+	ASSERT_FALSE(buf.contains(7));
+	buf.add(7);
+	ASSERT_TRUE(buf.contains(0));
+	ASSERT_TRUE(buf.contains(3));
+	ASSERT_TRUE(buf.contains(7));
+
+	ASSERT_FALSE(buf.contains(8));
+	buf.add(8);
+	ASSERT_TRUE(buf.contains(0));
+	ASSERT_TRUE(buf.contains(3));
+	ASSERT_TRUE(buf.contains(7));
+	ASSERT_TRUE(buf.contains(8));
+
+
+	ASSERT_FALSE(buf.contains(11));
+	buf.add(11);
+	ASSERT_FALSE(buf.contains(0));
+	ASSERT_TRUE(buf.contains(3));
+	ASSERT_TRUE(buf.contains(7));
+	ASSERT_TRUE(buf.contains(8));
+	ASSERT_TRUE(buf.contains(11));
+
+}
+
 TEST(RingBuffer, iterator) {
 
 	RingBuffer<int> buf(4);

tests/grid/TestStairs.cpp

@@ -0,0 +1,99 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include "Plot.h"
+#include "../../grid/factory/v2/GridFactory.h"
+#include "../../grid/factory/v2/Importance.h"
+
+#include "../../grid/walk/v2/GridWalker.h"
+#include "../../grid/walk/v2/modules/WalkModuleFavorZ.h"
+#include "../../grid/walk/v2/modules/WalkModuleSpread.h"
+#include "../../grid/walk/v2/modules/WalkModuleHeading.h"
+#include "../../grid/walk/v2/modules/WalkModuleNodeImportance.h"
+#include "../../grid/walk/v2/modules/WalkModulePreventVisited.h"
+
+#include "../../floorplan/v2/FloorplanReader.h"
+
+#include "../../data/RingBuffer.h"
+
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
+
+
+// ENSURE UNIQUE CLASS NAME
+struct MyNode345092134 : public GridPoint, public GridNode {
+	float navImportance = 0;
+	float getNavImportance() const {return navImportance;}
+	MyNode345092134() {;}
+	MyNode345092134(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
+};
+
+struct MyState102395234 : public WalkState, public WalkStateSpread, public WalkStateFavorZ {
+	RingBuffer<int> history;
+	MyState102395234(const int x, const int y, const int z) : WalkState(GridPoint(x,y,z)), history(5) {;}
+};
+
+//TEST(Walk, plot) {
+TEST(Stairs, live_testWalk) {
+
+	Grid<MyNode345092134> g(20);
+	GridFactory<MyNode345092134> gf(g);
+
+	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(getDataFile("MapStairs.xml"));
+	gf.build(map, nullptr);
+	Importance::addImportance(g);
+
+	K::Gnuplot gp;
+	K::GnuplotSplot splot;
+	K::GnuplotSplotElementPoints pnodes; splot.add(&pnodes);	pnodes.setColorHex("#888888");	pnodes.setPointType(7);
+	K::GnuplotSplotElementPoints pstates; splot.add(&pstates);	pstates.setColorHex("#0000ff");	pstates.setPointType(7);	pstates.setPointSize(1);
+
+	for (int i = 0; i < g.getNumNodes(); i+=2) {
+		const MyNode345092134& gp = g[i];
+		pnodes.add(K::GnuplotPoint3(gp.x_cm, gp.y_cm, gp.z_cm));
+	}
+
+	std::vector<MyState102395234> states;
+	for (int i = 0; i < 5000; ++i) {
+		states.push_back(MyState102395234(20,20,0));
+	}
+
+	GridWalker<MyNode345092134, MyState102395234> gw;
+	WalkModuleFavorZ<MyNode345092134, MyState102395234> modFavorZ;
+	WalkModuleSpread<MyNode345092134, MyState102395234> modSpread;
+	WalkModuleNodeImportance<MyNode345092134, MyState102395234> modImp;
+	WalkModulePreventVisited<MyNode345092134, MyState102395234> modSkipDup;
+	//WalkModuleHeading<MyNode345092134, MyState102395234> modHead;
+
+	gw.addModule(&modImp);
+	gw.addModule(&modFavorZ);
+	gw.addModule(&modSpread);
+	gw.addModule(&modSkipDup);
+	//gw.addModule(&modHead);
+
+	for (int run = 0; run < 5000; ++run) {
+
+		pstates.clear();
+		for (int i = 0; i < 100; ++i) {
+			MyState102395234& state = states[i];
+			state = gw.getDestination(g, state, 0.75);
+			pstates.add(K::GnuplotPoint3(state.position.x_cm, state.position.y_cm, state.position.z_cm));
+		}
+
+		gp.draw(splot);
+		gp.flush();
+		usleep(1000*50);
+
+	}
+
+
+
+
+
+
+}
+
+#endif

tests/sensors/pressure/TestBarometer.cpp

@@ -127,7 +127,7 @@ TEST(Barometer, Activity) {
 
     //read file
     std::string line;
-    std::string filename = getDataFile("baro/logfile_UAH_R2_S3_baro.dat");
+	std::string filename = getDataFile("barometer/baro1.dat");
     std::ifstream infile(filename);
 
     while (std::getline(infile, line))