FrankE
382a046df1
added new sanity checks fixed minor errors added corresponding test-cases added moving-median
162 lines
4.1 KiB
C++
162 lines
4.1 KiB
C++
#ifndef GRIDWALKPATHCONTROL_H
|
|
#define GRIDWALKPATHCONTROL_H
|
|
|
|
#include "../../geo/Heading.h"
|
|
#include "../Grid.h"
|
|
|
|
#include "../../math/Distributions.h"
|
|
#include "../../math/DrawList.h"
|
|
|
|
#include "../../nav/dijkstra/Dijkstra.h"
|
|
|
|
#include "GridWalkState.h"
|
|
#include "GridWalkHelper.h"
|
|
#include "GridWalk.h"
|
|
|
|
template <typename T> class GridWalkPathControl : public GridWalk<T> {
|
|
|
|
friend class GridWalkHelper;
|
|
|
|
private:
|
|
|
|
/** per-edge: change heading with this sigma */
|
|
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
|
|
|
|
/** fast random-number-generator */
|
|
std::minstd_rand gen;
|
|
|
|
/** 0-mean normal distribution */
|
|
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
|
|
|
|
Dijkstra<T> dijkstra;
|
|
|
|
DrawList<T&> drawer;
|
|
|
|
public:
|
|
|
|
|
|
/** ctor with the target you want to reach */
|
|
template <typename Access> GridWalkPathControl(Grid<T>& grid, const Access& acc, const T& target) {
|
|
|
|
gen.seed(1234);
|
|
|
|
// build all shortest path to reach th target
|
|
dijkstra.build(target, target, acc);
|
|
|
|
// attach a corresponding weight-information to each user-grid-node
|
|
for (T& node : grid) {
|
|
|
|
const DijkstraNode<T>* dn = dijkstra.getNode(node);
|
|
|
|
// should never be null as all nodes were evaluated
|
|
if (dn != nullptr) {
|
|
node.distToTarget = dn->cumWeight/2000;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
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.01);
|
|
|
|
// 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?
|
|
headChange_rad = headChange_rad*dHead.draw();
|
|
|
|
|
|
return walk(grid, start, distance_m, headChange_rad);
|
|
|
|
}
|
|
|
|
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) {
|
|
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);
|
|
|
|
// calculate the difference
|
|
const float diff = possibleHead.getDiffHalfRAD(head);
|
|
|
|
// // compare this heading with the requested one
|
|
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
|
|
const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.0;
|
|
|
|
// bring it together
|
|
return angleProb * nodeProb;
|
|
|
|
}
|
|
|
|
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) {
|
|
|
|
// try-again distribution
|
|
//static Distribution::Normal<float> dHead(0, Angle::degToRad(10));
|
|
//static Distribution::Normal<float> dUpdate(0, Angle::degToRad(3));
|
|
static Distribution::Uniform<float> dChange(Angle::degToRad(0), +Angle::degToRad(359));
|
|
|
|
int retries = 5;
|
|
float walked_m = 0;
|
|
GridWalkState<T> cur = start;
|
|
|
|
// the desired heading
|
|
Heading reqHeading = start.heading + (headChange_rad);
|
|
|
|
// walk until done
|
|
while(walked_m < distance_m) {
|
|
|
|
// evaluate all neighbors
|
|
drawer.reset();
|
|
for (T& neighbor : grid.neighbors(*cur.node)) {
|
|
|
|
const double prob = getProbability(*start.node, neighbor, reqHeading);
|
|
drawer.add(neighbor, prob);
|
|
|
|
}
|
|
|
|
// too bad? start over!
|
|
if (drawer.getCumProbability() < 0.1 && (--retries) >= 0) {
|
|
walked_m = 0;
|
|
cur = start;
|
|
//WHAT THE HELL
|
|
if (retries == 0) { reqHeading = dChange.draw(); }
|
|
continue;
|
|
}
|
|
|
|
// get the neighbor
|
|
const T& neighbor = drawer.get();
|
|
|
|
// update
|
|
walked_m += neighbor.getDistanceInMeter(*cur.node);
|
|
cur.node = &neighbor;
|
|
|
|
}
|
|
|
|
cur.distanceWalked_m = NAN;
|
|
cur.headingChange_rad = NAN;
|
|
cur.heading = reqHeading;
|
|
|
|
return cur;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif // GRIDWALKPATHCONTROL_H
|