179 lines
4.4 KiB
C++
179 lines
4.4 KiB
C++
#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 {
|
|
float getWalkImportance() const {return 1;}
|
|
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 turnSinceLastTransition_rad = 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.turnSinceLastTransition_rad = (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
|