openGL work and other parts
This commit is contained in:
225
main.cpp
225
main.cpp
@@ -7,11 +7,23 @@
|
||||
#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:
|
||||
@@ -29,23 +41,43 @@ public:
|
||||
|
||||
};
|
||||
|
||||
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[]) {
|
||||
//// test();
|
||||
// LeListener listener;
|
||||
// WiFiSensor& wifi = SensorFactory::getWiFi();
|
||||
// wifi.addListener(&listener);
|
||||
// wifi.start();
|
||||
|
||||
//// AccelerometerSensor& acc = SensorFactory::getAccelerometer();
|
||||
//// acc.addListener(&listener);
|
||||
//// acc.start();
|
||||
|
||||
// StepSensor& steps = SensorFactory::getSteps();
|
||||
// steps.addListener(&listener);
|
||||
// steps.start();;
|
||||
int sizeOfNode = sizeof(MyNode);
|
||||
|
||||
// std::this_thread::sleep_for(std::chrono::seconds(10000));
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -54,22 +86,175 @@ int main(int argc, char *argv[]) {
|
||||
|
||||
// OpenGL Setup
|
||||
QSurfaceFormat format;
|
||||
format.setDepthBufferSize(24);
|
||||
format.setDepthBufferSize(16);
|
||||
QSurfaceFormat::setDefaultFormat(format);
|
||||
|
||||
// QMainWindow* win = new QMainWindow();
|
||||
|
||||
// QVBoxLayout* lay = new QVBoxLayout();
|
||||
// win->setLayout(lay);
|
||||
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();
|
||||
// lay->addWidget(map);
|
||||
map->setMinimumHeight(200);
|
||||
map->setMinimumWidth(200);
|
||||
map->setMap(im);
|
||||
//map->setPath(path);
|
||||
map->show();
|
||||
// win->setMinimumWidth(400);
|
||||
// win->setMinimumHeight(400);
|
||||
// win->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")));
|
||||
|
||||
Reference in New Issue
Block a user