265 lines
7.1 KiB
C++
265 lines
7.1 KiB
C++
#include <misc/fixc11.h>
|
|
|
|
#include <QGuiApplication>
|
|
#include <QQmlApplicationEngine>
|
|
|
|
#include <QApplication>
|
|
#include <QMainWindow>
|
|
#include <QGridLayout>
|
|
|
|
#include <Indoor/math/Interpolator.h>
|
|
#include <Indoor/grid/factory/v2/Importance.h>
|
|
|
|
#include <Indoor/nav/dijkstra/Dijkstra.h>
|
|
#include <Indoor/nav/dijkstra/DijkstraPath.h>
|
|
|
|
#include "sensors/SensorFactory.h"
|
|
#include "map/MapView.h"
|
|
|
|
#include <iostream>
|
|
|
|
#include <Indoor/floorplan/v2/FloorplanReader.h>
|
|
#include <Indoor/api/DummyAPI.h>
|
|
#include <Indoor/sensors/radio/WiFiGridNode.h>
|
|
#include <Indoor/sensors/radio/WiFiGridEstimator.h>
|
|
#include <Indoor/sensors/radio/model/WiFiModelLogDist.h>
|
|
|
|
class LeListener : public SensorListener<WiFiSensorData>, public SensorListener<AccelerometerData>, public SensorListener<StepData> {
|
|
|
|
public:
|
|
void onSensorData(const WiFiSensorData& data) override {
|
|
const std::string str = "\n" + data.asString();
|
|
qDebug(str.c_str());
|
|
}
|
|
void onSensorData(const AccelerometerData& data) override {
|
|
const std::string str = data.asString();
|
|
qDebug(str.c_str());
|
|
}
|
|
void onSensorData(const StepData& data) override {
|
|
qDebug("STEP!");
|
|
}
|
|
|
|
};
|
|
|
|
struct MyNode : public GridPoint, public GridNode, public WiFiGridNode<10> {
|
|
|
|
float imp;
|
|
MyNode() {;}
|
|
MyNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
|
|
|
|
static void staticSerialize(std::ostream& out) {
|
|
WiFiGridNode::staticSerialize(out);
|
|
}
|
|
|
|
static void staticDeserialize(std::istream& inp) {
|
|
WiFiGridNode::staticDeserialize(inp);
|
|
}
|
|
|
|
};
|
|
|
|
int main(int argc, char *argv[]) {
|
|
|
|
|
|
int sizeOfNode = sizeof(MyNode);
|
|
|
|
std::vector<Point3> points = {
|
|
Point3(1140,530,1060), Point3(1140,2100,1060), Point3(1140,2880,1060), Point3(1140,4442.21,1060), Point3(1190,4840,1060), Point3(1660,4840,890), Point3(1660,5040,890), Point3(1180,5040,720), Point3(1180,4840,720), Point3(1180,4460,720), Point3(2350,4460,720), Point3(4440,4440,720), Point3(5183.26,4280,720), Point3(5800,4280,550), Point3(6110,4280,380), Point3(7680,4280,380), Point3(7680,3860,190), Point3(7680,3400,0), Point3(7400,3400,0), Point3(7400,4030,0)
|
|
};
|
|
std::vector<Point3> path;
|
|
|
|
Interpolator<int, Point3> interpol;
|
|
int ts = 0;
|
|
Point3 last = points[0];
|
|
for (Point3 p : points) {
|
|
const float dist = p.getDistance(last);
|
|
const float diffMS = (dist / 100.0f) / 1.5f * 1000;
|
|
ts += diffMS;
|
|
interpol.add(ts, p);
|
|
last = p;
|
|
path.push_back(p / 100.0f);
|
|
}
|
|
|
|
|
|
|
|
//QGuiApplication app(argc, argv);
|
|
QApplication app(argc, argv);
|
|
|
|
// OpenGL Setup
|
|
QSurfaceFormat format;
|
|
format.setDepthBufferSize(16);
|
|
QSurfaceFormat::setDefaultFormat(format);
|
|
|
|
|
|
Floorplan::IndoorMap* im = Floorplan::Reader::readFromFile("/mnt/data/workspaces/IndoorMap/maps/SHL21.xml");
|
|
|
|
Grid<MyNode> grid(20);
|
|
GridFactory<MyNode> gf(grid);
|
|
|
|
// build
|
|
//gf.build(im);
|
|
//Importance::addImportance(grid, 0);
|
|
//std::ofstream out("/tmp/grid.dat");
|
|
//grid.write(out); out.close();
|
|
|
|
// read
|
|
std::ifstream inp("/tmp/grid.dat");
|
|
grid.reset();
|
|
grid.read(inp);
|
|
|
|
// estimate WiFi signal strengths
|
|
WiFiModelLogDist mdlLogDist(-40.0f, 2.25f);
|
|
WiFiGridEstimator::estimate(grid, mdlLogDist, im);
|
|
|
|
|
|
|
|
MapView* map = new MapView();
|
|
map->setMinimumHeight(200);
|
|
map->setMinimumWidth(200);
|
|
map->setMap(im);
|
|
//map->setPath(path);
|
|
map->show();
|
|
|
|
|
|
struct DijkstraMapper {
|
|
Grid<MyNode>& grid;
|
|
DijkstraMapper(Grid<MyNode>& grid) : grid(grid) {;}
|
|
int getNumNeighbors(const MyNode& n) const {return grid.getNumNeighbors(n);}
|
|
const MyNode* getNeighbor(const MyNode& n, const int idx) const {return &grid.getNeighbor(n, idx);}
|
|
float getWeightBetween(const MyNode& n1, const MyNode& n2) const {return n1.getDistanceInCM(n2) / n2.imp;}
|
|
|
|
};
|
|
|
|
|
|
|
|
struct LeListener : public APIListener {
|
|
|
|
Grid<MyNode>& grid;
|
|
Dijkstra<MyNode>& d;
|
|
const DijkstraNode<MyNode>* dnEnd;
|
|
MapView* map;
|
|
|
|
Point3 nextPos;
|
|
Point3 pos1;
|
|
Point3 pos2;
|
|
|
|
LeListener(Grid<MyNode>& grid, Dijkstra<MyNode>& d, const DijkstraNode<MyNode>* dnEnd, MapView* map) : grid(grid), d(d), dnEnd(dnEnd), map(map) {
|
|
|
|
|
|
std::thread t(&LeListener::update, this);
|
|
t.detach();
|
|
|
|
}
|
|
|
|
/** the currently estimated path to the target has changed */
|
|
virtual void onPathUpdate(const std::vector<Point3>& curPath) override {
|
|
|
|
}
|
|
|
|
/** the currently estimated position has changed */
|
|
virtual void onPositionUpdate(const Point3 pos) override {
|
|
|
|
nextPos = pos;
|
|
|
|
// update the path to the target
|
|
const GridPoint xxx(pos.x, pos.y, pos.z);
|
|
const DijkstraNode<MyNode>* dnStart = d.getNode( grid.getNearestNode(xxx) );
|
|
const DijkstraPath<MyNode> dp(dnStart, dnEnd);
|
|
map->setPath(dp);
|
|
|
|
}
|
|
|
|
void update() {
|
|
|
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(700));
|
|
|
|
const int transMS = 500;
|
|
const int updateMS = 75;
|
|
const float factor1 = ((float) updateMS / (float) transMS) * 0.7;
|
|
const float factor2 = factor1 * 0.4f;
|
|
const Point3 human(0, 0, 180);
|
|
|
|
while(true) {
|
|
|
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(updateMS));
|
|
|
|
// Point3 diff = (nextPos - pos1);
|
|
// if (diff.length() > 30) {diff = diff.normalized() * 30;} else {diff *= factor1;}
|
|
// pos1 += diff;
|
|
// pos2 += diff*0.5;
|
|
|
|
|
|
pos1 = pos1 * (1-factor1) + nextPos * (factor1); // fast update
|
|
pos2 = pos2 * (1-factor2) + nextPos * (factor2); // slow update
|
|
const Point3 dir = pos2 - pos1;
|
|
|
|
map->setLookAt((pos1+human) / 100.0f, (dir) / 100.0f);
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
DummyAPI api("/mnt/data/workspaces/navindoor");
|
|
api.setTarget(4);
|
|
|
|
|
|
Dijkstra<MyNode> d;
|
|
//GridPoint end(points.front().x, points.front().y, points.front().z);
|
|
GridPoint end(api.getDst().x, api.getDst().y, api.getDst().z);
|
|
d.build(&grid.getNearestNode(end), DijkstraMapper(grid));
|
|
const DijkstraNode<MyNode>* dnEnd = d.getNode(grid.getNearestNode(end));
|
|
|
|
api.addListener(new LeListener(grid, d, dnEnd, map));
|
|
api.setSpeed(2);
|
|
api.startNavigation();
|
|
|
|
// auto run = [&] () {
|
|
// //int ts = 0;
|
|
// int ts = 97000;
|
|
// while(ts < interpol.getMaxKey() && ts >= 0) {
|
|
// std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
// //ts += 50;
|
|
// ts -= 150;
|
|
// const Point3 human(0, 0, 180);
|
|
//// const Point3 pos0 = interpol.get(ts-500);
|
|
//// const Point3 pos1 = interpol.get(ts+500);
|
|
// const Point3 pos = interpol.get(ts);
|
|
//// //const Point3 dir = Point3(-50000, -50000, 50000);//pos0 - pos;
|
|
//// Point3 dir = pos1 - pos0; dir.z /= 2; // only slight down/up looking
|
|
//// map->setLookAt((pos+human) / 100.0f, (dir) / 100.0f);
|
|
|
|
// GridPoint cur(pos.x, pos.y, pos.z);
|
|
// const DijkstraNode<MyNode>* dnStart = d.getNode( grid.getNearestNode(cur) );
|
|
// DijkstraPath<MyNode> dp(dnStart, dnEnd);
|
|
// map->setPath(dp);
|
|
|
|
|
|
//// Point3 next =
|
|
//// dp.getFromStart(2).element->inCentimeter() +
|
|
//// dp.getFromStart(4).element->inCentimeter() +
|
|
//// dp.getFromStart(6).element->inCentimeter();
|
|
//// next /= 3;
|
|
//// const Point3 dir = pos - next ;
|
|
// static Point3 lastPos;
|
|
// lastPos = lastPos * 0.85 + pos * 0.15;
|
|
|
|
// const Point3 dir = lastPos - pos ;
|
|
// map->setLookAt((pos+human) / 100.0f, (dir) / 100.0f);
|
|
|
|
// }
|
|
// };
|
|
// std::thread t(run);
|
|
|
|
|
|
// QQmlApplicationEngine engine;
|
|
// engine.load(QUrl(QStringLiteral("qrc:/main.qml")));
|
|
|
|
return app.exec();
|
|
|
|
}
|