kazu
04d8ae8c74
- some should be the same as previous commit (sorry!) - some should be new: LINT checks, ...?
166 lines
4.2 KiB
C++
166 lines
4.2 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>
|
|
|
|
|
|
// ENSURE UNIQUE CLASS NAME
|
|
struct MyNode1239 : public GridPoint, public GridNode, public GridNodeImportance, public WiFiGridNode<10> {
|
|
MyNode1239() {;}
|
|
MyNode1239(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);
|
|
}
|
|
};
|
|
|
|
// ENSURE UNIQUE CLASS NAME
|
|
struct MyState23452 : public WalkState, public WalkStateHeading {
|
|
MyState23452(const GridPoint& pos, const Heading head) : WalkState(pos), WalkStateHeading(head, 0) {;}
|
|
};
|
|
|
|
|
|
TEST(GridWalk2, LIVE_error) {
|
|
|
|
|
|
Grid<MyNode1239> grid(20);
|
|
GridFactory<MyNode1239> 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 turnSinceLastTransition_rad = 0; // keep the angle as-is!
|
|
} ctrl;
|
|
|
|
GridWalker<MyNode1239, MyState23452> walker;
|
|
WalkModuleHeadingControl<MyNode1239, MyState23452, Control> modHead(&ctrl, 3.0f);
|
|
|
|
walker.addModule(&modHead);
|
|
|
|
|
|
MyState23452 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.heading.direction.getRAD() << "'\n";
|
|
|
|
lines.add(K::GnuplotPoint2(state.position.x_cm, state.position.y_cm));
|
|
|
|
gp.draw(plot);
|
|
gp.flush();
|
|
usleep(1000*50);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
TEST(GgridWalk2, LIVE_walkHeading) {
|
|
|
|
Grid<MyNode1239> 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<MyNode1239, MyState23452> walker;
|
|
WalkModuleHeading<MyNode1239, MyState23452> wmHead;
|
|
WalkModuleFollowDestination<MyNode1239, MyState23452> 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.getColor().setHexStr("#888888"); splot.add(&nodes);
|
|
K::GnuplotSplotElementPoints states; states.setPointSize(0.5); states.getColor().setHexStr("#0000ff"); splot.add(&states);
|
|
|
|
for (const MyNode1239& 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) {
|
|
MyState23452 start(points[i], Heading(0.0));
|
|
MyState23452 next = walker.getDestination(grid, start, 2.0);
|
|
points[i] = next.position;
|
|
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
|