FrankE
a2c9e575a2
- 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
164 lines
4.0 KiB
C++
164 lines
4.0 KiB
C++
#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
|