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

worked on grid-walker

Browse Source 
started adding code for synthetic sensor simulation based on given paths
This commit is contained in:
k-a-z-u
2017-10-17 17:10:23 +02:00
parent 3807c621c7
commit 72f083d32a
6 changed files with 311 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
grid/walk/v3/GridWalker3.h
View File

@@ -26,12 +26,19 @@ private:
public:
/** paremters for the walk */
struct WalkParams {
Point3 start;
float distance_m;
Heading heading = Heading(0);
};
/** result of the random walk */
struct WalkResult {
Point3 position;
Heading heading = Heading(0);
};
using Helper = GridWalk3Helper<Node>;
using Walk = typename GridWalk3Helper<Node>::Walk3;
@@ -48,7 +55,7 @@ public:
}
/** perform the walk based on the configured setup */
const Point3 getDestination(Grid<Node>& grid, const WalkParams& params) {
const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) {
//return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m);
return _drawThenCheck(grid, params);
@@ -62,6 +69,7 @@ public:
// throw "error";
// }
/** does the given grid-node contain the provided point-in-question? */
const bool contains(const Grid<Node>& grid, const Node* n, Point2 pt) {
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
const float d = gridSize_m / 2.0f;
@@ -71,7 +79,7 @@ public:
return bbox.contains(pt);
}
const Point3 _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
const WalkResult _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
const GridPoint gpStart = p3ToGp(params.start);
const Node* startNode = grid.getNodePtrFor(gpStart);
@@ -84,13 +92,15 @@ public:
const float range_m = params.distance_m + secBuffer_m;
const Nodes nodes = Helper::getAllReachableNodes(grid, startNode, range_m);
WalkResult res;
res.heading = params.heading;
res.position = params.start;
float realDist_m = params.distance_m;
Heading realHead = params.heading;// + dHead.draw();
int cnt = 0;
while(true) {
const Point2 dir = realHead.asVector();
const Point2 dir = res.heading.asVector();
const Point2 dst = params.start.xy() + (dir * realDist_m);
// is dst reachable?
@@ -101,7 +111,9 @@ public:
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
const GridPoint gp = p3ToGp(p3);
if (grid.hasNodeFor(gp)) {
return p3;
res.position = p3; // new position
res.heading; // keep as-is
return res;
} else {
std::cout << "failed: " << p3.asString() << ":" << gp.asString() << std::endl;
}
@@ -111,11 +123,16 @@ public:
// before trying again, modify distance and angle
if (1 == 0) {
realDist_m *= dDist.draw();
realHead += dHead.draw();
res.heading += dHead.draw();
}
// reached max retries?
if (++cnt > 10) {return params.start;} // did not work out....
if (++cnt > 10) {
WalkResult res;
res.position = params.start;
res.heading = params.heading;
return res;
} // did not work out....
}