FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

added new sanity checks and compile-time assertions to prevent errors

Browse Source 
fixed stair-building issue
new test-cases
added elevator support
fixed/improved some walker modules
This commit is contained in:
FrankE
2016-09-10 15:12:39 +02:00
parent 7baeecb3f9
commit 82f8828a04
26 changed files with 996 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
Assertions.h
View File

@@ -67,7 +67,6 @@ namespace Assert {
if (std::abs(v1-v2) > delta) {doThrow(err);}
}
template <typename T, typename STR> static inline void isBetween(const T v, const T min, const T max, const STR err) {
if (v < min || v > max) {
std::stringstream ss; ss << "\n[" << min << ":" << max << "] but is " << v << "\n";
@@ -77,4 +76,24 @@ namespace Assert {
}
namespace StaticAssert {
// yes and no have a different size so sizeof(yes) != sizeof(no)
using yes = uint32_t;
using no = uint64_t;
template<class BaseClass, class SubClass> struct isXbaseOfY {
static yes test(const BaseClass&); // will be chosen if X is base of Y
static no test(...); // will be chosen otherwise
static const SubClass& helper();
static const bool value = sizeof(test(helper())) == sizeof(yes); // true if test(const B&) was chosen
};
template <typename A, typename B> static inline void AinheritsB() {
static_assert(isXbaseOfY<B, A>::value, "A does not inherit from B!");
}
}
#endif // ASSERTIONS_H

29
data/RingBuffer.h
View File

@@ -21,14 +21,18 @@ public:
;
}
/** add a new element */
void add(const Element& element) {
/** add a new element WITH overflow check! */
void addSafe(const Element& element) {
Assert::isTrue(available != (int)data.size(), "buffer overflow");
add(element);
}
/** add a new element WIUTHOUT checking for overflows */
void add(const Element& element) {
data[iWrite] = element;
++iWrite;
iWrite %= data.size();
++available;
//if (available > (int)data.size()) {available = data.size();}
}
/** get the next element */
@@ -42,6 +46,25 @@ public:
return tmp;
}
/** peek into the given element without removing it */
const Element& peek(const int idx) const {
const Element& tmp = data[(iRead + idx) % data.size()];
return tmp;
}
/** peek into the given element without removing it */
const Element& operator [] (const int idx) const {
return peek(idx);
}
/** does the buffer contain the given element? */
bool contains(const Element& e) const {
for (int i = 0; i < available; ++i) {
if (peek(i) == e) {return true;}
}
return false;
}
/** reset the ringbuffer */
void reset() {
iWrite = 0;

123
floorplan/v2/Floorplan.h
View File

@@ -116,6 +116,7 @@ namespace Floorplan {
struct UnderlayImage;
struct POI;
struct Stair;
struct Elevator;
using FloorOutline = std::vector<FloorOutlinePolygon*>;
using FloorObstacles = std::vector<FloorObstacle*>;
@@ -125,6 +126,7 @@ namespace Floorplan {
using FloorUnderlays = std::vector<UnderlayImage*>;
using FloorPOIs = std::vector<POI*>;
using FloorStairs = std::vector<Stair*>;
using FloorElevators = std::vector<Elevator*>;
/** describes one floor within the map, starting at a given height */
struct Floor {
@@ -140,6 +142,7 @@ namespace Floorplan {
FloorUnderlays underlays; // underlay images (used for map-building)
FloorPOIs pois; // POIs within the floor
FloorStairs stairs; // all stairs within one floor
FloorElevators elevators; // all elevators within one floor
//FloorKeyValue other; // other, free elements
Floor() {;}
@@ -249,8 +252,6 @@ namespace Floorplan {
FloorRegion(const std::string& name, const Polygon2& poly) : name(name), poly(poly) {;}
};
static Point3 xy0(const Point2 p) {
return Point3(p.x, p.y, 0);
}
@@ -305,7 +306,36 @@ namespace Floorplan {
};
static std::vector<Quad3> getQuads(const std::vector<StairPart>& parts, const Floor* floor) {
/** snap quad-edges together if their distance is below the given maximum. is used to close minor gaps */
static void snapQuads(std::vector<Floorplan::Quad3>& quads, const float maxDist) {
for (Floorplan::Quad3& quad1 : quads) {
for (int i1 = 0; i1 < 4; ++i1) {
Point3& p1 = (Point3&) quad1[i1];
for (const Floorplan::Quad3& quad2 : quads) {
if (&quad1 == &quad2) {continue;}
for (int i1 = 0; i1 < 4; ++i1) {
Point3& p2 = (Point3&) quad2[i1];
if (p1.getDistance(p2) < maxDist) {
const Point3 p3 = (p1+p2) / 2;
p1 = p3;
p2 = p3;
break;
}
}
}
}
}
}
/** convert stair-parts to quads. the scaling factor may be used to slightly grow each quad. e.g. needed to ensure that the quads overlap */
static std::vector<Quad3> getQuads(const std::vector<StairPart>& parts, const Floor* floor, const float s = 1.0f) {
std::vector<Quad3> vec;
@@ -318,10 +348,10 @@ namespace Floorplan {
const Point2 dir2(dir.x, dir.y); // direction without height (just 2D)
const Point2 perp = dir2.perpendicular(); // perendicular vector
const Point2 perpN = perp / perp.length(); // normalized perpendicular vector
const Point3 p1 = start + xy0(perpN * width / 2) + Point3(0,0,floor->atHeight);
const Point3 p2 = start - xy0(perpN * width / 2) + Point3(0,0,floor->atHeight);
const Point3 p3 = end - xy0(perpN * width / 2) + Point3(0,0,floor->atHeight);
const Point3 p4 = end + xy0(perpN * width / 2) + Point3(0,0,floor->atHeight);
const Point3 p1 = start + xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight);
const Point3 p2 = start - xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight);
const Point3 p3 = end - xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight);
const Point3 p4 = end + xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight);
const Quad3 q(p1,p2,p3,p4);
vec.push_back(q);
@@ -337,17 +367,12 @@ namespace Floorplan {
}
snapQuads(vec, 0.05);
return vec;
}
// /**
// * get the ABSOLUTE quad for this stair-part
// * (relative-height + floor-height = absolute-height
// */
// Quad3 getQuad(const Floor* floor) const {
// }
/** base-class for stairs */
struct Stair {
@@ -355,43 +380,6 @@ namespace Floorplan {
virtual std::vector<StairPart> getParts() const = 0;
};
// /** just a normal, straigt stair */
// struct StairNormal : public Stair {
// Point2 center;
// float angleDeg;
// float atHeight;
// float height;
// float width;
// float length;
// StairNormal(const Point2 center, const float atHeight, const float angleDeg, const float height, const float width, const float length) :
// center(center), angleDeg(angleDeg), atHeight(atHeight), height(height), width(width), length(length) {
// }
// std::vector<StairPart> getParts() const {
// std::vector<StairPart> parts;
// Point3 cen(center.x, center.y, atHeight);
// parts.push_back(StairPart(cen, angleDeg, length, width, height));
// return parts;
// }
// };
// /** OLD 3-part stair, up[left]->platform->up[right] */
// struct StairFreeformOLD : public Stair {
// float width;
// std::vector<Point3> nodes;
// StairFreeformOLD() {;}
// std::vector<StairPart> getParts() const {
// std::vector<StairPart> parts;
// for (int i = 1; i < (int)nodes.size(); ++i) {
// const Point3 p1 = nodes[i-1];
// const Point3 p2 = nodes[i ];
// parts.push_back(StairPart(p1, p2, width));
// }
// return parts;
// }
// };
/** 3-part stair, up[left]->platform->up[right] */
struct StairFreeform : public Stair {
std::vector<StairPart> parts;
@@ -399,7 +387,42 @@ namespace Floorplan {
std::vector<StairPart> getParts() const {return parts;}
};
/**
* describes an elevator between two floors
* so: the height depends on the height of the floor!
*/
struct Elevator {
/** the elevator's center */
Point2 center;
/** the elevator's width (in meter) */
float width;
/** the elevator's depth (in meter) */
float depth;
/** the elevator's rotation (in radians) */
float rotation;
/** get the 4 corner points for the elevator */
Polygon2 getPoints() const {
const Point2 p1 = Point2(+width/2, +depth/2).rotated(rotation) + center;
const Point2 p2 = Point2(-width/2, +depth/2).rotated(rotation) + center;
const Point2 p3 = Point2(-width/2, -depth/2).rotated(rotation) + center;
const Point2 p4 = Point2(+width/2, -depth/2).rotated(rotation) + center;
Polygon2 poly;
poly.points = {p1, p2, p3, p4};
return poly;
}
/** empty ctor */
Elevator() : center(), width(2), depth(2), rotation(0) {;}
/** ctor */
Elevator(const Point2 center) : center(center), width(2), depth(2), rotation(0) {;}
};

20
floorplan/v2/FloorplanReader.h
View File

@@ -88,11 +88,21 @@ namespace Floorplan {
if (std::string("underlays") == n->Name()) {floor->underlays = parseFloorUnderlays(n);}
if (std::string("pois") == n->Name()) {floor->pois = parseFloorPOIs(n);}
if (std::string("stairs") == n->Name()) {floor->stairs = parseFloorStairs(n);}
if (std::string("elevators") == n->Name()) {floor->elevators = parseFloorElevators(n);}
}
return floor;
}
/** parse the <elevators> tag */
static std::vector<Elevator*> parseFloorElevators(const XMLElem* el) {
std::vector<Elevator*> vec;
FOREACH_NODE(n, el) {
if (std::string("elevator") == n->Name()) { vec.push_back(parseFloorElevator(n)); }
}
return vec;
}
/** parse the <stairs> tag */
static std::vector<Stair*> parseFloorStairs(const XMLElem* el) {
std::vector<Stair*> vec;
@@ -102,6 +112,16 @@ namespace Floorplan {
return vec;
}
/** parse an <elevator> tag */
static Elevator* parseFloorElevator(const XMLElem* el) {
Elevator* elev = new Elevator();
elev->center = Point2(el->FloatAttribute("cx"), el->FloatAttribute("cy"));
elev->depth = el->FloatAttribute("depth");
elev->width = el->FloatAttribute("width");
elev->rotation = el->FloatAttribute("rotation");
return elev;
}
/** parse a <stair> tag */
static Stair* parseFloorStair(const XMLElem* el) {
Stair* stair = nullptr;

18
floorplan/v2/FloorplanWriter.h
View File

@@ -78,6 +78,24 @@ namespace Floorplan {
addFloorPOI(doc, floor, mf);
addStairs(doc, floor, mf);
addElevators(doc, floor, mf);
}
/** add all elevators to the floor */
static void addElevators(XMLDoc& doc, XMLElem* floor, const Floor* mf) {
XMLElem* elevators = doc.NewElement("elevators");
for (const Elevator* elevator : mf->elevators) {
XMLElem* elem = doc.NewElement("elevator");
elem->SetAttribute("cx", elevator->center.x);
elem->SetAttribute("cy", elevator->center.y);
elem->SetAttribute("width", elevator->width);
elem->SetAttribute("depth", elevator->depth);
elem->SetAttribute("rotation", elevator->rotation);
elevators->InsertEndChild(elem);
}
floor->InsertEndChild(elevators);
}

2
grid/Grid.h
View File

@@ -108,9 +108,9 @@ public:
void connectUniDir(T& n1, const T& n2) {
Assert::isFalse(n1.hasNeighbor(n2._idx), "this neighbor is already connected"); // already connected?
Assert::notEqual(n1.getIdx(), n2.getIdx(), "can not connect node with itself");
Assert::isFalse(n1.fullyConnected(), "this node has already reached its neighbor-limit");
n1._neighbors[n1._numNeighbors] = n2._idx;
++n1._numNeighbors;
Assert::isBetween(n1._numNeighbors, (uint8_t) 0, (uint8_t) 10, "number of neighbors out of bounds!");
}
/**

8
grid/GridNode.h
View File

@@ -18,6 +18,10 @@ template<typename> class Grid;
*/
struct GridNode {
private:
static int constexpr MAX_NEIGHBORS = 10;
private:
/** grant full access to the grid */
@@ -27,7 +31,7 @@ private:
int _idx;
/** INTERNAL: store neighbors (via index) */
int _neighbors[10];
int _neighbors[MAX_NEIGHBORS];
/** INTERNAL: number of neighbors */
uint8_t _numNeighbors;
@@ -61,7 +65,7 @@ public:
int getNumNeighbors() const {return _numNeighbors;}
/** reached neighbor limit? */
bool fullyConnected() const {return _numNeighbors >= 10;}
bool fullyConnected() const {return _numNeighbors >= MAX_NEIGHBORS;}
/** is this node connected to the given index? */
bool hasNeighbor(const int idx) const {

2
grid/GridPoint.h
View File

@@ -37,6 +37,8 @@ struct GridPoint {
GridPoint operator * (const float f) const {return GridPoint(x_cm*f, y_cm*f, z_cm*f);}
GridPoint operator + (const GridPoint& o) const {return GridPoint(x_cm+o.x_cm, y_cm+o.y_cm, z_cm+o.z_cm);}
GridPoint& operator += (const GridPoint& o) {x_cm += o.x_cm; y_cm += o.y_cm; z_cm += o.z_cm; return *this;}
GridPoint& operator /= (const float f) {x_cm /= f; y_cm /= f; z_cm /= f; return *this;}

99
grid/factory/v2/Elevators.h Normal file
View File

@@ -0,0 +1,99 @@
#ifndef GRID_ELEVATORS_H
#define GRID_ELEVATORS_H
#include <vector>
#include <set>
#include "../../Grid.h"
#include "Helper.h"
#include "../../../floorplan/v2/Floorplan.h"
#include <fstream>
template <typename T> class Elevators {
private:
/** the grid to build into */
Grid<T>& grid;
/** calculation helper */
Helper<T> helper;
public:
/** ctor */
Elevators(Grid<T>& grid) : grid(grid), helper(grid) {
;
}
~Elevators() {
;
}
/** build the given elevator */
void build(const Floorplan::Floor* floor, const Floorplan::Elevator* elevator) {
const int gs_cm = grid.getGridSize_cm();
struct IntPos {
int x_cm;
int y_cm;
IntPos(int x_cm, int y_cm) : x_cm(x_cm), y_cm(y_cm) {;}
};
// identify all grid-aligned nodes that belong to the elevator
std::vector<IntPos> nodesWithin;
const HelperPoly poly(elevator->getPoints());
auto callback = [&] (const int x_cm, const int y_cm) {
const GridPoint gp1(x_cm, y_cm, floor->getStartingZ()*100); // starting floor
const GridPoint gp2(x_cm, y_cm, floor->getEndingZ()*100); // the floor above
// ensure such a node is present in both floors (and thus a connection is possible)
if (grid.hasNodeFor(gp1) && grid.hasNodeFor(gp2)) {
nodesWithin.push_back(IntPos(x_cm, y_cm));
}
};
poly.forEachGridPoint(gs_cm, callback);
// now create the interconnection in z-direction
const int z1_cm = std::ceil((floor->getStartingZ()*100+1) / gs_cm) * gs_cm; // the next node above the current flor
const int z2_cm = std::floor((floor->getEndingZ()*100-1) / gs_cm) * gs_cm; // the last node below the next floor
for (const IntPos nodePos : nodesWithin) {
// create nodes BETWEEN the two floors (skip the floors themselves! -> floor1+gridSize <-> floor2-gridSize
for (int z_cm = z1_cm; z_cm <= z2_cm; z_cm += gs_cm) {
const GridPoint gp1(nodePos.x_cm, nodePos.y_cm, z_cm); // the to-be-added node
Assert::isFalse(grid.hasNodeFor(gp1), "elevator collission"); // such a node must not yet exist! otherwise we e.g. collide with a stari
const int idx = grid.add(T(gp1.x_cm, gp1.y_cm, gp1.z_cm)); // create the node
grid[idx].setType(GridNode::TYPE_ELEVATOR); // set the node-type
}
// connect each of the new nodes with the node below it. NOW ALSO EXAMINE THE floor above (z2_cm + gs_cm)
for (int z_cm = z1_cm; z_cm <= z2_cm + gs_cm; z_cm += gs_cm) {
const GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm);
const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm);
Assert::isTrue(grid.hasNodeFor(gpBelow), "missing node");
Assert::isTrue(grid.hasNodeFor(gp), "missing node");
T& n1 = (T&) grid.getNodeFor(gpBelow);
T& n2 = (T&) grid.getNodeFor(gp);
grid.connectBiDir(n1, n2);
}
}
}
};
#endif // GRID_ELEVATORS_H

102
grid/factory/v2/GridFactory.h
View File

@@ -8,7 +8,8 @@
#include "../../../floorplan/v2/Floorplan.h"
#include "Helper.h"
#include "Stairs.h"
#include "Stairs2.h"
#include "Elevators.h"
#include "../../../geo/Units.h"
#include "../../GridNodeBBox.h"
@@ -37,6 +38,9 @@ private:
/** stair builder */
Stairs<T> stairs;
/** elevator builder */
Elevators<T> elevators;
bool _buildStairs = true;
bool _removeIsolated = true;
@@ -44,7 +48,9 @@ private:
public:
/** ctor with the grid to fill */
GridFactory(Grid<T>& grid) : grid(grid), helper(grid), stairs(grid) {;}
explicit GridFactory(Grid<T>& grid) : grid(grid), helper(grid), stairs(grid), elevators(grid) {
}
/** whether or not to build stairs */
@@ -59,7 +65,7 @@ public:
Log::add(name, "building grid from IndoorMap", true);
const int total = map->floors.size()*2 + 1;
const int total = map->floors.size()*3 + 1;
int cur = 0;
// build all the floors
@@ -78,6 +84,15 @@ public:
}
}
// build all elevators
if (listener) {listener->onGridBuildUpdateMajor("adding elevators");}
if (_buildStairs) {
for (Floorplan::Floor* f : map->floors) {
buildElevators(f, listener);
if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
}
}
// remove isolated nodes
if (_removeIsolated) {
if (listener) {listener->onGridBuildUpdateMajor("removing isolated nodes");}
@@ -180,78 +195,23 @@ public:
}
void buildElevators(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
const int total = floor->elevators.size();
int cur = 0;
// process each elevator within the floor
for (const Floorplan::Elevator* elevator : floor->elevators) {
if (listener) {listener->onGridBuildUpdateMinor("adding " + floor->name + " elevator " + std::to_string(cur+1));}
elevators.build(floor, elevator);
if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
}
}
// void addWithin(const Floorplan::Floor* floor, std::vector<Entry>& nodeList) {
// const int fz1_cm = floor->atHeight*100;
// const int fz2_cm = (floor->atHeight+floor->height)*100;
// for (Entry& e : nodeList) {
// if ( (e.pos.z_cm <= fz1_cm) || (e.pos.z_cm >= fz2_cm) ) {
// e.idx = grid.getNearestNode(e.pos).getIdx();
// e.part = {9999};
// } else {
// const T t(e.pos.x_cm, e.pos.y_cm, e.pos.z_cm);
// e.idx = grid.addUnaligned(t, false);
// }
// }
// }
// void filter(std::vector<Entry>& nodeList) {
// const int gs_cm = grid.getGridSize_cm();
// const int limit_cm = gs_cm * 0.50;
// // remove duplicate nodes or nodes, that are too close to each other
// for(auto it = nodeList.begin(); it != nodeList.end(); ) {
// // currently examined node
// const Entry& e1 = *it;
// // matches for nodes that are NOT the same instance AND nearby (< grid-size)
// auto matches = [&] (const Entry& e2) { return (e1.pos != e2.pos) && (e1.pos.getDistanceInCM(e2.pos) <= limit_cm); };
// auto match = std::find_if(nodeList.begin(), nodeList.end(), matches);
// // remove if this node has a nearby neighbor
// if (match != nodeList.end()) {
// // combine both nodes within one:
// // the node belongs to more than one stair-part
// Entry& e2 = *match;
// e2.part.insert(e2.part.end(), e1.part.begin(), e1.part.end());
// it = nodeList.erase(it);
// } else {
// ++it;
// }
// }
// }
// std::vector<Entry> minOnly(const std::vector<Entry>& lst) {
// auto comp = [&] (const Entry& a, const Entry& b) {return grid[a.idx].z_cm < grid[b.idx].z_cm;};
// auto it = std::min_element(lst.begin(), lst.end(), comp);
// T& min = grid[it->idx];
// std::vector<Entry> res;
// for (const Entry& e : lst) {
// if (grid[e.idx].z_cm == min.z_cm) {res.push_back(e);}
// }
// return res;
// }
/** connect all neighboring nodes part of the given index-vector */
void connectAdjacent(const std::vector<int>& indices) {

59
grid/factory/v2/Helper.h
View File

@@ -26,7 +26,12 @@ struct HelperPoly {
/** ctor from floorplan-quad */
HelperPoly(const Floorplan::Quad3& quad) {
add(quad.p1); add(quad.p2); add(quad.p3); add(quad.p4);
add(quad.p1*100); add(quad.p2*100); add(quad.p3*100); add(quad.p4*100);
}
/** ctor from floorplan-polygon */
HelperPoly(const Floorplan::Polygon2& poly) {
for (Point2 p : poly.points) { add(p * 100); }
}
void add(const Point2 p) {
@@ -67,6 +72,30 @@ struct HelperPoly {
}
/** call a user-function for each GRID-ALIGNED point within the polygon */
void forEachGridPoint(const int gridSize_cm, std::function<void(int x_cm, int y_cm)> callback) const {
int x1 = std::floor(bbox_cm.getMin().x / gridSize_cm) * gridSize_cm;
int x2 = std::ceil(bbox_cm.getMax().x / gridSize_cm) * gridSize_cm;
int y1 = std::floor(bbox_cm.getMin().y / gridSize_cm) * gridSize_cm;
int y2 = std::ceil(bbox_cm.getMax().y / gridSize_cm) * gridSize_cm;
// process each point within the (aligned) bbox
for (int y = y1; y <= y2; y += gridSize_cm) {
for (int x = x1; x <= x2; x += gridSize_cm) {
// does this point belong to the polygon?
if (!contains(Point2(x,y))) {continue;}
// call the callback
callback(x,y);
}
}
}
};
@@ -94,7 +123,7 @@ public:
}
/** connect the given node to all its neighbors */
/** connect the given node to all its neighbors (x,y) */
void connectToNeighbors(T& n1) {
const int gs_cm = grid.getGridSize_cm();
@@ -120,6 +149,32 @@ public:
}
/** connect the given node to all its neighbors )x,y,z) */
void connectToNeighborsXYZ(T& n1) {
const int gs_cm = grid.getGridSize_cm();
for (int z = -gs_cm; z <= +gs_cm; z += gs_cm) {
for (int y = -gs_cm; y <= +gs_cm; y += gs_cm) {
for (int x = -gs_cm; x <= +gs_cm; x += gs_cm) {
// skip the node itself
if (x == 0 && y == 0 && z == 0) {continue;}
// try to find a matching neighbor
const GridPoint gp(n1.x_cm + x, n1.y_cm + y, n1.z_cm + z);
const T* n2 = grid.getNodePtrFor(gp);
if (!n2) {continue;}
// connect
if (n1.hasNeighbor(n2->getIdx())) {continue;}
grid.connectUniDir(n1, *n2);
}
}
}
}
int gridSize() const {
return grid.getGridSize_cm();;

5
grid/factory/v2/Importance.h
View File

@@ -72,6 +72,7 @@ public:
switch(n1.getType()) {
case GridNode::TYPE_DOOR: doors.push_back(n1); break;
case GridNode::TYPE_STAIR: stairs.push_back(n1); break;
case GridNode::TYPE_ELEVATOR: stairs.push_back(n1); break;
}
}
@@ -87,7 +88,7 @@ public:
// probability adjustments
Distribution::Normal<float> avoidWalls(0.0, 0.35);
Distribution::Normal<float> favorDoors(0.0f, 0.5f);
Distribution::Normal<float> favorStairs(0.0f, 3.5f);
Distribution::Normal<float> favorStairs(0.0f, 1.5f);
if (l) {
l->onGridBuildUpdateMajor(2, 1);
@@ -124,7 +125,7 @@ public:
// final probability
n1.navImportance = 1.0f;
n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 2.5f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 30.5f;
// use wall avoidance
if (useNormal) {

207
grid/factory/v2/Stairs2.h Normal file
View File

@@ -0,0 +1,207 @@
#ifndef STAIRS_H
#define STAIRS_H
#include <vector>
#include <set>
#include "../../Grid.h"
#include "Helper.h"
#include "../../../floorplan/v2/Floorplan.h"
#include <fstream>
#include <unordered_set>
/**
* this one should be both, simpler and more robus than v1.
* - prevents duplicate (x,y) nodes
* - slightly grows the stair-quads to ensure they overlap (sometimes the do not by a few mm)
* - only connects the corener nodes between adjacent quads (seems better for most stair-situations)
*
*/
template <typename T> class Stairs {
private:
/** the grid to build into */
Grid<T>& grid;
/** calculation helper */
Helper<T> helper;
// keep a list of all vertices below stairwells and remove them hereafter
std::vector<T*> toDelete;
private:
/** helper struct */
struct StairNode {
const int x_cm;
const int y_cm;
const int belongsToQuadIdx;
int gridIdx = -1;
StairNode(const int x_cm, const int y_cm, const int quadIdx) : x_cm(x_cm), y_cm(y_cm), belongsToQuadIdx(quadIdx) {;}
};
public:
/** ctor */
Stairs(Grid<T>& grid) : grid(grid), helper(grid) {
}
~Stairs() {
finalize();
}
void build(const Floorplan::Floor* floor, const Floorplan::Stair* stair) {
const int gs_cm = grid.getGridSize_cm();
// get all of the parts for the stair
const std::vector<Floorplan::StairPart> parts = stair->getParts();
// convert each part to a quad. hereby, slightly grow each quad, to ensure stair-parts are connected without gaps!
const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(parts, floor, 1.05);
std::vector<StairNode> stairNodes;
// process each quad to get a list of all stair-nodes to add
for (int i = 0; i < (int) quads.size(); ++i) {
// create a 2D polygon (ignore z) for the quad
const Floorplan::Quad3& quad = quads[i];
HelperPoly poly(quad);
// get a callback for each node that belongs to the quad
poly.forEachGridPoint(gs_cm, [&] (int x_cm, int y_cm) {
const StairNode sn(x_cm, y_cm, i);
// IMPORTANT
// skip nodes that belong to more than one quad -> skip duplicates -> much more stable!
// NOTE: currently this will kill circular stairs that are above themselves
// FIX: skip dupliactes only between adjacent quads? this should help
auto comp = [&] (const StairNode& sn1) { return sn1.x_cm == sn.x_cm && sn1.y_cm == sn.y_cm; };
auto it = std::find_if(stairNodes.begin(), stairNodes.end(), comp);
if (it == stairNodes.end()) {stairNodes.push_back(sn);}
});
}
// add the new nodes to the grid
// nodes that are near to the two floors that stair is within, are replaced by already existing floor-nodes!
for (StairNode& sn : stairNodes) {
// use the nodes (x,y) to reconstruct the z-value for this position using barycentric interpolation
const Point2 p(sn.x_cm, sn.y_cm);
// the stair-quads centimeter position
const Floorplan::Quad3& quad = quads[sn.belongsToQuadIdx];
const Point3 p1 = quad.p1 * 100;
const Point3 p2 = quad.p2 * 100;
const Point3 p3 = quad.p3 * 100;
const Point3 p4 = quad.p4 * 100;
// get the z-value from one of the both triangles
int z_cm;
float u,v,w;
if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) {
z_cm = p1.z*u + p2.z*v + p3.z*w;
} else {
helper.bary(p, p1.xy(), p3.xy(), p4.xy(), u, v, w);
z_cm = p1.z*u + p3.z*v + p4.z*w;
}
// the to-be-added position
GridPoint gp(sn.x_cm, sn.y_cm, z_cm);
// if a node is near an existing one (the floor above/below) use the existing one!
// this ensures the stair is connected to the floor above and below
if (grid.hasNodeFor(gp)) {
sn.gridIdx = grid.getNodeFor(gp).getIdx();
} else {
sn.gridIdx = grid.add(T(gp.x_cm, gp.y_cm, gp.z_cm));
// check if there is a nearby floor-node to delete
const int deleteDist_cm = 100;
const float distToBelow = gp.z_cm - floor->getStartingZ()*100;
const float distToAbove = floor->getEndingZ()*100 - gp.z_cm;
if (distToBelow > gs_cm && distToBelow < deleteDist_cm) {
T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getStartingZ()*100));
if (n) {toDelete.push_back(n);}
} else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) {
T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getEndingZ()*100));
if (n) {toDelete.push_back(n);}
}
}
}
// now connect all new nodes with their neighbors
// do not perform normal grid-connection but examine the nodes within the generated vector
// this allows for some additional checks/criteria to be used
for (const StairNode& sn1 : stairNodes) {
T& n1 = (T&) grid[sn1.gridIdx];
for (const StairNode& sn2 : stairNodes) {
// node full?
if (n1.fullyConnected()) {continue;}
T& n2 = (T&) grid[sn2.gridIdx];
// do not connect node with itself
if (n2.getIdx() == n1.getIdx()) {continue;}
if (n1.hasNeighbor(n2.getIdx())) {continue;}
// connect adjacent stair-nodes
// but only if their stair-parts are also adjacent
// this addresses several error-situations with round stairs (end connected to the start) , ...
const int dx = sn1.x_cm - sn2.x_cm;
const int dy = sn1.y_cm - sn2.y_cm;
const int dz = sn1.belongsToQuadIdx - sn2.belongsToQuadIdx;
// connect
if (std::abs(dx) <= gs_cm && std::abs(dy) <= gs_cm && std::abs(dz) <= 1) {
grid.connectUniDir(n1, n2);
}
}
}
}
void finalize() {
// delete all pending nodes and perform a cleanup
if (!toDelete.empty()) {
for (T* n : toDelete) {grid.remove(*n);}
toDelete.clear();
grid.cleanup();
}
}
};
#endif // STAIRS_H

75
grid/walk/v2/modules/WalkModuleFavorZ.h
View File

@@ -7,15 +7,46 @@
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../Assertions.h"
/** state-parameter needed for WalkModuleFavorZ */
struct WalkStateFavorZ {
/** nested struct to prevent name clashes */
struct {
/**
* 0 = up / down / stay is legal
* > 0 force states to walk upwards
* < 0 force states to walk downwards
*
* shifted towards 0 after every taken edge
* so: we force states to walk into the same z-direction for some time
*/
int zTendence = 0;
} favorZ;
};
/** favor z-transitions */
template <typename Node, typename WalkState> class WalkModuleFavorZ : public WalkModule<Node, WalkState> {
private:
// force states to walk into the same z-direction for 30 edges
const int keepForXEdges = 12;
public:
/** ctor */
WalkModuleFavorZ() {
;
// ensure the template WalkState inherits from 'WalkStateFavorZ'
StaticAssert::AinheritsB<WalkState, WalkStateFavorZ>();
}
virtual void updateBefore(WalkState& state) override {
@@ -30,9 +61,27 @@ public:
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) state;
(void) curNode;
(void) nextNode;
// currently no walk-tendence configured
if (state.favorZ.zTendence == 0) {
// does the taken edge indicate a z-change?
const int diff = nextNode.z_cm - curNode.z_cm;
// if so, keep this z-direction for the next few edges to come!
if (diff != 0) {
state.favorZ.zTendence = (diff > 0) ? (+keepForXEdges) : (-keepForXEdges);
}
// currently there IS a walk-tendence configured
} else {
// update the tendence (shift towards 0)
if (state.favorZ.zTendence < 0) {++state.favorZ.zTendence;}
else if (state.favorZ.zTendence > 0) {--state.favorZ.zTendence;}
}
}
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
@@ -40,11 +89,19 @@ public:
(void) state;
(void) startNode;
if (curNode.z_cm != potentialNode.z_cm) {
return 40;
} else {
return 1;
}
const int tendence = state.favorZ.zTendence;
const int diff = potentialNode.z_cm - curNode.z_cm;
// tendence available + tendence match? -> high score!
if (tendence > 0 && diff > 0) {return 0.95;}
if (tendence < 0 && diff < 0) {return 0.95;}
// tendence available + tendence mismatch? -> very low score!
if (tendence > 0 && diff < 0) {return 0.05;}
if (tendence < 0 && diff > 0) {return 0.05;}
// no tendence available -> just favor z-transitions over non-z-transitions
return (diff != 0) ? (0.7) : (0.3);
}

47
grid/walk/v2/modules/WalkModuleHeading.h
View File

@@ -4,10 +4,52 @@
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../Assertions.h"
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../geo/Heading.h"
/**
* base-class e.g. needed for GridWalkHeading and GridWalkHeadingControl to work
*/
struct WalkStateHeading {
/** used for better naming: heading.error instead of headingError */
struct _Heading {
/**
* the direction [0:2pi] the walk should move to
* e.g. indiciated by:
* compass
* integration over gyroscope values
*/
Heading direction;
/**
* (cumulative) error between walked edges and requested direction (above).
* is used to ensure that (even though the grid contains only 45° edges) we
* approximately walk into the requested direction.
*/
float error = 0;
/** ctor */
_Heading(const Heading direction, const float error) : direction(direction), error(error) {;}
} heading;
/** ctor */
explicit WalkStateHeading(const Heading& direction, const float error) : heading(direction, error) {;}
};
/** keep the state's heading */
template <typename Node, typename WalkState> class WalkModuleHeading : public WalkModule<Node, WalkState> {
@@ -23,7 +65,10 @@ public:
/** ctor */
WalkModuleHeading() : dist(Distribution::VonMises<double>(0.0f, 1.0f).getLUT()), draw(dist.getDrawList()) {
;
// ensure the template WalkState inherits from 'WalkStateHeading'!
StaticAssert::AinheritsB<WalkState, WalkStateHeading>();
}
virtual void updateBefore(WalkState& state) override {

7
grid/walk/v2/modules/WalkModuleHeadingControl.h
View File

@@ -58,6 +58,9 @@ public:
(void) state;
// for elevator edges [same (x,y) but different z] do not adjust anything
if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}
// get the heading denoted by the way from curNode to nextNode
const Heading head(curNode.x_cm, curNode.y_cm, nextNode.x_cm, nextNode.y_cm);
@@ -80,6 +83,10 @@ public:
(void) startNode;
// for elevator edges [same (x,y) but different z] just return 1
if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
// get the heading between curNode and potentialNode
const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);

2
grid/walk/v2/modules/WalkModuleNodeImportance.h
View File

@@ -4,6 +4,7 @@
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../Assertions.h"
/**
* favor edges based on the importance-factor of the next node.
@@ -44,7 +45,6 @@ public:
(void) curNode;
const double prob = potentialNode.getNavImportance();
//return std::pow(prob, 10);
return prob;
}

77
grid/walk/v2/modules/WalkModulePreventVisited.h Normal file
View File

@@ -0,0 +1,77 @@
#ifndef WALKMODULEPREVENTVISITED_H
#define WALKMODULEPREVENTVISITED_H
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../data/RingBuffer.h"
#include "../../../../Assertions.h"
struct WalkStatePreventVisited {
struct PV {
RingBuffer<int> history;
PV(const int size) : history(size) {;}
} preventVisited;
/** ctor */
explicit WalkStatePreventVisited(const int historySize) : preventVisited(historySize) {;}
};
/**
* prevent a state from visiting nodes he has already visited
* within a certain timeframe (ringbuffer)
*
* this should avoid deadlocks in some situations where the transition
* just switched back and forth between two nodes
*
*/
template <typename Node, typename WalkState> class WalkModulePreventVisited : public WalkModule<Node, WalkState> {
private:
public:
/** ctor */
WalkModulePreventVisited() {
// ensure the templated WalkState inherits from 'WalkStatePreventVisited'
StaticAssert::AinheritsB<WalkState, WalkStatePreventVisited>();
}
virtual void updateBefore(WalkState& state) override {
(void) state;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
(void) state;
(void) startNode;
(void) endNode;
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) curNode;
state.preventVisited.history.add(nextNode.getIdx());
}
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) startNode;
(void) curNode;
return (state.preventVisited.history.contains(potentialNode.getIdx())) ? 0.001 : 0.999;
}
};
#endif // WALKMODULEPREVENTVISITED_H

51
grid/walk/v2/modules/WalkModuleSpread.h
View File

@@ -4,6 +4,21 @@
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../Assertions.h"
/** state-parameter needed for WalkModuleSpread */
struct WalkStateSpread {
/** nested struct to prevent name-clashes */
struct {
/** keep something like a moving-average-position we want to strictly depart from */
GridPoint departFrom;
} spread;
};
/**
* simply try to move away from the starting node as much as possible
@@ -12,19 +27,26 @@ template <typename Node, typename WalkState> class WalkModuleSpread : public Wal
private:
Point3 avg;
/**
* how fast to adjust the average-position to depart from
* values between 3% and 10% seem fine
*/
const float kappa = 0.10;
public:
/** ctor */
WalkModuleSpread() {
;
/** ensure the templated WalkState inherits from WalkStateSpread */
StaticAssert::AinheritsB<WalkState, WalkStateSpread>();
}
virtual void updateBefore(WalkState& state) override {
(void) state;
avg = avg * 0.999 + state.position.inMeter() * 0.001;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -34,24 +56,27 @@ public:
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) state;
(void) curNode;
(void) nextNode;
state.spread.departFrom = state.spread.departFrom * (1.0f-kappa) + nextNode * (kappa);
}
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) state;
(void) startNode;
(void) curNode;
const float dOld = avg.getDistance(curNode.inMeter());
const float dNew = avg.getDistance(potentialNode.inMeter());
// current distance from the depart-from position
const float dOld = state.spread.departFrom.getDistanceInCM(curNode);
if (dNew > dOld) {return 0.8;}
if (curNode.z_cm != potentialNode.z_cm) {return 0.8;}
if (dNew == dOld) {return 0.2;}
return 0;
// potential distance from the depart-from position
const float dNew = state.spread.departFrom.getDistanceInCM(potentialNode);
// now, favor edges that depart even further from the depart-from position!
if (dNew > dOld) {return 0.90;} // departing
if (dNew == dOld) {return 0.09;} // distance does not change
{return 0.01;} // NOT departing.. unlikely
}

36
grid/walk/v2/modules/WalkStateHeading.h
View File

@@ -1,43 +1,7 @@
#ifndef WALKSTATEHEADING_H
#define WALKSTATEHEADING_H
#include "../../../../geo/Heading.h"
/**
* base-class e.g. needed for GridWalkHeading and GridWalkHeadingControl to work
*/
struct WalkStateHeading {
/** used for better naming: heading.error instead of headingError */
struct _Heading {
/**
* the direction [0:2pi] the walk should move to
* e.g. indiciated by:
* compass
* integration over gyroscope values
*/
Heading direction;
/**
* (cumulative) error between walked edges and requested direction (above).
* is used to ensure that (even though the grid contains only 45° edges) we
* approximately walk into the requested direction.
*/
float error = 0;
/** ctor */
_Heading(const Heading direction, const float error) : direction(direction), error(error) {;}
} heading;
/** ctor */
explicit WalkStateHeading(const Heading& direction, const float error) : heading(direction, error) {;}
};
#endif // WALKSTATEHEADING_H

5
main.cpp
View File

@@ -17,9 +17,10 @@ int main(int argc, char** argv) {
::testing::InitGoogleTest(&argc, argv);
// skip all tests starting with LIVE_
::testing::GTEST_FLAG(filter) = "*Barometer*";
//::testing::GTEST_FLAG(filter) = "*Barometer*";
::testing::GTEST_FLAG(filter) = "*Activity*";
::testing::GTEST_FLAG(filter) = "*Stairs*";
//::testing::GTEST_FLAG(filter) = "*RingBuffer*";
//::testing::GTEST_FLAG(filter) = "*Grid.*";
//::testing::GTEST_FLAG(filter) = "*Dijkstra.*";

4
sensors/imu/StepDetection.h
View File

@@ -35,8 +35,8 @@ private:
bool waitForUp = false;
const Timestamp blockTime = Timestamp::fromMS(150); // 150-250 looks good
const float upperThreshold = +0.4f; // + is usually smaller than down (look at graphs)
const float lowerThreshold = -0.8f;
const float upperThreshold = +0.4*0.6f; // + is usually smaller than down (look at graphs)
const float lowerThreshold = -0.8*0.6f; // the 0.8 is for testing!
public:

3
tests/Tests.h
View File

@@ -6,7 +6,8 @@
#include <gtest/gtest.h>
static inline std::string getDataFile(const std::string& name) {
return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name;
return "/mnt/data/workspaces/Indoor/tests/data/" + name;
//return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name;
}

91
tests/data/TestRingBuffer.cpp
View File

@@ -60,6 +60,97 @@ TEST(RingBuffer, add) {
}
TEST(RingBuffer, peek) {
RingBuffer<int> buf(4);
buf.add(11);
ASSERT_EQ(11, buf.peek(0));
buf.add(13);
ASSERT_EQ(11, buf.peek(0));
ASSERT_EQ(13, buf.peek(1));
buf.add(10);
ASSERT_EQ(11, buf.peek(0));
ASSERT_EQ(13, buf.peek(1));
ASSERT_EQ(10, buf.peek(2));
buf.add(17);
ASSERT_EQ(11, buf.peek(0));
ASSERT_EQ(13, buf.peek(1));
ASSERT_EQ(10, buf.peek(2));
ASSERT_EQ(17, buf.peek(3));
buf.add(18);
ASSERT_EQ(18, buf.peek(0));
ASSERT_EQ(13, buf.peek(1));
ASSERT_EQ(10, buf.peek(2));
ASSERT_EQ(17, buf.peek(3));
buf.add(9);
ASSERT_EQ(18, buf.peek(0));
ASSERT_EQ( 9, buf.peek(1));
ASSERT_EQ(10, buf.peek(2));
ASSERT_EQ(17, buf.peek(3));
buf.add(88);
ASSERT_EQ(18, buf.peek(0));
ASSERT_EQ( 9, buf.peek(1));
ASSERT_EQ(88, buf.peek(2));
ASSERT_EQ(17, buf.peek(3));
buf.add(54);
ASSERT_EQ(18, buf.peek(0));
ASSERT_EQ( 9, buf.peek(1));
ASSERT_EQ(88, buf.peek(2));
ASSERT_EQ(54, buf.peek(3));
buf.add(1);
ASSERT_EQ( 1, buf.peek(0));
ASSERT_EQ( 9, buf.peek(1));
ASSERT_EQ(88, buf.peek(2));
ASSERT_EQ(54, buf.peek(3));
}
TEST(RingBuffer, contains) {
RingBuffer<int> buf(4);
ASSERT_FALSE(buf.contains(0));
buf.add(0);
ASSERT_TRUE(buf.contains(0));
ASSERT_FALSE(buf.contains(3));
buf.add(3);
ASSERT_TRUE(buf.contains(0));
ASSERT_TRUE(buf.contains(3));
ASSERT_FALSE(buf.contains(7));
buf.add(7);
ASSERT_TRUE(buf.contains(0));
ASSERT_TRUE(buf.contains(3));
ASSERT_TRUE(buf.contains(7));
ASSERT_FALSE(buf.contains(8));
buf.add(8);
ASSERT_TRUE(buf.contains(0));
ASSERT_TRUE(buf.contains(3));
ASSERT_TRUE(buf.contains(7));
ASSERT_TRUE(buf.contains(8));
ASSERT_FALSE(buf.contains(11));
buf.add(11);
ASSERT_FALSE(buf.contains(0));
ASSERT_TRUE(buf.contains(3));
ASSERT_TRUE(buf.contains(7));
ASSERT_TRUE(buf.contains(8));
ASSERT_TRUE(buf.contains(11));
}
TEST(RingBuffer, iterator) {
RingBuffer<int> buf(4);

99
tests/grid/TestStairs.cpp Normal file
View File

@@ -0,0 +1,99 @@
#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 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.setColorHex("#888888"); pnodes.setPointType(7);
K::GnuplotSplotElementPoints pstates; splot.add(&pstates); pstates.setColorHex("#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

2
tests/sensors/pressure/TestBarometer.cpp
View File

@@ -127,7 +127,7 @@ TEST(Barometer, Activity) {
//read file
std::string line;
std::string filename = getDataFile("baro/logfile_UAH_R2_S3_baro.dat");
std::string filename = getDataFile("barometer/baro1.dat");
std::ifstream infile(filename);
while (std::getline(infile, line))