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

added missing code changes

Browse Source 
started working on refactoring of sensors
new test-cases
This commit is contained in:
FrankE
2016-03-17 17:28:41 +01:00
parent 6346231a64
commit f8d7f768f1
18 changed files with 524 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
grid/walk/GridWalkPathControl.h
View File

@@ -32,6 +32,10 @@ private:
DrawList<T&> drawer;
public:
float pOther = 0.10;
public:
@@ -65,9 +69,11 @@ public:
// proportional change of the to-be-walked distance
static Distribution::Normal<float> dWalk(1, 0.10);
distance_m = distance_m*dWalk.draw()*2; // TODO: why *2?
distance_m = distance_m*dWalk.draw()*1.4; // TODO: why *2?
headChange_rad = headChange_rad*dHead.draw();
static Distribution::Normal<float> sWalk(0, 0.15);
if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); }
return walk(grid, start, distance_m, headChange_rad);
@@ -75,12 +81,12 @@ public:
private:
double getProbability(const T& start, const T& possible, const Heading head) const {
double getProbability(const T& start, const T& prev, const T& possible, const Heading head) const {
// TODO: WHY?! not only when going back to the start?
if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {
if (start.z_cm == possible.z_cm) {return 0;} // back to the start
//throw 1;
throw 1;
return 0.5;// stair start/end TODO: fix
}
@@ -95,7 +101,8 @@ private:
// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally
// nodes own importance
const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.025;
//const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.025; // from start
const double nodeProb = (possible.distToTarget < prev.distToTarget) ? 1 : pOther; // from previous node
// bring it together
return angleProb * nodeProb;
@@ -123,7 +130,7 @@ private:
drawer.reset();
for (T& neighbor : grid.neighbors(*cur.node)) {
const double prob = getProbability(*start.node, neighbor, reqHeading);
const double prob = getProbability(*start.node, *cur.node, neighbor, reqHeading);
drawer.add(neighbor, prob);
}