From f8d7f768f1883c40d3a03c267b1d0bf3b3a1153e Mon Sep 17 00:00:00 2001 From: FrankE Date: Thu, 17 Mar 2016 17:28:41 +0100 Subject: [PATCH] added missing code changes started working on refactoring of sensors new test-cases --- CMakeLists.txt | 3 +- geo/BBox3.h | 11 ++ geo/Point3.h | 4 + grid/factory/GridFactory.h | 3 + grid/factory/GridImportance.h | 12 +- grid/walk/GridWalkPathControl.h | 17 ++- grid/walk/GridWalkShortestPathControl.h | 82 +++++++++--- grid/walk/GridWalkSimpleControl.h | 10 +- main.cpp | 2 +- nav/dijkstra/Dijkstra.h | 97 +++++++++----- nav/dijkstra/DijkstraPath.h | 2 + nav/dijkstra/DijkstraStructs.h | 11 +- sensors/MACAddress.h | 129 +++++++++++++++++++ sensors/radio/AccessPoint.h | 35 +++++ sensors/radio/LogDistanceModel.h | 47 +++++++ tests/nav/dijkstra/TestDijkstra.cpp | 64 +++++++++ tests/sensors/TestMAC.cpp | 32 +++++ tests/sensors/radio/TestLogDistanceModel.cpp | 37 ++++++ 18 files changed, 524 insertions(+), 74 deletions(-) create mode 100644 sensors/MACAddress.h create mode 100644 sensors/radio/AccessPoint.h create mode 100644 sensors/radio/LogDistanceModel.h create mode 100644 tests/sensors/TestMAC.cpp create mode 100644 tests/sensors/radio/TestLogDistanceModel.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6ecdbd1..7460753 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,10 +65,11 @@ ADD_DEFINITIONS( -fstack-protector-all -g3 - -O0 + -O2 -DWITH_TESTS -DWITH_ASSERTIONS + -march=native ) diff --git a/geo/BBox3.h b/geo/BBox3.h index 7b9c02d..44cd281 100644 --- a/geo/BBox3.h +++ b/geo/BBox3.h @@ -60,6 +60,17 @@ public: p2 -= p; // decrease maximum } + /** grow the bbox by the amount given for each dimension */ + void grow(const float v) { + grow(Point3(v,v,v)); + } + + /** grow the bbox by the amount given for each dimension */ + void grow(const Point3& p) { + p1 -= p; // decrease minimum + p2 += p; // increase maximum + } + /** does the bbox contain the given point? */ bool contains(const Point3& p) const { if (p.x < p1.x) {return false;} diff --git a/geo/Point3.h b/geo/Point3.h index 5518190..d775e0c 100644 --- a/geo/Point3.h +++ b/geo/Point3.h @@ -26,6 +26,8 @@ struct Point3 { Point3 operator - (const Point3& o) const {return Point3(x-o.x, y-o.y, z-o.z);} + Point3 operator * (const Point3& o) const {return Point3(x*o.x, y*o.y, z*o.z);} + Point3 operator * (const float v) const {return Point3(v*x, v*y, v*z);} Point3 operator / (const float v) const {return Point3(x/v, y/v, z/v);} @@ -40,6 +42,8 @@ struct Point3 { Point3& operator -= (const Point3& o) {x-=o.x; y-=o.y; z-=o.z; return *this;} + bool operator < (const Point3& o) const {return x 2.0) {dist_m = 2.0;} // overall importance - return - avoidWalls.getProbability(dist_m) * 0.30 // avoid walls - + stickToWalls.getProbability(dist_m) * 0.15 // walk near walls - + farAway.getProbability(dist_m) * 0.15 // walk in the middle +// return - avoidWalls.getProbability(dist_m) * 0.30 // avoid walls +// + stickToWalls.getProbability(dist_m) * 0.15 // walk near walls +// + farAway.getProbability(dist_m) * 0.15 // walk in the middle + return - avoidWalls.getProbability(dist_m) // avoid walls + //+ stickToWalls.getProbability(dist_m) // walk near walls + //+ farAway.getProbability(dist_m) // walk in the middle ; diff --git a/grid/walk/GridWalkPathControl.h b/grid/walk/GridWalkPathControl.h index fcabec3..4eca917 100644 --- a/grid/walk/GridWalkPathControl.h +++ b/grid/walk/GridWalkPathControl.h @@ -32,6 +32,10 @@ private: DrawList drawer; +public: + + float pOther = 0.10; + public: @@ -65,9 +69,11 @@ public: // proportional change of the to-be-walked distance static Distribution::Normal dWalk(1, 0.10); - distance_m = distance_m*dWalk.draw()*2; // TODO: why *2? + distance_m = distance_m*dWalk.draw()*1.4; // TODO: why *2? headChange_rad = headChange_rad*dHead.draw(); + static Distribution::Normal sWalk(0, 0.15); + if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); } return walk(grid, start, distance_m, headChange_rad); @@ -75,12 +81,12 @@ public: private: - double getProbability(const T& start, const T& possible, const Heading head) const { + double getProbability(const T& start, const T& prev, 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; + throw 1; return 0.5;// stair start/end TODO: fix } @@ -95,7 +101,8 @@ private: // 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.025; + //const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.025; // from start + const double nodeProb = (possible.distToTarget < prev.distToTarget) ? 1 : pOther; // from previous node // bring it together return angleProb * nodeProb; @@ -123,7 +130,7 @@ private: drawer.reset(); for (T& neighbor : grid.neighbors(*cur.node)) { - const double prob = getProbability(*start.node, neighbor, reqHeading); + const double prob = getProbability(*start.node, *cur.node, neighbor, reqHeading); drawer.add(neighbor, prob); } diff --git a/grid/walk/GridWalkShortestPathControl.h b/grid/walk/GridWalkShortestPathControl.h index a2f196b..7ae0183 100644 --- a/grid/walk/GridWalkShortestPathControl.h +++ b/grid/walk/GridWalkShortestPathControl.h @@ -17,6 +17,8 @@ #include "GridWalkHelper.h" #include "GridWalk.h" +#include + template class GridWalkShortestPathControl : public GridWalk { @@ -82,14 +84,17 @@ protected: Dijkstra dijkstra; const T& target; - Point3 centerOfMass = Point3(0,0,0); + //Point3 centerOfMass = Point3(0,0,0); Wrapper wrapper; DijkstraPath* path = nullptr; - KNN* knn = nullptr; + //KNN* knn = nullptr; DrawList drawer; + std::vector points; + Point3 centerOfMass; + float stdDevDist; public: @@ -109,17 +114,51 @@ public: int recalc = 0; + int times = 3; + float pOther = 0.10; + GridWalkState getDestination(Grid& grid, const GridWalkState& start, float distance_m, float headChange_rad) { - // update the center-of-mass - centerOfMass = (centerOfMass * 0.999) + (((Point3)*start.node) * 0.001); - if (path == nullptr) {rebuildPath(grid);} - if (++recalc > 100 * 1000) { + // update the center-of-mass + points.push_back( (Point3)*start.node ); + + //centerOfMass = (centerOfMass * 0.999) + (((Point3)*start.node) * 0.001); + //if (path == nullptr) {rebuildPath(grid);} + + + + if (++recalc >= 7500) { + + // center of mass + Point3 sum(0,0,0); + for (const Point3& p : points) {sum += p;} + centerOfMass = sum/points.size(); + + // deviation from the center of mass + float dSum = 0; + float dSum2 = 0; + for (const Point3& p : points) { + const float d = p.getDistance(centerOfMass); + dSum += d; + dSum2 += d*d; + } + dSum /= points.size(); + dSum2 /= points.size(); + stdDevDist = std::sqrt( (dSum2) - (dSum * dSum) ) * times; + + // restart + points.clear(); recalc = 0; - rebuildPath(grid); + + // update + rebuildPath(grid, centerOfMass); + } + + + // if (knn != nullptr) { // const float dist = knn->getNearestDistance( {(float)start.node->x_cm, (float)start.node->y_cm, (float)start.node->z_cm} ); // if (dist > 10000) { @@ -133,9 +172,11 @@ public: // proportional change of the to-be-walked distance static Distribution::Normal dWalk(1, 0.10); - distance_m = distance_m*dWalk.draw()*2; // TODO: why *2? + distance_m = distance_m*dWalk.draw()*1.5; // TODO: why *2? headChange_rad = headChange_rad*dHead.draw(); + static Distribution::Normal sWalk(0, 0.10); + if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); } return walk(grid, start, distance_m, headChange_rad); @@ -143,12 +184,12 @@ public: private: - double getProbability(const T& start, const T& possible, const Heading head) const { + double getProbability(Grid& grid, const T& start, const T& prev, 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; + throw 1; return 0.5;// stair start/end TODO: fix } @@ -165,11 +206,16 @@ private: // nodes own importance double nodeProb = 1;//(possible.distToTarget < start.distToTarget) ? 1 : 0.025; - if (knn != nullptr) { - const float pd_m = knn->getNearestDistance( {(float)possible.x_cm, (float)possible.y_cm, (float)possible.z_cm} ) / 100; + if (path != nullptr) { + //const float pd_m = knn->getNearestDistance( {(float)possible.x_cm, (float)possible.y_cm, (float)possible.z_cm} ) / 100; + int steps = stdDevDist / grid.getGridSize_cm(); + const float pToTarget = possible.getDistanceInMeter(*path->getFromStart(steps).element); + const float sToTarget = prev.getDistanceInMeter(*path->getFromStart(steps).element); + nodeProb = (pToTarget < sToTarget) ? (1.0) : (pOther); //const float sd = knn->getNearestDistance( {(float)start.x_cm, (float)start.y_cm, (float)start.z_cm} ); //nodeProb = (pd < sd) ? 1 : 0.0; - nodeProb = Distribution::Exponential::getProbability(0.15, pd_m); + //nodeProb = Distribution::Exponential::getProbability(0.9, pToTarget); + //nodeProb = Distribution::Exponential::getProbability(1.0, tDist_m); } // bring it together @@ -198,7 +244,7 @@ private: drawer.reset(); for (T& neighbor : grid.neighbors(*cur.node)) { - const double prob = getProbability(*start.node, neighbor, reqHeading); + const double prob = getProbability(grid, *start.node, *cur.node, neighbor, reqHeading); drawer.add(neighbor, prob); } @@ -230,14 +276,14 @@ private: } /** rebuild the path for the given center point */ - void rebuildPath(Grid& grid) { + void rebuildPath(Grid& grid, const Point3 centerOfMass) { // find the grid node nearest to the current center-of-mass auto nearestGridNode = [&] (const T& n1, const T& n2) { return ((Point3)n1).getDistance(centerOfMass) < ((Point3)n2).getDistance(centerOfMass); }; const T& currentMass = *std::min_element(grid.begin(), grid.end(), nearestGridNode); delete path; path = nullptr; - delete knn; knn = nullptr; + //delete knn; knn = nullptr; DijkstraNode* dnTarget = dijkstra.getNode(target); DijkstraNode* dnStart = dijkstra.getNode(currentMass); @@ -252,9 +298,9 @@ private: // create k-nn lookup wrapper = Wrapper(path); - knn = new KNN(wrapper); + //knn = new KNN(wrapper); } catch (...) { - knn = nullptr; + //knn = nullptr; path = nullptr; } diff --git a/grid/walk/GridWalkSimpleControl.h b/grid/walk/GridWalkSimpleControl.h index 3552851..7ea6cef 100644 --- a/grid/walk/GridWalkSimpleControl.h +++ b/grid/walk/GridWalkSimpleControl.h @@ -47,11 +47,13 @@ public: static Distribution::Normal dHead(1, 0.01); // proportional change of the to-be-walked distance - static Distribution::Normal dWalk(1, 0.50); + static Distribution::Normal dWalk(1, 0.10); - distance_m = distance_m*dWalk.draw()*2; // TODO: why *2? + distance_m = distance_m*dWalk.draw()*1.4; // TODO: why * X? headChange_rad = headChange_rad*dHead.draw(); + static Distribution::Normal sWalk(0, 0.10); + if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); } return walk(grid, start, distance_m, headChange_rad); @@ -79,8 +81,8 @@ private: // const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally // nodes own importance - //const double nodeProb = Distribution::Logistic::getCDF(possible.imp, 1, 0.5); - const double nodeProb = std::pow(possible.imp, 5); + const double nodeProb = Distribution::Logistic::getCDF(possible.imp, 1, 0.9); + //const double nodeProb = std::pow(possible.imp, 2); //const double nodeProb = 1.0; // bring it together diff --git a/main.cpp b/main.cpp index bec819d..82ea72a 100755 --- a/main.cpp +++ b/main.cpp @@ -15,7 +15,7 @@ int main(int argc, char** argv) { #ifdef WITH_TESTS ::testing::InitGoogleTest(&argc, argv); - //::testing::GTEST_FLAG(filter) = "*Importance*"; + ::testing::GTEST_FLAG(filter) = "*huge*"; //::testing::GTEST_FLAG(filter) = "*Walk*"; return RUN_ALL_TESTS(); diff --git a/nav/dijkstra/Dijkstra.h b/nav/dijkstra/Dijkstra.h index 0180643..8aa9c95 100644 --- a/nav/dijkstra/Dijkstra.h +++ b/nav/dijkstra/Dijkstra.h @@ -44,18 +44,15 @@ public: // sorted list of all to-be-processed nodes ToProcess toBeProcessedNodes; - // all already processed edges - std::unordered_set usedEdges; - // run from start const T* cur = &start; // create a node for the start element - DijkstraNode* dnStart = getNode(cur); + DijkstraNode* dnStart = getOrCreateNode(cur); dnStart->cumWeight = 0; - // add this node to the processing list - toBeProcessedNodes.add(dnStart); + // add this node to the processing list (and mark it as "enqueued") + toBeProcessedNodes.addOnce(dnStart, dnStart->cumWeight); // until we are done while(unlikely(!toBeProcessedNodes.empty())) { @@ -66,28 +63,14 @@ public: // stop when end was reached?? //if (dnSrc->element == &end) {break;} - // process each neighbor of the current element + // visit (and maybe update) each neighbor of the current element for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) { // get the neighbor itself const T* dst = acc.getNeighbor(*dnSrc->element, i); - // get-or-create a node for the neighbor - DijkstraNode* dnDst = getNode(dst); - - // get-or-create the edge describing the connection - //const DijkstraEdge edge = getEdge(dnSrc, dnDst); - const auto edge = getEdge(dnSrc, dnDst); - - // was this edge already processed? -> skip it - if (usedEdges.find(edge) != usedEdges.end()) {continue;} - - // otherwise: remember it - usedEdges.insert(edge); - - // and add the node for later processing - //toBeProcessedNodes.push_back(dnDst); - toBeProcessedNodes.add(dnDst); + // get-or-create a DijkstraNode for the neighbor + DijkstraNode* dnDst = getOrCreateNode(dst); // get the distance-weight to the neighbor const float weight = acc.getWeightBetween(*dnSrc->element, *dst); @@ -96,8 +79,17 @@ public: // update the weight to the destination? const float potentialWeight = dnSrc->cumWeight + weight; if (potentialWeight < dnDst->cumWeight) { + + // re-sort (weight has changed) within the list of to-be-processed nodes + toBeProcessedNodes.addOrUpdate(dnDst, dnDst->cumWeight, potentialWeight); dnDst->cumWeight = potentialWeight; dnDst->previous = dnSrc; + + } else { + + // if this neighbor was encountered for the first time, add it (and mark it as "enqueued") + toBeProcessedNodes.addOnce(dnDst, dnDst->cumWeight); + } } @@ -114,30 +106,67 @@ private: /** helper class to sort to-be-processed nodes by their distance from the start */ class ToProcess { - /** sort comparator */ - struct setComp { - bool operator() (const DijkstraNode* dn1, const DijkstraNode* dn2) { - return dn1->cumWeight < dn2->cumWeight; + /** assign a weight to a node and provide the corresponding comparator */ + struct WeightedNode { + + DijkstraNode* dn; + const float weight; + + /** ctor */ + WeightedNode(DijkstraNode* dn, const float weight) : dn(dn), weight(weight) {;} + + /** compare by weight. same weight? : compare by pointer */ + bool operator < (const WeightedNode& wn) const { + return (weight != wn.weight) ? (weight < wn.weight) : (dn < wn.dn); } + }; /** sorted list of to-be-processed nodes */ - std::set*, setComp> toBeProcessedNodes; + std::set nodes; public: /** add a new to-be-processed node */ - void add(DijkstraNode* node) {toBeProcessedNodes.insert(node);} + void addOnce(DijkstraNode* node, const float weight) { + + // skip nodes that were already enqueued + if (node->enqueued) {return;} + + // add the combination (node+weight) + nodes.insert(WeightedNode(node, weight)); + + // mark the node as processed + node->enqueued = true; + + } + + /** add a new to-be-processed node or update its old weight to the new one */ + void addOrUpdate(DijkstraNode* node, const float oldWeight, const float newWeight) { + + // find and remove the previous combination (node+weight) if any + const auto old = nodes.find(WeightedNode(node, oldWeight)); + if (old != nodes.end()) {nodes.erase(old);} + + // add the new combination (node+weight) + nodes.insert(WeightedNode(node, newWeight)); + + // mark the node as processed + node->enqueued = true; + + } /** get the next to-be-processed node (smallest distance) */ DijkstraNode* pop() { - DijkstraNode* next = *toBeProcessedNodes.begin(); - toBeProcessedNodes.erase(toBeProcessedNodes.begin()); - return next; + DijkstraNode* dn = (*nodes.begin()).dn; + nodes.erase(nodes.begin()); + return dn; } /** set empty? */ - bool empty() const {return toBeProcessedNodes.empty();} + bool empty() const { + return nodes.empty(); + } }; @@ -145,7 +174,7 @@ private: /** get (or create) a new node for the given user-node */ - inline DijkstraNode* getNode(const T* userNode) { + inline DijkstraNode* getOrCreateNode(const T* userNode) { auto it = nodes.find(userNode); if (unlikely(it == nodes.end())) { DijkstraNode* dn = new DijkstraNode(userNode); diff --git a/nav/dijkstra/DijkstraPath.h b/nav/dijkstra/DijkstraPath.h index 1e92823..dc4738d 100644 --- a/nav/dijkstra/DijkstraPath.h +++ b/nav/dijkstra/DijkstraPath.h @@ -48,6 +48,8 @@ public: const DijkstraNode& operator [] (const int idx) const {return *(path[idx]);} size_t size() const {return path.size();} + /** get the idx-th element from the starting-node. if this is beyond the end, the last node is returned */ + const DijkstraNode& getFromStart(const int idx) const {return (idx >= (int) path.size()) ? (*path.back()) : (*path[idx]);} /** NANOFLANN: number of elements in the path */ inline int kdtree_get_point_count() const { diff --git a/nav/dijkstra/DijkstraStructs.h b/nav/dijkstra/DijkstraStructs.h index 27ec136..7f6a9e9 100644 --- a/nav/dijkstra/DijkstraStructs.h +++ b/nav/dijkstra/DijkstraStructs.h @@ -19,15 +19,12 @@ template struct DijkstraNode { /** the weight from the start up to this element */ float cumWeight; + /** whether we already enqueued this node into the processing list (do NOT examing nodes twice) */ + bool enqueued; + /** ctor */ - DijkstraNode(const T* element) : element(element), previous(), cumWeight(INF) {;} - - -// /** equal? (bi-dir) */ -// bool operator == (const DijkstraNode& other) const { -// return element == other.element; -// } + DijkstraNode(const T* element) : element(element), previous(), cumWeight(INF), enqueued(false) {;} }; diff --git a/sensors/MACAddress.h b/sensors/MACAddress.h new file mode 100644 index 0000000..a4120aa --- /dev/null +++ b/sensors/MACAddress.h @@ -0,0 +1,129 @@ +#ifndef MACADDRESS_H +#define MACADDRESS_H + +#include +#include +#include "../Exception.h" + +/** + * describes a MAC-Address as 64-bit integer. + * provides 8-bit access to all 6 values + */ +union MACAddress { + +private: + + /** fieldwise access */ + struct { + uint8_t h5; + uint8_t h4; + uint8_t h3; + uint8_t h2; + uint8_t h1; + uint8_t h0; + } fields; + + /** store as 64-bit integer */ + uint64_t mac; + +public: + + /** empty ctor */ + MACAddress() { + ; + } + + /** copy ctor */ + MACAddress(const MACAddress& o) : mac(o.mac) { + ; + } + + /** ctor form long */ + MACAddress(const uint64_t mac) : mac(mac) { + ; + } + + /** ctor form string (e.g. "xx:xx:xx:xx:xx:xx") */ + MACAddress(const std::string& str) { + + // sanity check + if (str.size() != 17) {throw Exception("invalid hex string length. must be 17");} + + mac = 0; // all zeros + fields.h5 = hexWordToInt(str[ 0], str[ 1]); + fields.h4 = hexWordToInt(str[ 3], str[ 4]); + fields.h3 = hexWordToInt(str[ 6], str[ 7]); + fields.h2 = hexWordToInt(str[ 9], str[10]); + fields.h1 = hexWordToInt(str[12], str[13]); + fields.h0 = hexWordToInt(str[15], str[16]); + + } + + /** convert to hex-string ("xx:xx:xx:xx:xx:xx") */ + std::string asString() const { + + std::string str = ":::::::::::::::::"; + + intToHexStr(fields.h5, &str[ 0]); + intToHexStr(fields.h4, &str[ 3]); + intToHexStr(fields.h3, &str[ 6]); + intToHexStr(fields.h2, &str[ 9]); + intToHexStr(fields.h1, &str[12]); + intToHexStr(fields.h0, &str[15]); + + return str; + + } + + /** get the mac address as a long-int value */ + uint64_t asLong() const { + return mac; + } + + /** equal? */ + bool operator == (const MACAddress& o) const { + return o.asLong() == asLong(); + } + +private: + + /** convert the given hex char [0-F] to an integer [0-15] */ + static uint8_t hexCharToInt(const char _hex) { + + // to upper case + const char hex = (_hex >= 'a') ? (_hex - ('a' - 'A')) : (_hex); + + // convert + return (hex - '0' < 10) ? (hex - '0') : (hex - 'A' + 10); + + } + + /** convert the given hex-word to an integer */ + static uint8_t hexWordToInt(const char hi, const char lo) { + return hexCharToInt(hi) << 4 | hexCharToInt(lo); + } + + /** conver the given integer [0-15] to a hex char [0-F] */ + static char intToHexChar(const uint8_t val) { + return (val < 10) ? ('0' + val) : ('A' - 10 + val); + } + + /** insert two hex chars into the provided string buffer */ + static void intToHexStr(const uint8_t val, char* dst) { + dst[0] = intToHexChar((val >> 4) & 0xF); + dst[1] = intToHexChar((val >> 0) & 0xF); + } + +}; + + +/** hash-method for MAC-Addresses */ +namespace std { + template <> struct hash { + std::size_t operator() (const MACAddress& mac) const { + return std::hash()(mac.asLong()); + } + }; +} + +#endif // MACADDRESS_H diff --git a/sensors/radio/AccessPoint.h b/sensors/radio/AccessPoint.h new file mode 100644 index 0000000..62b7282 --- /dev/null +++ b/sensors/radio/AccessPoint.h @@ -0,0 +1,35 @@ +#ifndef ACCESSPOINT_H +#define ACCESSPOINT_H + +#include "../MACAddress.h" + +/** + * represents a Wi-Fi-AccessPoint + * an AP is represented by its MAC-Address and + * may provide a readably SSID + */ +class AccessPoint { + +public: + + /** the AP's MAC-Address */ + const MACAddress mac; + + /** the AP's readable SSID */ + const std::string ssid; + +public: + + /** ctor */ + AccessPoint(const MACAddress& mac, const std::string& ssid) : mac(mac), ssid(ssid) { + ; + } + + /** ctor */ + AccessPoint(const std::string& mac, const std::string& ssid) : mac(mac), ssid(ssid) { + ; + } + +}; + +#endif // ACCESSPOINT_H diff --git a/sensors/radio/LogDistanceModel.h b/sensors/radio/LogDistanceModel.h new file mode 100644 index 0000000..2072e43 --- /dev/null +++ b/sensors/radio/LogDistanceModel.h @@ -0,0 +1,47 @@ +#ifndef LOGDISTANCEMODEL_H +#define LOGDISTANCEMODEL_H + +#include + +class LogDistanceModel { + +public: + + /** convert from RSSI to a distance (in meter) */ + static float rssiToDistance(const float txPower, const float pathLoss, const float rssi) { + return pow(10, (txPower - rssi) / (10 * pathLoss)); + } + + /** convert from a distance (in meter) to the expected RSSI */ + static float distanceToRssi(const float txPower, const float pathLoss, const float distance) { + if (distance <= 1) {return txPower;} + return (txPower - (10 * pathLoss * std::log10(distance))); + } + +}; + +#include + +class LogDistFastModel { + +private: + + std::vector lut; + +public: + + LogDistFastModel() { + lut.resize(2000); + for (int i = 0; i < 2000; ++i) { + lut[i] = std::log10(i/10.0f); + } + } + + float distanceToRssi(const float txPower, const float pathLoss, const float distance) const { + if (distance <= 1) {return txPower;} + return (txPower - (10 * pathLoss * lut[ (int) (distance*10) ])); + } + +}; + +#endif // LOGDISTANCEMODEL_H diff --git a/tests/nav/dijkstra/TestDijkstra.cpp b/tests/nav/dijkstra/TestDijkstra.cpp index ec19753..8a33976 100644 --- a/tests/nav/dijkstra/TestDijkstra.cpp +++ b/tests/nav/dijkstra/TestDijkstra.cpp @@ -48,4 +48,68 @@ TEST(Dijkstra, build) { } +void build(Grid& grid) { + + const int size = 10000; + const int gs = grid.getGridSize_cm(); + + for (int x = 0; x < size; x += gs) { + for (int y = 0; y < size; y += gs) { + grid.add(GP(x,y,0)); + } + } + + std::set done; + + for (int x = 0; x < size; x += gs) { + for (int y = 0; y < size; y += gs) { + + const GridPoint gp1(x,y,0); + const int idx1 = grid.getNodeFor(gp1).getIdx(); + + for (int x1 = -gs; x1 <= +gs; x1 += gs) { + for (int y1 = -gs; y1 <= +gs; y1 += gs) { + const GridPoint gp2(x+x1, y+y1, 0); + if (grid.hasNodeFor(gp2)) { + int idx2 = grid.getNodeFor(gp2).getIdx(); + if (done.find(idx2) != done.end()) {continue;} + grid.connectBiDir(idx1, idx2); + } + + } + } + + done.insert(idx1); + + } + } + +} + +void dijkstra(Grid& grid) { + + class TMP { + Grid& grid; + public: + TMP(Grid& grid) : grid(grid) {;} + int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();} + const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);} + float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();} + } tmp(grid); + + Dijkstra d; + d.build(grid[0], grid[0], tmp); + +} + +TEST(Dijkstra, huge) { + + + Grid grid(10); + build(grid); + dijkstra(grid); + +} + + #endif diff --git a/tests/sensors/TestMAC.cpp b/tests/sensors/TestMAC.cpp new file mode 100644 index 0000000..4638bf3 --- /dev/null +++ b/tests/sensors/TestMAC.cpp @@ -0,0 +1,32 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../sensors/MACAddress.h" + +TEST(MAC, ctorSize) { + + ASSERT_THROW(MACAddress("12:34:56:78:9A:A"), std::exception); + MACAddress("12:34:56:78:9A:AB"); + ASSERT_THROW(MACAddress("12:34:56:78:9A:ABC"), std::exception); + +} + +TEST(MAC, caseInsensitive) { + + MACAddress mac1("12:34:56:78:9A:BC"); + MACAddress mac2("12:34:56:78:9a:bc"); + ASSERT_EQ(mac1, mac2); + +} + +TEST(MAC, convertLong) { + + MACAddress mac1("12:34:56:78:9A:BC"); + MACAddress mac2 = MACAddress( mac1.asLong() ); + ASSERT_EQ(mac1, mac2); + +} + + +#endif diff --git a/tests/sensors/radio/TestLogDistanceModel.cpp b/tests/sensors/radio/TestLogDistanceModel.cpp new file mode 100644 index 0000000..b5095f5 --- /dev/null +++ b/tests/sensors/radio/TestLogDistanceModel.cpp @@ -0,0 +1,37 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" + +#include "../../../sensors/radio/LogDistanceModel.h" + +TEST(LogDistanceModel, calc) { + + const float txp = -40; + const float exp = 1; + ASSERT_EQ(-40, LogDistanceModel::distanceToRssi(txp, exp, 1.0)); + + // only exponent changed. at 1.0 meter: no difference + ASSERT_EQ(LogDistanceModel::distanceToRssi(-40, 1.0, 1.0), LogDistanceModel::distanceToRssi(-40, 2.0, 1.0)); + + // distance increment + ASSERT_GT(LogDistanceModel::distanceToRssi(-40, 1.0, 1.0), LogDistanceModel::distanceToRssi(-40, 1.0, 2.0)); + + // exponent at more than 1.0m + ASSERT_GT(LogDistanceModel::distanceToRssi(-40, 1.0, 3.0), LogDistanceModel::distanceToRssi(-40, 1.1, 3.0)); + + // other txp + ASSERT_GT(LogDistanceModel::distanceToRssi(-40, 1.0, 1.0), LogDistanceModel::distanceToRssi(-45, 1.0, 1.0)); + +} + +TEST(LogDistanceModel, forwardBackward) { + + const float txp = -40; + const float exp = 1; + ASSERT_EQ(1.0, LogDistanceModel::rssiToDistance(txp, exp, LogDistanceModel::distanceToRssi(txp, exp, 1.0))); + ASSERT_EQ(5.0, LogDistanceModel::rssiToDistance(txp, exp, LogDistanceModel::distanceToRssi(txp, exp, 5.0))); + ASSERT_EQ(10.0, LogDistanceModel::rssiToDistance(txp, exp, LogDistanceModel::distanceToRssi(txp, exp, 10.0))); + +} + +#endif