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

new walker (control+path)

Browse Source 
added new sanity checks
fixed minor errors
added corresponding test-cases
added moving-median
This commit is contained in:
FrankE
2016-02-05 20:18:48 +01:00
parent 56203e59ed
commit 382a046df1
10 changed files with 296 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
grid/walk/GridWalkSimpleControl.h
View File

@@ -44,12 +44,12 @@ public:
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
// proportional change of the heading
static Distribution::Normal<float> dHead(1, 0.10);
static Distribution::Normal<float> dHead(1, 0.01);
// proportional change of the to-be-walked distance
static Distribution::Normal<float> dWalk(1, 0.10);
static Distribution::Normal<float> dWalk(1, 0.50);
distance_m = distance_m*dWalk.draw(); // TODO: why *2?
distance_m = distance_m*dWalk.draw()*2; // TODO: why *2?
headChange_rad = headChange_rad*dHead.draw();
@@ -62,7 +62,11 @@ private:
double getProbability(const T& start, 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) {return 0.1;}
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;
return 0.5;// stair start/end TODO: fix
}
// get the angle between START and the possible next node
const Heading possibleHead = GridWalkHelper::getHeading(start, possible);
@@ -71,7 +75,7 @@ private:
const float diff = possibleHead.getDiffHalfRAD(head);
// // compare this heading with the requested one
const double angleProb = Distribution::Normal<float>::getProbability(0, Angle::degToRad(30), diff);
const double angleProb = Distribution::Normal<float>::getProbability(0, Angle::degToRad(15), diff);
// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally
// nodes own importance