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

geometry changes/fixes/new features

Browse Source 
new grid walkers + fixes
new test-cases
worked on step/and turn detection
android offline-data-reader
worked on vap-grouping
This commit is contained in:
FrankE
2016-09-07 10:16:51 +02:00
parent a203305628
commit d283d9b326
27 changed files with 976 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -13,11 +13,11 @@ template <typename Node, typename WalkState, typename Control> class WalkModuleH
private:
/** van-Mises distribution */
/** CURRENTLY NOT USED van-Mises distribution */
Distribution::LUT<double> dist;
/** van-Mises draw list */
DrawList<double> draw;
/** random noise */
Distribution::Normal<float> distNoise;
Control* ctrl;
@@ -26,16 +26,25 @@ private:
public:
/** ctor 3.0 should be OK! */
WalkModuleHeadingControl(Control* ctrl) : dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()), draw(dist.getDrawList()), ctrl(ctrl) {
WalkModuleHeadingControl(Control* ctrl, const float sensorNoiseDegreesSigma) :
dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()),
distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
ctrl(ctrl) {
;
}
virtual void updateBefore(WalkState& state) override {
const float var = draw.get() * 0.20;//0.05;
//const float var = 0;
state.startHeading += ctrl->turnAngle + var;
// 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?");
// sensor noise
const float var = distNoise.draw();
// adjust the state's heading using the control-data
state.heading.direction += ctrl->turnAngle + var;
}
@@ -46,9 +55,24 @@ public:
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) state;
(void) curNode;
(void) nextNode;
// 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);
// get the heading requested by the state
const Heading stateHead = state.heading.direction;
// get the error (signed difference) between both
const float angularDiff = stateHead.getSignedDiff(head);
// adjust the error.
// note: the error may get > +/- 2PI but this is not an issue!
// when the error is added to the current heading within getProbability(),
// it is ensured their sum is within [0:2pi]
state.heading.error += angularDiff;
}
@@ -60,11 +84,18 @@ public:
const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
// compare the heading against the state's heading - the last error
const Heading stateHead = state.startHeading;
const Heading stateHead = state.heading.direction + state.heading.error;
// 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;