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

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;
}