kazu
04d8ae8c74
- some should be the same as previous commit (sorry!) - some should be new: LINT checks, ...?
102 lines
3.0 KiB
C++
102 lines
3.0 KiB
C++
#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 walkImportance = 0;
|
|
float getWalkImportance() const {return walkImportance;}
|
|
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.getColor().setHexStr("#888888"); pnodes.setPointType(7);
|
|
K::GnuplotSplotElementPoints pstates; splot.add(&pstates); pstates.getColor().setHexStr("#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
|