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

some fixes [multithreading,..]

Browse Source 
needed interface changes [new options]
logger for android
wifi-ap-optimization
new test-cases
This commit is contained in:
FrankE
2016-09-28 12:19:14 +02:00
parent 91e3367372
commit 4f511d907e
62 changed files with 1418 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
grid/Grid.h
View File

@@ -6,6 +6,8 @@
#include <unordered_map>
#include <algorithm>
#include "../Assertions.h"
#include "../Exception.h"
#include "GridPoint.h"
#include "GridNode.h"
@@ -47,8 +49,11 @@ public:
/** ctor with the grid's size (in cm) */
Grid(const int gridSize_cm) : gridSize_cm(gridSize_cm) {
static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
//static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
//static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
StaticAssert::AinheritsB<T, GridNode>(); // "T must inherit from GridNode!"
StaticAssert::AinheritsB<T, GridPoint>(); // "T must inherit from GridPoint!"
Log::add(name, "empty grid with " + std::to_string(gridSize_cm) + "cm grid-size");
}
/** no-copy */
@@ -395,11 +400,13 @@ public:
/** serialize into the given stream */
void write(std::ostream& out) {
// serialize static
T::staticSerialize(out);
// size (in bytes) one node has. this is a sanity check whether the file matches the code!
const int nodeSize = sizeof(T);
out.write((const char*) &nodeSize, sizeof(nodeSize));
// number of nodes
const int numNodes = nodes.size();
Assert::isTrue(numNodes > 0, "grid says it contains 0 nodes. there must be some error!");
out.write((const char*) &numNodes, sizeof(numNodes));
// serialize
@@ -407,23 +414,37 @@ public:
out.write((const char*) &node, sizeof(T));
}
// serialize static parameters
T::staticSerialize(out);
out.flush();
}
/** deserialize from the given stream */
void read(std::istream& inp) {
// deserialize static
T::staticDeserialize(inp);
Log::add(name, "loading grid from input-stream");
// size (in bytes) one node has. this is a sanity check whether the file matches the code!
int nodeSize;
inp.read((char*) &nodeSize, sizeof(nodeSize));
Assert::equal(nodeSize, (int)sizeof(T), "sizeof(node) of the saved grid does not match sizeof(node) for the code!");
// number of nodes
int numNodes;
inp.read((char*) &numNodes, sizeof(numNodes));
Assert::isTrue(numNodes > 0, "grid-file says it contains 0 nodes. there must be some error!");
// allocate node-space
nodes.resize(numNodes);
// deserialize
inp.read((char*) nodes.data(), numNodes*sizeof(T));
Log::add(name, "deserialized " + std::to_string(nodes.size()) + " nodes");
// deserialize static parameters
T::staticDeserialize(inp);
// update
rebuildHashes();

19
grid/GridPoint.h
View File

@@ -67,7 +67,12 @@ struct GridPoint {
/** cast to string */
operator std::string() const {return "(" + std::to_string(x_cm) + "," + std::to_string(y_cm) + "," + std::to_string(z_cm) + ")";}
operator std::string() const {return asString();}
/** get as string */
std::string asString() const {
return "(" + std::to_string(x_cm) + "," + std::to_string(y_cm) + "," + std::to_string(z_cm) + ")";
}
/** read-only array access */
float operator [] (const int idx) const {
@@ -79,4 +84,16 @@ struct GridPoint {
};
namespace std {
template<> struct hash<GridPoint> {
int64_t operator() (const GridPoint& gp) const {
return
(((int64_t)gp.x_cm) << 0) +
(((int64_t)gp.y_cm) << 8) +
(((int64_t)gp.z_cm) << 16);
}
};
}
#endif // GRIDPOINT_H

2
grid/factory/GridImportance.h
View File

@@ -130,7 +130,7 @@ public:
}
template <typename T> void addImportance(Grid<T>& g, DijkstraNode<T>* start, DijkstraNode<T>* end) {
template <typename T> void addImportance(Grid<T>& g, const DijkstraNode<T>* start, const DijkstraNode<T>* end) {
// routing path
DijkstraPath<T> path(end, start);

1
grid/factory/v2/Elevators.h
View File

@@ -40,7 +40,6 @@ public:
const int gs_cm = grid.getGridSize_cm();
struct IntPos {
int x_cm;
int y_cm;

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

@@ -136,7 +136,7 @@ public:
/** add the given floor to the grid */
void addFloor(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
Log::add(name, "adding floor " + floor->name, true);
Log::add(name, "adding floor '" + floor->name + "'", true);
if (listener) {listener->onGridBuildUpdateMinor("adding floor " + floor->name);}
const BBox2 bbox = getFloorOutlineBBox(floor->outline);
@@ -148,6 +148,7 @@ public:
const int total = (x2-x1) / helper.gridSize();
int cur = 0;
int numNodes = 0;
// build grid-points for floor-outline
for(int x_cm = x1; x_cm < x2; x_cm += helper.gridSize()) {
@@ -169,10 +170,17 @@ public:
if (grid.hasNodeFor(t)) {continue;}
grid.add(t);
// debug
++numNodes;
}
if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
}
Log::add(name, "added " + std::to_string(numNodes) + " nodes");
// connect the g
connectAdjacent(floor, z_cm);
@@ -225,9 +233,9 @@ public:
}
/** connect all neighboring nodes located on the given height-plane */
void connectAdjacent(const Floorplan::Floor* floor, const float z_cm) {
void connectAdjacent(const Floorplan::Floor* floor, const int z_cm) {
Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm), false);
Log::add(name, "connecting all adjacent nodes within floor '" + floor->name + "' (atHeight: " + std::to_string(z_cm) + "cm)", false);
Log::tick();
// connect adjacent grid-points

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

@@ -86,8 +86,8 @@ public:
KNN<KNNArray<std::vector<T>>, 3> knnStairs(knnArrStairs);
// probability adjustments
Distribution::Normal<float> avoidWalls(0.0, 0.35);
Distribution::Normal<float> favorDoors(0.0f, 0.5f);
Distribution::Triangle<float> avoidWalls(0.0, 0.45f);
Distribution::Normal<float> favorDoors(0.0f, 0.4f);
Distribution::Normal<float> favorStairs(0.0f, 1.5f);
if (l) {
@@ -120,20 +120,33 @@ public:
// get the distance to the nearest stair
const float distToStair_m = Units::cmToM(knnStairs.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
const bool useNormal = (distToWall_m < distToDoor_m && distToWall_m < distToStair_m);
const bool useNormal = (distToWall_m*3.5 < distToDoor_m && distToWall_m*3.5 < distToStair_m);
// final probability
n1.navImportance = 1.0f;
n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 30.5f;
n1.walkImportance = 1.0f;
//n1.walkImportance += favorDoors.getProbability(distToDoor_m) * 0.75f;
n1.walkImportance += favorStairs.getProbability(distToStair_m) * 1.0f;
// use wall avoidance
if (useNormal) {
n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.5f;
n1.walkImportance -= avoidWalls.getProbability(distToWall_m) * 0.4f;
}
// navigation importance is calculated using other formulae
n1.navImportance = 1.0f;
if (useNormal) {
n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.4;
}
//n1.navImportance += favorDoors.getProbability(distToDoor_m) * 0.5;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 1.0;
// sanity check
Assert::isTrue(n1.navImportance >= 0, "detected negative importance. does not make sense!");
Assert::isTrue(n1.walkImportance >= 0, "detected negative walk importance. does not make sense!");
Assert::isTrue(n1.navImportance >= 0, "detected negative nav importance. does not make sense!");
}

4
grid/factory/v2/Stairs2.h
View File

@@ -112,7 +112,7 @@ public:
const Point3 p4 = quad.p4 * 100;
// get the z-value from one of the both triangles
int z_cm;
// int z_cm;
float u,v,w;
if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) {
sn.z_cm = p1.z*u + p2.z*v + p3.z*w;
@@ -124,7 +124,7 @@ public:
}
// this might lead to stairs the start slightly above the starting-floor
// or end slightly below the ending floor. this would lead to DISCONNECTION!
// or ending slightly below the ending floor. this would lead to DISCONNECTION!
// therefore re-scale the z-values to ensure they start at floor1 and end at floor 2
float minZ = +9999999;
float maxZ = -9999999;

46
grid/walk/v2/GridWalker.h
View File

@@ -1,6 +1,7 @@
#ifndef GRIDWALKER_H
#define GRIDWALKER_H
#include "../../../data/Timestamp.h"
#include "../../Grid.h"
#include "../../../math/DrawList.h"
#include "modules/WalkModule.h"
@@ -16,7 +17,7 @@ private:
/** all modules to evaluate */
std::vector<WalkModule<Node, WalkState>*> modules;
DrawList<const Node*> drawer;
RandomGenerator rnd;
public:
@@ -26,14 +27,16 @@ public:
}
/** perform the walk based on the configured setup */
WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, float dist_m) {
WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, float dist_m, double& probability) {
Assert::isTrue(dist_m >= 0, "walk distance must not be negative!");
Assert::isTrue(dist_m < 10.0, "walking more than 10.0 meters at once does not make sense!");
// keep the starting state for reference
//const WalkState startState = _startState;
// the current state that is modified for each step
WalkState currentState = _startState;
updateBefore(currentState);
// get the node that corresponds to start;
const Node* startNode = grid.getNodePtrFor(currentState.position);
@@ -42,10 +45,26 @@ public:
// currently examined node
const Node* curNode = startNode;
// perform initial update
updateBefore(currentState, *curNode);
// add the previous walked-distance-error to the desired distance (is usually negative, thus dist_m is reduced)
dist_m += _startState.distance.error_m;
probability = 1.0;
int cnt = 0;
// until distance is reached
while (dist_m > 0) {
drawer.reset();
// only needed for a global (no-thread-safe) drawer
//drawer.reset();
// the rnd-generator is shared among several threads which should be fine
// the draw-list, however, is per-thread, which is mandatory!
// alternative to the generator would be seeding the interal one which prooved very unstable!
DrawList<const Node*> drawer(rnd); drawer.reserve(10);
// evaluate each neighbor
for (const Node& neighbor : grid.neighbors(*curNode)) {
@@ -53,8 +72,12 @@ public:
drawer.add(&neighbor, prob);
}
// pick a neighbor
const Node* nextNode = drawer.get();
// pick a neighbor and get its probability
double nodeProbability;
const Node* nextNode = drawer.get(nodeProbability);
if ( nodeProbability < probability ) {probability = nodeProbability;} // keep the smallest one
//probability += nodeProbability;
//++cnt;
// inform
step(currentState, *curNode, *nextNode);
@@ -64,12 +87,17 @@ public:
curNode = nextNode;
currentState.position = *curNode;
}
//if (cnt != 0) {probability /= cnt;} else {probability = 1.0;}
probability = curNode->getWalkImportance();
// update after
updateAfter(currentState, *startNode, *curNode);
// calculate the walked-distance-error (is usually negative, as we walked a little too far)
currentState.distance.error_m = dist_m;
// done
return currentState;
@@ -78,10 +106,10 @@ public:
private:
/** update the state before starting the random walk (e.g. based on sensor readings, ..) */
inline void updateBefore(WalkState& state) {
inline void updateBefore(WalkState& state, const Node& startNode) {
for (WalkModule<Node, WalkState>* mdl : modules) {
mdl->updateBefore(state);
mdl->updateBefore(state, startNode);
}
}

18
grid/walk/v2/modules/WalkModule.h
View File

@@ -9,6 +9,22 @@ struct WalkState {
/** current position within the grid (-> in cm!) */
GridPoint position;
/** nested struct to prevent name clashes */
struct Distance {
/**
* for every walk, the walker is given a desired distance
* however, the walking distance depends on the grid size and can
* therefore never be reached exactly. therefor we track the
* error between desired and walked distance to ensure "in average"
* the walked distance is correct
*/
float error_m;
Distance() : error_m(0) {;}
} distance;
/** ctor */
explicit WalkState(const GridPoint& position) : position(position) {;}
@@ -23,7 +39,7 @@ template <typename Node, typename WalkState> class WalkModule {
public:
/** update the given WalkState before starting the walk. e.g. based on sensor readings */
virtual void updateBefore(WalkState& state) = 0;
virtual void updateBefore(WalkState& state, const Node& startNode) = 0;
/** get the probability p(e) from curNode to potentialNode */
virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const = 0;

111
grid/walk/v2/modules/WalkModuleActivityControl.h Normal file
View File

@@ -0,0 +1,111 @@
#ifndef WALKMODULEACTIVITYCONTROL_H
#define WALKMODULEACTIVITYCONTROL_H
#include "WalkModule.h"
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../sensors/pressure/ActivityButterPressure.h"
/**
* use the currently detected activity (stay-on-floor, walk-up, walk-down, ..)
* from the system's control data to favor edges that resemble this activity
*/
template <typename Node, typename WalkState, typename Control> class WalkModuleActivityControl : public WalkModule<Node, WalkState> {
private:
const Control* ctrl;
public:
/** ctor */
WalkModuleActivityControl(const Control* ctrl) : ctrl(ctrl) {
;
}
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
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) state;
(void) curNode;
(void) nextNode;
}
// static double getProbability(const Node& lastNode, const Node& nextNode, const ActivityButterPressure::Activity activity) {
// switch (activity) {
// case ActivityButterPressure::Activity::DOWN:
// if (deltaZ_cm < 0) {return 0.85;}
// if (deltaZ_cm == 0) {return 0.10;}
// {return 0.05;}
// case ActivityButterPressure::Activity::UP:
// if (deltaZ_cm > 0) {return 0.85;}
// if (deltaZ_cm == 0) {return 0.10;}
// {return 0.05;}
// case ActivityButterPressure::Activity::STAY:
// if (deltaZ_cm == 0) {return 0.85;}
// {return 0.15;}
// default:
// throw Exception("not yet implemented");
// }
// return 1.0;
// }
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) state;
(void) startNode;
const int deltaZ_cm = potentialNode.z_cm - curNode.z_cm;
// TODO: general activity enum and activity-detector based on barometer and accelerometer?
const ActivityButterPressure::Activity activity = ctrl->activity;
// const float kappa = 0.75;
switch (activity) {
case ActivityButterPressure::Activity::DOWN:
if (deltaZ_cm < 0) {return 0.60;}
if (deltaZ_cm == 0) {return 0.25;}
{return 0.15;}
case ActivityButterPressure::Activity::UP:
if (deltaZ_cm > 0) {return 0.60;}
if (deltaZ_cm == 0) {return 0.25;}
{return 0.15;}
case ActivityButterPressure::Activity::STAY:
if (deltaZ_cm == 0) {return 0.60;}
{return 0.40;}
// case ActivityButterPressure::Activity::DOWN:
// case ActivityButterPressure::Activity::UP:
// if (potentialNode.getType() == GridNode::TYPE_STAIR) {return kappa;}
// if (potentialNode.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
// {return 1-kappa;}
// case ActivityButterPressure::Activity::STAY:
// if (potentialNode.getType() == GridNode::TYPE_DOOR) {return kappa;}
// if (potentialNode.getType() == GridNode::TYPE_FLOOR) {return kappa;}
// {return 1-kappa;}
default:
throw Exception("not yet implemented");
}
}
};
#endif // WALKMODULEACTIVITYCONTROL_H

32
grid/walk/v2/modules/WalkModuleButterActivity.h
View File

@@ -2,12 +2,30 @@
#define WALKMODULEBUTTERACTIVITY_H
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../sensors/pressure/ActivityButterPressure.h"
DEPREACTED
SEE WalkModuleActivityControl
struct WalkStateBarometerActivity {
/** innser-struct to prevent name-clashes */
struct Barometer {
/** activity currently detected from the baromter */
ActivityButterPressure::Activity activity;
Barometer() : activity(ActivityButterPressure::Activity::STAY) {;}
} barometer;
/** ctor */
WalkStateBarometerActivity() : barometer() {;}
};
/** favor z-transitions */
template <typename Node, typename WalkState> class WalkModuleButterActivity : public WalkModule<Node, WalkState> {
@@ -16,11 +34,15 @@ public:
/** ctor */
WalkModuleButterActivity() {
;
// ensure templates WalkState inherits from 'WalkStateBarometerActivity'
StaticAssert::AinheritsB<WalkState, WalkStateBarometerActivity>();
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -43,11 +65,11 @@ public:
const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
if(state.act == ActivityButterPressure::Activity::DOWN){
if(state.barometer.activity == ActivityButterPressure::Activity::DOWN){
if (deltaZ_cm < 0) {return 0;}
if (deltaZ_cm == 0) {return 0.1;}
return 0.9;
} else if (state.act == ActivityButterPressure::Activity::UP){
} else if (state.barometer.activity == ActivityButterPressure::Activity::UP){
if (deltaZ_cm > 0) {return 0;}
if (deltaZ_cm == 0) {return 0.1;}
return 0.9;

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

@@ -36,7 +36,7 @@ template <typename Node, typename WalkState> class WalkModuleFavorZ : public Wal
private:
// force states to walk into the same z-direction for 30 edges
const int keepForXEdges = 12;
const int keepForXEdges = 8;
public:
@@ -49,8 +49,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -93,15 +94,15 @@ public:
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;}
if (tendence > 0 && diff >= 0) {return 0.90;}
if (tendence < 0 && diff <= 0) {return 0.90;}
// tendence available + tendence mismatch? -> very low score!
if (tendence > 0 && diff < 0) {return 0.05;}
if (tendence < 0 && diff > 0) {return 0.05;}
if (tendence > 0 && diff < 0) {return 0.10;}
if (tendence < 0 && diff > 0) {return 0.10;}
// no tendence available -> just favor z-transitions over non-z-transitions
return (diff != 0) ? (0.7) : (0.3);
return (diff != 0) ? (0.75) : (0.25);
}

78
grid/walk/v2/modules/WalkModuleFollowDestination.h
View File

@@ -2,39 +2,77 @@
#define WALKMODULEFOLLOWDESTINATION_H
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../nav/dijkstra/Dijkstra.h"
#include "../../../../nav/dijkstra/DijkstraPath.h"
#include "../../../../math/Distributions.h"
#include "../../../../Assertions.h"
/**
* favour edges p(e) that approach the destination
* favor nodes that approach a known destination
*/
template <typename Node, typename WalkState> class WalkModuleFollowDestination : public WalkModule<Node, WalkState> {
private:
const Grid<Node>& grid;
Dijkstra<Node> dijkstra;
const DijkstraNode<Node>* dnDest;
struct DijkstraMapper {
struct DijkstraAccess {
const Grid<Node>& grid;
DijkstraMapper(const Grid<Node>& grid) : grid(grid) {;}
DijkstraAccess(const Grid<Node>& grid) : grid(grid) {;}
int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
const Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
float getWeightBetween(const Node& n1, const Node& n2) const {
return n1.getDistanceInCM(n2) * n2.navImportance;
return n1.getDistanceInMeter(n2) / n2.getNavImportance();
}
};
public:
/** ctor */
WalkModuleFollowDestination(Grid<Node>& grid, const Node& destination) {
/** ctor WITHOUT known destination*/
WalkModuleFollowDestination(const Grid<Node>& grid) : grid(grid) {
// shortest path calculation
dijkstra.build(&destination, DijkstraMapper(grid));
// ensure the template WalkState inherits from 'WalkStateFavorZ'
//StaticAssert::AinheritsB<WalkState, WalkStateFavorZ>();
}
virtual void updateBefore(WalkState& state) override {
/** ctor WITH known destination*/
WalkModuleFollowDestination(const Grid<Node>& grid, const Node& destination) : grid(grid) {
setDestination(destination);
}
/** set the desired destination node */
void setDestination(const Node& dest) {
DijkstraAccess acc(grid);
dijkstra.build(&dest, acc);
dnDest = dijkstra.getNode(dest);
}
/** get the shortest path from the given start to the configured destination */
DijkstraPath<Node> getShortestPath(const Node& start) {
const DijkstraNode<Node>* dnStart = dijkstra.getNode(start);
const DijkstraPath<Node> path(dnStart, dnDest);
return path;
}
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
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 {
@@ -43,26 +81,28 @@ public:
(void) nextNode;
}
virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) state;
(void) startNode;
const float kappa = 0.8;
const DijkstraNode<Node>* dnCur = dijkstra.getNode(curNode);
const DijkstraNode<Node>* dnNext = dijkstra.getNode(potentialNode);
const DijkstraNode<Node>* dnPot = dijkstra.getNode(potentialNode);
// probability
return (dnNext->cumWeight < dnCur->cumWeight) ? (kappa) : (1.0 - kappa);
if (dnCur == nullptr) {return 1.0;}
if (dnPot == nullptr) {return 1.0;}
constexpr double kappa = 0.70;
const float curDistToTarget = dnCur->cumWeight;
const float potDistToTarget = dnPot->cumWeight;
return (potDistToTarget < curDistToTarget) ? (kappa) : (1.0-kappa);
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
(void) state;
(void) startNode;
(void) endNode;
}
};
#endif // WALKMODULEFOLLOWDESTINATION_H

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

@@ -36,7 +36,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) startNode;
// add noise
state.heading.direction += draw.get();

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

@@ -19,14 +19,12 @@ private:
/** random noise */
Distribution::Normal<float> distNoise;
Control* ctrl;
//std::unordered_map<WalkState*, float> errorTracker;
const Control* ctrl;
public:
/** ctor 3.0 should be OK! */
WalkModuleHeadingControl(Control* ctrl, const float sensorNoiseDegreesSigma) :
WalkModuleHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) :
dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()),
distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
ctrl(ctrl) {
@@ -37,17 +35,20 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
// reset this one after every transition!
Assert::isBetween(ctrl->turnAngle, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
Assert::isBetween(ctrl->turnSinceLastTransition_rad, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
// sensor noise
const float var = distNoise.draw();
float var = distNoise.draw();
// stair? -> increase variance
if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;}
// adjust the state's heading using the control-data
state.heading.direction += ctrl->turnAngle + var;
state.heading.direction += ctrl->turnSinceLastTransition_rad + var;
}
@@ -61,8 +62,13 @@ public:
(void) state;
// ignore for stairs?
//if (nextNode.getType() == GridNode::TYPE_STAIR) {return;}
// 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;}
if (nextNode.getType() == GridNode::TYPE_ELEVATOR) {return;}
if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return;}
//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);
@@ -86,9 +92,13 @@ public:
(void) startNode;
// ignore for stairs?
//if (potentialNode.getType() == GridNode::TYPE_STAIR) {return 1.0;}
// 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;}
if (potentialNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
//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);
@@ -99,16 +109,18 @@ public:
// get the difference
const float angularDiff = head.getDiffHalfRAD(stateHead);
if (angularDiff > Angle::degToRad(135)) {return 0.01;}
if (angularDiff > Angle::degToRad(90)) {return 0.02;}
if (angularDiff > Angle::degToRad(45)) {return 0.07;}
{return 0.90;}
// add error to allow stronger deviation with respect to the "BIG GLOBAL SCOPE"
// determine probability
const float prob = dist.getProbability(angularDiff);
return prob;
// const float prob = dist.getProbability(angularDiff);
// return prob;
}

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

@@ -22,8 +22,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -44,7 +45,12 @@ public:
(void) startNode;
(void) curNode;
const double prob = potentialNode.getNavImportance();
//const double prob = potentialNode.getWalkImportance();
const float i1 = curNode.getWalkImportance();
const float i2 = potentialNode.getWalkImportance();
const double prob = (i2 > i1) ? (0.9) : (0.1);
return prob;
}

3
grid/walk/v2/modules/WalkModulePreventVisited.h
View File

@@ -45,8 +45,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

3
grid/walk/v2/modules/WalkModuleRelativePressureControl.h
View File

@@ -46,8 +46,9 @@ public:
;
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

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

@@ -45,8 +45,9 @@ public:
}
virtual void updateBefore(WalkState& state) override {
virtual void updateBefore(WalkState& state, const Node& startNode) override {
(void) state;
(void) startNode;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {