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

graphical exception [temporary solution]

Browse Source 
performance fixes
minor changes
This commit is contained in:
FrankE
2016-09-29 21:03:49 +02:00
parent 4f511d907e
commit e75327090d
6 changed files with 46 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
Exception.h
View File

@@ -4,6 +4,10 @@
#include <exception> #include <exception>
#include <string> #include <string>
#ifdef ANDROID
#include <QMessageBox>
#endif
class Exception : public std::exception { class Exception : public std::exception {
private: private:
@@ -14,7 +18,14 @@ private:
public: public:
/** ctor */ /** ctor */
Exception(const std::string& str) : str(str) {;} Exception(const std::string& str) : str(str) {
// TODO better solution?
#ifdef ANDROID
QMessageBox::question(nullptr, "Exception", str.c_str(), QMessageBox::Ok);
#endif
}
const char* what() const throw() {return str.c_str();} const char* what() const throw() {return str.c_str();}

26
geo/Angle.h
View File

@@ -5,6 +5,8 @@
#include "../Assertions.h" #include "../Assertions.h"
#include "Point2.h" #include "Point2.h"
#define PI ((float) M_PI)
struct Angle { struct Angle {
@@ -14,7 +16,7 @@ public:
static float getRAD_2PI(const float x1, const float y1, const float x2, const float y2) { static float getRAD_2PI(const float x1, const float y1, const float x2, const float y2) {
Assert::isFalse( (x1==x2)&&(y1==y2), "(x1,y1) must not equal (x2,y2)!!"); Assert::isFalse( (x1==x2)&&(y1==y2), "(x1,y1) must not equal (x2,y2)!!");
const float tmp = std::atan2(y2-y1, x2-x1); const float tmp = std::atan2(y2-y1, x2-x1);
return (tmp < 0) ? (tmp + 2*M_PI) : (tmp); return (tmp < 0) ? (tmp + 2*PI) : (tmp);
} }
/** get the radians from (0,0) to (p.x,p.y) between 0 (to-the-right) and <2_PI */ /** get the radians from (0,0) to (p.x,p.y) between 0 (to-the-right) and <2_PI */
@@ -34,10 +36,10 @@ public:
* - as a change-in-direction between [0:PI] * - as a change-in-direction between [0:PI]
*/ */
static float getDiffRAD_2PI_PI(const float r1, const float r2) { static float getDiffRAD_2PI_PI(const float r1, const float r2) {
Assert::isBetween(r1, 0.0f, (float)(2*M_PI), "r1 out of bounds"); Assert::isBetween(r1, 0.0f, (2*PI), "r1 out of bounds");
Assert::isBetween(r2, 0.0f, (float)(2*M_PI), "r2 out of bounds"); Assert::isBetween(r2, 0.0f, (2*PI), "r2 out of bounds");
float tmp = std::abs(r1-r2); float tmp = std::abs(r1-r2);
return (tmp <= M_PI) ? (tmp) : (2*M_PI-tmp); return (tmp <= PI) ? (tmp) : (2*PI-tmp);
} }
/** /**
@@ -46,23 +48,23 @@ public:
* - as a change-in-direction between [-PI:+PI] * - as a change-in-direction between [-PI:+PI]
*/ */
static float getSignedDiffRAD_2PI(const float r1, const float r2) { static float getSignedDiffRAD_2PI(const float r1, const float r2) {
Assert::isBetween(r1, 0.0f, (float)(2*M_PI), "r1 out of bounds"); // [0:360] deg Assert::isBetween(r1, 0.0f, (float)(2*PI), "r1 out of bounds"); // [0:360] deg
Assert::isBetween(r2, 0.0f, (float)(2*M_PI), "r2 out of bounds"); // [0:360] deg Assert::isBetween(r2, 0.0f, (float)(2*PI), "r2 out of bounds"); // [0:360] deg
float diff = r1-r2; float diff = r1-r2;
if (diff > +M_PI) {diff = -(2*M_PI - diff);} if (diff > +PI) {diff = -(2*PI - diff);}
else if (diff < -M_PI) {diff = +(2*M_PI + diff);} else if (diff < -PI) {diff = +(2*PI + diff);}
Assert::isBetween(diff, (float)-M_PI, (float)(+M_PI), "result out of bounds"); // [-180:+180] deg Assert::isBetween(diff, -PI, (float)(+PI), "result out of bounds"); // [-180:+180] deg
return diff; return diff;
} }
/** convert degrees to radians */ /** convert degrees to radians */
static constexpr inline float degToRad(const float deg) { static constexpr inline float degToRad(const float deg) {
return deg / 180.0f * M_PI; return deg / 180.0f * PI;
} }
/** convert radians to degrees */ /** convert radians to degrees */
static constexpr inline float radToDeg(const float rad) { static constexpr inline float radToDeg(const float rad) {
return rad * 180.0f / M_PI; return rad * 180.0f / PI;
} }
/** get a pointer vector (length 1) pointing to the given angle (in radians) */ /** get a pointer vector (length 1) pointing to the given angle (in radians) */
@@ -74,4 +76,6 @@ public:
}; };
#undef PI
#endif // ANGLE_H #endif // ANGLE_H

10
geo/Heading.h
View File

@@ -8,7 +8,7 @@
struct Heading { struct Heading {
#define _2PI (2*M_PI) #define _2PI (2*(float)M_PI)
private: private:
@@ -105,10 +105,10 @@ public:
}; };
namespace Headings { namespace Headings {
static const Heading RIGHT = Heading(M_PI*0/2); static const Heading RIGHT = Heading((float)M_PI*0.0f/2.0f);
static const Heading UP = Heading(M_PI*1/2); static const Heading UP = Heading((float)M_PI*1.0f/2.0f);
static const Heading LEFT = Heading(M_PI*2/2); static const Heading LEFT = Heading((float)M_PI*2.0f/2.0f);
static const Heading DOWN = Heading(M_PI*3/2); static const Heading DOWN = Heading((float)M_PI*3.0f/2.0f);
} }
#endif // HEADING_H #endif // HEADING_H

7
grid/walk/v2/modules/WalkModuleFollowDestination.h
View File

@@ -20,7 +20,7 @@ private:
const Grid<Node>& grid; const Grid<Node>& grid;
Dijkstra<Node> dijkstra; Dijkstra<Node> dijkstra;
const DijkstraNode<Node>* dnDest; const DijkstraNode<Node>* dnDest = nullptr;
struct DijkstraAccess { struct DijkstraAccess {
const Grid<Node>& grid; const Grid<Node>& grid;
@@ -58,9 +58,14 @@ public:
/** get the shortest path from the given start to the configured destination */ /** get the shortest path from the given start to the configured destination */
DijkstraPath<Node> getShortestPath(const Node& start) { DijkstraPath<Node> getShortestPath(const Node& start) {
// destination unknown? -> empty path
if (!dnDest) {return DijkstraPath<Node>();}
const DijkstraNode<Node>* dnStart = dijkstra.getNode(start); const DijkstraNode<Node>* dnStart = dijkstra.getNode(start);
const DijkstraPath<Node> path(dnStart, dnDest); const DijkstraPath<Node> path(dnStart, dnDest);
return path; return path;
} }
virtual void updateBefore(WalkState& state, const Node& startNode) override { virtual void updateBefore(WalkState& state, const Node& startNode) override {

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

@@ -14,7 +14,7 @@ template <typename Node, typename WalkState, typename Control> class WalkModuleH
private: private:
/** CURRENTLY NOT USED van-Mises distribution */ /** CURRENTLY NOT USED van-Mises distribution */
Distribution::LUT<double> dist; Distribution::LUT<float> dist;
/** random noise */ /** random noise */
Distribution::Normal<float> distNoise; Distribution::Normal<float> distNoise;
@@ -25,7 +25,7 @@ public:
/** ctor 3.0 should be OK! */ /** ctor 3.0 should be OK! */
WalkModuleHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) : WalkModuleHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) :
dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()), dist(Distribution::VonMises<float>(0.0f, 2.0f).getLUT()),
distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)), distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
ctrl(ctrl) { ctrl(ctrl) {

6
sensors/radio/WiFiProbabilityGrid.h
View File

@@ -56,6 +56,12 @@ public:
double prob = 1; double prob = 1;
int numMatchingAPs = 0; int numMatchingAPs = 0;
// after some runtime, check whether the wifi timestamps make sense
// those must not be zero, otherwise something is wrong!
if (!obs.entries.empty()) {
Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().ts.isZero(), "WiFiMeasurement timestamp is 0. this does not make sense...");
}
// process each observed measurement // process each observed measurement
for (const WiFiMeasurement& measurement : obs.entries) { for (const WiFiMeasurement& measurement : obs.entries) {