From 8930be1e2ca8eff1e0f316097a9a4414a2242b4e Mon Sep 17 00:00:00 2001 From: kazu Date: Fri, 31 Mar 2017 11:47:29 +0200 Subject: [PATCH] added new sanity-check assertions fixed issue with angles [bad interface] - adjusted other parts accordingly - added corresponding test-cases started working on absolute heading --- geo/Angle.h | 10 ++- geo/Heading.h | 7 +- grid/factory/v2/Importance.h | 2 + grid/walk/v2/GridWalker.h | 12 ++- .../WalkModuleAbsoluteHeadingControl.h | 85 +++++++++++++++++++ .../v2/modules/WalkModuleHeadingControl.h | 3 +- .../v2/modules/WalkModuleHeadingVonMises.h | 3 +- main.cpp | 2 +- sensors/offline/OfflineAndroid.h | 26 +++++- tests/geo/TestAngle.cpp | 37 ++++++-- 10 files changed, 168 insertions(+), 19 deletions(-) create mode 100644 grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h diff --git a/geo/Angle.h b/geo/Angle.h index 5732897..eabe999 100755 --- a/geo/Angle.h +++ b/geo/Angle.h @@ -29,6 +29,13 @@ public: return radToDeg(getRAD_2PI(x1,y1,x2,y2)); } + /** ensure the given radians-value is within [0:2pi] */ + static float makeSafe_2PI(float rad) { + while(rad < 0) {rad += 2*PI;} + while(rad >= 2*PI) {rad -= 2*PI;} + return rad; + } + /** * gets the angular difference between @@ -44,13 +51,14 @@ public: /** * gets the angular difference between + * "angular change from r1 to r2" * - the given radians [0:2PI] * - as a change-in-direction between [-PI:+PI] */ static float getSignedDiffRAD_2PI(const float r1, const float r2) { Assert::isBetween(r1, 0.0f, (float)(2*PI), "r1 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 = r2-r1; if (diff > +PI) {diff = -(2*PI - diff);} else if (diff < -PI) {diff = +(2*PI + diff);} Assert::isBetween(diff, -PI, (float)(+PI), "result out of bounds"); // [-180:+180] deg diff --git a/geo/Heading.h b/geo/Heading.h index eb9720e..ca22b6c 100644 --- a/geo/Heading.h +++ b/geo/Heading.h @@ -33,8 +33,11 @@ public: } /** signled angular difference [-PI:+PI] */ - float getSignedDiff(const Heading other) const { - return Angle::getSignedDiffRAD_2PI(rad, other.rad); +// float getSignedDiff(const Heading other) const { +// return Angle::getSignedDiffRAD_2PI(other.rad, rad); +// } + static float getSignedDiff(const Heading from, const Heading to) { + return Angle::getSignedDiffRAD_2PI(from.rad, to.rad); } /** update the angle but ensure we stay within [0:2PI] */ diff --git a/grid/factory/v2/Importance.h b/grid/factory/v2/Importance.h index 618185d..0906fa2 100644 --- a/grid/factory/v2/Importance.h +++ b/grid/factory/v2/Importance.h @@ -153,6 +153,8 @@ public: // sanity check + Assert::isNotNaN(n1.walkImportance, "detected NaN walk importance for " + n1.asString()); + Assert::isNotNaN(n1.navImportance, "detected NaN walk importance for " + n1.asString()); Assert::isTrue(n1.walkImportance >= 0, "detected negative walk importance. does not make sense!"); Assert::isTrue(n1.navImportance >= 0, "detected negative nav importance. does not make sense!"); diff --git a/grid/walk/v2/GridWalker.h b/grid/walk/v2/GridWalker.h index 1fe88b7..59f51e9 100644 --- a/grid/walk/v2/GridWalker.h +++ b/grid/walk/v2/GridWalker.h @@ -104,9 +104,19 @@ public: //if (cnt != 0) {probability /= cnt;} else {probability = 1.0;} probability = 1.0; //probability = (maxEdgeProb.isValid()) ? (maxEdgeProb.get()) : (1.0); // dist_m might be zero -> no edges -> no maximum - probability *= curNode->getWalkImportance();// < 0.4f ? (0.1) : (1.0); // "kill" particles that walk near walls (most probably trapped ones) + + // add the walk importance to the probabiliy [each node has a to-be-walked-probability depending on its distance to walls, etc...] + const float walkImportance = curNode->getWalkImportance(); + Assert::isNotNaN(walkImportance, "grid-node's walk-importance is NaN. Did you forget to calculate the importance values after building the grid?"); + Assert::isBetween(walkImportance, 0.0f, 2.5f, "grid-node's walk-importance is out of range. Did you forget to calculate the importance values after building the grid?"); + probability *= walkImportance;// < 0.4f ? (0.1) : (1.0); // "kill" particles that walk near walls (most probably trapped ones) + + //probability = std::pow(probability, 5); + // sanity check + Assert::isNotNaN(probability, "detected NaN grid-walk probability"); + // update after updateAfter(currentState, *startNode, *curNode); diff --git a/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h b/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h new file mode 100644 index 0000000..d3987f9 --- /dev/null +++ b/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h @@ -0,0 +1,85 @@ +#ifndef WALKMODULEABSOLUTEHEADINGCONTROL_H +#define WALKMODULEABSOLUTEHEADINGCONTROL_H + +/** + * compare the state's absolute heading against a given compass [control] + */ + +#include "WalkModule.h" +#include "WalkStateHeading.h" + +#include "../../../../geo/Heading.h" +#include "../../../../math/Distributions.h" + + +/** keep the state's heading */ +template class WalkModuleAbsoluteHeadingControl : public WalkModule { + + const float sigma_rad; + const Control* ctrl; + +public: + + /** ctor. 180 should be OK! */ + WalkModuleAbsoluteHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) : + sigma_rad(Angle::degToRad(sensorNoiseDegreesSigma)), + ctrl(ctrl) { + + // ensure the template WalkState inherits from 'WalkStateHeading'! + StaticAssert::AinheritsB(); + + } + + + virtual void updateBefore(WalkState& state, const Node& startNode) override { + (void) state; + (void) startNode; + } + + virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override { + (void) state; + (void) startNode; + (void) endNode; + } + + + virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override { + (void) state; + (void) curNode; + (void) nextNode; + } + + double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { + + (void) startNode; + + // NOTE: ctrl->turnAngle is cumulative SINCE the last transition! + // reset this one after every transition! + Assert::isBetween(ctrl->compassAzimuth_rad, 0.0f, (float)(2*M_PI), "the given absolute heading is out of bounds"); + + // ignore for stairs? + //if (potentialNode.getType() == GridNode::TYPE_STAIR) {return 1.0;} + + // for elevator edges [same (x,y) but different z] just return 1 + if (potentialNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;} + if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;} + //if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;} + + // get the heading between curNode and potentialNode + const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm); + + // compare the heading against the state's heading - the last error + const Heading stateHead = state.heading.direction; + + // get the difference + const float angularDiff = head.getDiffHalfRAD(stateHead); + + if (angularDiff > Angle::degToRad(180)) {return 0.05;} + if (angularDiff > Angle::degToRad(90)) {return 0.25;} + {return 0.70;} + + } + +}; + +#endif // WALKMODULEABSOLUTEHEADINGCONTROL_H diff --git a/grid/walk/v2/modules/WalkModuleHeadingControl.h b/grid/walk/v2/modules/WalkModuleHeadingControl.h index 01fe668..a79cdad 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingControl.h +++ b/grid/walk/v2/modules/WalkModuleHeadingControl.h @@ -77,7 +77,8 @@ public: const Heading stateHead = state.heading.direction; // get the error (signed difference) between both - const float angularDiff = stateHead.getSignedDiff(head); +// const float angularDiff = stateHead.getSignedDiff(head); + const float angularDiff = Heading::getSignedDiff(head, stateHead); // adjust the error. // note: the error may get > +/- 2PI but this is not an issue! diff --git a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h index 9c01a79..30be8e1 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h +++ b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h @@ -81,7 +81,8 @@ public: const Heading stateHead = state.heading.direction; // get the error (signed difference) between both - const float angularDiff = stateHead.getSignedDiff(head); + //const float angularDiff = stateHead.getSignedDiff(head); + const float angularDiff = Heading::getSignedDiff(head, stateHead); // adjust the error. // note: the error may get > +/- 2PI but this is not an issue! diff --git a/main.cpp b/main.cpp index 411cec6..2808d16 100755 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Offline.readWrite*"; - ::testing::GTEST_FLAG(filter) = "*Earth*"; + ::testing::GTEST_FLAG(filter) = "*Angle*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; diff --git a/sensors/offline/OfflineAndroid.h b/sensors/offline/OfflineAndroid.h index 19049b1..3d32fc0 100644 --- a/sensors/offline/OfflineAndroid.h +++ b/sensors/offline/OfflineAndroid.h @@ -54,7 +54,7 @@ private: std::vector> gyro; std::vector> accel; - std::vector> gravity; + std::vector> gravity; std::vector> compass; std::vector> barometer; @@ -86,7 +86,7 @@ public: const std::vector>& getAccelerometer() const {return accel;} /** get all gravity readings */ - const std::vector>& getGravity() const {return gravity;} + const std::vector>& getGravity() const {return gravity;} /** get all barometer readings */ const std::vector>& getBarometer() const {return barometer;} @@ -173,8 +173,8 @@ private: } case (int) Offline::Sensor::GRAVITY: { - const AccelerometerData data = parseAccelerometer(sensorData); - gravity.push_back(OfflineEntry(ts, data)); + const GravityData data = parseGravity(sensorData); + gravity.push_back(OfflineEntry(ts, data)); if (listener) {listener->onGravity(ts, data);} break; } @@ -301,6 +301,24 @@ private: } + static inline GravityData parseGravity(const std::string& data) { + + const size_t pos1 = data.find(';', 0); + const size_t pos2 = data.find(';', pos1+1); + const size_t pos3 = data.find(';', pos2+1); + + Assert::isTrue(pos1 != std::string::npos, "format error"); + Assert::isTrue(pos2 != std::string::npos, "format error"); + Assert::isTrue(pos3 != std::string::npos, "format error"); + + const std::string sx = data.substr(0, pos1); + const std::string sy = data.substr(pos1+1, pos2-pos1-1); + const std::string sz = data.substr(pos2+1, pos3-pos2-1); + + return GravityData(std::stof(sx), std::stof(sy), std::stof(sz)); + + } + /** parse the given Barometer entry */ static inline BarometerData parseBarometer(const std::string& data) { diff --git a/tests/geo/TestAngle.cpp b/tests/geo/TestAngle.cpp index 713e571..0299627 100755 --- a/tests/geo/TestAngle.cpp +++ b/tests/geo/TestAngle.cpp @@ -14,17 +14,38 @@ TEST(Angle, dir) { } +TEST(Angle, safe) { + + ASSERT_EQ(0, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(0))))); + ASSERT_EQ(0, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(360))))); + ASSERT_EQ(85, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(85))))); + ASSERT_EQ(155, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(155))))); + ASSERT_EQ(275, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(275))))); + ASSERT_EQ(355, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(355))))); + + // negative + ASSERT_EQ(330, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(-30))))); + ASSERT_EQ(270, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(-90))))); + ASSERT_EQ(185, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(-175))))); + + // too positive + ASSERT_EQ(30, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(390))))); + ASSERT_EQ(140, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(500))))); + ASSERT_EQ(180, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(900))))); + +} + TEST(Angle, calc) { - ASSERT_EQ(0, Angle::getDEG_360(0,0, +1,0)); // to the right - ASSERT_EQ(90, Angle::getDEG_360(0,0, 0,+1)); // upwards - ASSERT_EQ(180, Angle::getDEG_360(0,0, -1,0)); // to the left - ASSERT_EQ(270, Angle::getDEG_360(0,0, 0,-1)); // downwards + ASSERT_EQ(0, (int)Angle::getDEG_360(0,0, +1,0)); // to the right + ASSERT_EQ(90, (int)Angle::getDEG_360(0,0, 0,+1)); // upwards + ASSERT_EQ(180, (int)Angle::getDEG_360(0,0, -1,0)); // to the left + ASSERT_EQ(270, (int)Angle::getDEG_360(0,0, 0,-1)); // downwards - ASSERT_EQ(45, Angle::getDEG_360(0,0, +1,+1)); // to the upper right - ASSERT_EQ(135, Angle::getDEG_360(0,0, -1,+1)); // to the upper left - ASSERT_EQ(225, Angle::getDEG_360(0,0, -1,-1)); // to the lower left - ASSERT_EQ(315, Angle::getDEG_360(0,0, +1,-1)); // to the lower right + ASSERT_EQ(45, (int)Angle::getDEG_360(0,0, +1,+1)); // to the upper right + ASSERT_EQ(135, (int)Angle::getDEG_360(0,0, -1,+1)); // to the upper left + ASSERT_EQ(225, (int)Angle::getDEG_360(0,0, -1,-1)); // to the lower left + ASSERT_EQ(315, (int)Angle::getDEG_360(0,0, +1,-1)); // to the lower right }