Première version non testée de la triangulation

CMakeLists.txt

@@ -12,7 +12,7 @@ pkg_check_modules(TCPSocket REQUIRED TCPSocket)
 include_directories(include)
 add_executable(${PROJECT_NAME} src/main.cpp src/localization.h src/localization.cpp)
 
-set(RPLIDAR_SDK_PATH "/home/isen/rplidar_sdk")
+set(RPLIDAR_SDK_PATH "/home/modelec/rplidar_sdk")
 
 add_library(rplidar STATIC IMPORTED)
 

src/localization.cpp

@@ -22,15 +22,6 @@ void Localization::setBeaconsMode(bool state){
 }
 
 
-vector<int> Localization::getRobotPosition() const {
-    vector<int> data;
-    data.push_back(this->x_robot);
-    data.push_back(this->y_robot);
-    data.push_back(this->robotPositionGap);
-    return data;
-}
-
-
 bool Localization::getBeaconsMode() const{
     return this->beaconsMode;
 }
@@ -50,6 +41,11 @@ bool Localization::isStarted() const {
 }
 
 
+bool Localization::isTriangulating() const {
+    return this->triangulationMode;
+}
+
+
 pair<double, double>
 Localization::robotToCartesian(sl_lidar_response_measurement_node_hq_t node, int x_robot, int y_robot,
                                int alpha_robot) {
@@ -123,62 +119,6 @@ int Localization::getMaxGap(const list<pair<double, double>> &positionList, pair
 }
 
 
-pair<int, int> Localization::getCircleCenter(list<pair<double, double>> detectedPositions, int radius) {
-    // Implementing https://math.stackexchange.com/a/1781546
-    list<pair<double, double>> deductedCenters;
-    double min_x = (*detectedPositions.begin()).first;
-    double max_x = (*detectedPositions.begin()).first;
-    double min_y = (*detectedPositions.begin()).second;
-    double max_y = (*detectedPositions.begin()).second;
-    for (auto iter = detectedPositions.begin(); iter != detectedPositions.end(); iter++) {
-        if ((*iter).first < min_x) {
-            min_x = (*iter).first;
-        }
-        if ((*iter).first > max_x) {
-            max_x = (*iter).first;
-        }
-        if ((*iter).second < min_y) {
-            min_y = (*iter).second;
-        }
-        if ((*iter).second > max_y) {
-            max_y = (*iter).second;
-        }
-        auto iterPlus3 = iter;
-        for (int i = 0; i < 3; i++) {
-            if (next(iterPlus3) == detectedPositions.end()) {
-                iterPlus3 = detectedPositions.begin();
-            } else {
-                iterPlus3 = next(iterPlus3);
-            }
-        }
-        double xa = ((*iter).first - (*iterPlus3).first) / 2.f;
-        double ya = ((*iter).second - (*iterPlus3).second) / 2.f;
-        double x0 = (*iter).first + xa;
-        double y0 = (*iter).second + ya;
-        double a = sqrt(pow((*iter).first, 2) + pow((*iter).second, 2));
-        double b = sqrt(pow(radius, 2) - pow(a, 2));
-        deductedCenters.emplace_back(x0 + b * ya / a, y0 - b * xa / a);
-        deductedCenters.emplace_back(x0 - b * ya / a, y0 + b * xa / a);
-    }
-    double total_x = 0;
-    double total_y = 0;
-    unsigned int n = 0;
-    for (auto &deductedCenter: deductedCenters) {
-        if (deductedCenter.first > min_x && deductedCenter.first < max_x && deductedCenter.second > min_y &&
-            deductedCenter.second < max_y) {
-            n++;
-            total_x += deductedCenter.first;
-            total_y += deductedCenter.second;
-        }
-    }
-    if (n > 0) {
-        return make_pair((int) total_x / n, (int) total_y / n);
-    } else {
-        return make_pair(-1, -1);
-    }
-}
-
-
 vector<list<pair<double, double>>> Localization::getAgglomerates(list<pair<double, double>> &positionsList) {
     vector<list<pair<double, double>>> agglomerated_points;
     list<pair<double, double>> new_list;
@@ -227,15 +167,13 @@ vector<list<pair<double, double>>> Localization::getAgglomerates(list<pair<doubl
 
 list<pair<double, double>> Localization::getMostProbableAgglomerate(vector<list<pair<double, double>>> &agglomerated_points) {
     pair<int ,int> last_enemy_pos = this->enemyPosition;
-    pair<int, int> most_probable_average = Localization::getAveragePosition(agglomerated_points[0]);
     unsigned int most_probable_index = 0;
-    int best_distance = Localization::distanceBetween(most_probable_average, last_enemy_pos);
+    int best_distance = Localization::distanceBetween(Localization::getAveragePosition(agglomerated_points[0]), last_enemy_pos);
     for(unsigned int i=1; i<agglomerated_points.size(); i++){
         pair<int, int>agglomerate_average = Localization::getAveragePosition(agglomerated_points[i]);
         int distance = Localization::distanceBetween(agglomerate_average, last_enemy_pos);
         if(distance < best_distance){
             best_distance = distance;
-            most_probable_average = agglomerate_average;
             most_probable_index = i;
         }
     }
@@ -243,9 +181,70 @@ list<pair<double, double>> Localization::getMostProbableAgglomerate(vector<list<
 }
 
 
+vector<pair<double, double>> Localization::extractBeaconsMeasuredPoints(sl_lidar_response_measurement_node_hq_t nodes[NODES_LEN], size_t count) {
+    vector<pair<double, double>> nearestBeaconDetectedPointRelative(3, make_pair(-1, -1));
+    for (int pos = 0; pos < count; ++pos) {
+        //checking measurement quality
+        if(nodes[pos].quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT != 0){
+            pair<int, int> position = Localization::robotToCartesian(nodes[pos], this->x_robot, this->y_robot, this->alpha_robot);
+            //Select points next to beacons
+            if(this->getBeaconsMode()) {
+                int beaconNumber = this->getBeaconNumber(position);
+                if(beaconNumber != -1){
+                    if ((double) nodes[pos].dist_mm_q2 / 4.0f < nearestBeaconDetectedPointRelative[beaconNumber].first || nearestBeaconDetectedPointRelative[beaconNumber].first == -1) {
+                        double angle_radians_non_trigo = (((double) nodes[pos].angle_z_q14 * 90.f) / 16384.f) * 2.f * M_PI / 360.f;
+                        nearestBeaconDetectedPointRelative[beaconNumber] = make_pair((double) nodes[pos].dist_mm_q2 / 4.0f, angle_radians_non_trigo);
+                    }
+                }
+            }
+        }
+    }
+    return nearestBeaconDetectedPointRelative;
+}
+
+
+vector<int> Localization::determineRobotPosition(vector<pair<double, double>> beaconsDistanceAndAngleRelative, vector<bool> beaconsDetected) {
+    pair<int, int>* beaconsPos = this->beaconsPositions;
+    /* GETTING MEDIATORS CARTESIAN EQUATIONS */
+    list<pair<double, double>> circlesIntersectionsEquations;
+    if(beaconsDetected[0] && beaconsDetected[1]){
+        double a = (double)(beaconsPos[1].first - beaconsPos[0].first)/(beaconsPos[0].second - beaconsPos[1].second);
+        double b = (pow(beaconsDistanceAndAngleRelative[0].first + BEACONS_RADIUS, 2) + pow(beaconsDistanceAndAngleRelative[1].first + BEACONS_RADIUS, 2))/(2*(beaconsPos[0].second - beaconsPos[1].second));
+        circlesIntersectionsEquations.emplace_back(a, b);
+    }
+    if(beaconsDetected[1] && beaconsDetected[2]){
+        double a = (double)(beaconsPos[2].first - beaconsPos[1].first)/(beaconsPos[1].second - beaconsPos[2].second);
+        double b = (pow(beaconsDistanceAndAngleRelative[1].first + BEACONS_RADIUS, 2) + pow(beaconsDistanceAndAngleRelative[2].first + BEACONS_RADIUS, 2))/(2*(beaconsPos[1].second - beaconsPos[2].second));
+        circlesIntersectionsEquations.emplace_back(a, b);
+    }
+    if(beaconsDetected[2] && beaconsDetected[0]){
+        double a = (double)(beaconsPos[0].first - beaconsPos[2].first)/(beaconsPos[2].second - beaconsPos[0].second);
+        double b = (pow(beaconsDistanceAndAngleRelative[2].first + BEACONS_RADIUS, 2) + pow(beaconsDistanceAndAngleRelative[0].first + BEACONS_RADIUS, 2))/(2*(beaconsPos[2].second - beaconsPos[0].second));
+        circlesIntersectionsEquations.emplace_back(a, b);
+    }
+    list<pair<double, double>> intersections;
+    for (auto i = circlesIntersectionsEquations.begin(); i != prev(circlesIntersectionsEquations.end()); i++) {
+        for (auto j = next(i); j != circlesIntersectionsEquations.end(); j++) {
+            pair<double, double> intersection;
+            double x_intersect = ((*i).second - (*j).second) / ((*j).first - (*i).first);
+            intersection = make_pair(x_intersect, (*i).second - (*i).first * x_intersect);
+            intersections.emplace_back(intersection);
+        }
+    }
+    pair<int, int> robotPos = Localization::getAveragePosition(intersections);
+    //TODO : detect angle
+    int angle = -1;
+    vector<int> result;
+    result.push_back(robotPos.first);
+    result.push_back(robotPos.second);
+    result.push_back(angle);
+    return result;
+}
+
+
 void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[NODES_LEN], size_t count) {
     list<pair<double, double>> points_inside;
-    list<pair<double, double>> beacons_points[3];
+    pair<double, double> nearestBeaconDetectedPointRelative[3] = {make_pair(-1, -1)};
     bool proximityAlert = false;
     for (int pos = 0; pos < count; ++pos) {
         //checking measurement quality
@@ -254,7 +253,7 @@ void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[N
             if((double)nodes[pos].dist_mm_q2/4.0f < PROXIMITY_ALERT_RANGE) {
                 proximityAlert = true;
                 if (this->proximityLastRound) {
-                    int angle_radians = (int)((((double) nodes[pos].angle_z_q14 * 90.f) / 16384.f) * 2.f * M_PI / 360.f);
+                    int angle_radians = (int)(M_PI - (((double) nodes[pos].angle_z_q14 * 90.f) / 16384.f) * 2.f * M_PI / 360.f);
                     int angle_robot_base = angle_radians > 2*M_PI ? angle_radians - M_PI : angle_radians + M_PI;
                     this->sendProximityAlert((int) ((double) nodes[pos].dist_mm_q2 / 4.0f),angle_robot_base);
                 }
@@ -264,9 +263,12 @@ void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[N
             if (Localization::isInsideMap(position)) {
                 points_inside.emplace_back(position);
             } else {
-                int beaconNumber = this->getBeaconNumber(position);
-                if (beaconNumber != -1) {
-                    beacons_points[beaconNumber].emplace_back(position);
+                if(this->getBeaconsMode()){
+                    int beaconNumber = this->getBeaconNumber(position);
+                    if((double)nodes[pos].dist_mm_q2 / 4.0f < nearestBeaconDetectedPointRelative[beaconNumber].first || nearestBeaconDetectedPointRelative[beaconNumber].first == -1){
+                        double angle_radians_non_trigo = (((double) nodes[pos].angle_z_q14 * 90.f) / 16384.f) * 2.f * M_PI / 360.f;
+                        nearestBeaconDetectedPointRelative[beaconNumber] = make_pair((double)nodes[pos].dist_mm_q2 / 4.0f, angle_radians_non_trigo);
+                    }
                 }
             }
         }
@@ -277,27 +279,26 @@ void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[N
     list<pair<double, double>> ennemyAgglomerate = Localization::getMostProbableAgglomerate(agglomerated_points);
     pair<int,int> averageDetection = Localization::getAveragePosition(ennemyAgglomerate);
     int maxGap = Localization::getMaxGap(ennemyAgglomerate, averageDetection);
-    //Measure beacons shift
-    int xBeaconsShift = 0;
-    int yBeaconsShift = 0;
-    /*for (int i = 0; i < 3; i++) {
-        pair<int, int> beaconDetectedPosition = Localization::getCircleCenter(beacons_points[i], this->beaconsRadius);
-        int xShift = beaconDetectedPosition.first - this->beaconsPositions[i].first;
-        int yShift = beaconDetectedPosition.second - this->beaconsPositions[i].second;
-        xBeaconsShift = xBeaconsShift + xShift / 3;
-        yBeaconsShift = yBeaconsShift + yShift / 3;
-    }*/
-
-    //All writes at the same time to prevent inconsistent data
-    //TODO : appliquer la même transformation à la position ennemie que celle appliquée à la position du robot
-    //this->x_robot = this->x_robot + xBeaconsShift;
-    //this->y_robot = this->y_robot + yBeaconsShift;
+    //TODO estimate robot pos and compare it with robot pos in variables. If too much difference then send alert message
     this->enemyPosition = averageDetection;
     this->enemyPositionGap = maxGap;
     this->proximityLastRound = proximityAlert;
 }
 
 
+void Localization::processTriangulation(const vector<pair<double, double>>& overallNearestBeaconDetectedPointRelative) {
+    vector<bool> beaconsDetected(3, true);
+    for(unsigned int i = 0; i < 3; i++){
+        if(overallNearestBeaconDetectedPointRelative[i].first ==  -1){
+            beaconsDetected[i] = false;
+        }
+    }
+    vector<int> robotPos = this->determineRobotPosition(overallNearestBeaconDetectedPointRelative, beaconsDetected);
+    this->sendMessage("strat", "set pos", to_string(robotPos[0]) + ',' + to_string(robotPos[1]) + ',' + to_string(robotPos[2]));
+    this->triangulationMode = false;
+}
+
+
 void Localization::sendMessage(const string &recipient, const string &verb, const string &data) {
     this->TCPClient::sendMessage(("lidar;" + recipient + ";" + verb + ";" + data).c_str());
 }
@@ -321,9 +322,8 @@ void Localization::handleMessage(const std::string &message) {
             }
         }
         if (contains(verb, "get data")) {
-            vector<int> position = this->getRobotPosition();
+            //Get Data from robot
             vector<int> avoidance = this->getAvoidance();
-            this->sendMessage(issuer, "set position", to_string(position[0]) + "," + to_string(position[1]) + "," + to_string(position[2]));
             this->sendMessage(issuer, "set avoidance",to_string(avoidance[0]) + "," + to_string(avoidance[1]) + "," + to_string(avoidance[2]));
         }
         if (contains(verb, "start")) {
@@ -333,16 +333,24 @@ void Localization::handleMessage(const std::string &message) {
             this->started = false;
         }
         if (contains(verb, "set pos")) {
+            //Update robot position and orientation
             vector<string> position = split(data, ",");
             this->setRobotPosition(stoi(position[0]), stoi(position[1]), stoi(position[2]));
         }
         if (contains(verb, "set beacon mode")) {
+            //Enable or disable beacons triangulation
             this->setBeaconsMode(stoi(data));
         }
         if (contains(verb, "set beacon position")) {
+            //Update beacons position
             vector<string> position = split(data, ",");
             this->setRobotPosition(stoi(position[0]), stoi(position[1]), stoi(position[2]));
         }
+        if (contains(verb, "get pos")) {
+            //ONLY IF ROBOT IS STOPPED : measure robot position from multiple lidar runs.
+            this->setBeaconsMode(true);
+            this->triangulationMode = true;
+        }
     }
 }
 

src/localization.h

@@ -9,10 +9,12 @@
 #define BORDER_DETECT_TRIGGER 50
 #define AGGLOMERATES_TRIGGER 250
 #define BEACONS_RADIUS 50
+#define TRIANGULATION_ROUNDS 3
 
 #include <iostream>
-#include <cmath>
+#include <vector>
 #include <list>
+#include <cmath>
 #include <chrono>
 #include <ctime>
 #include <rplidar.h>
@@ -27,15 +29,14 @@ private:
     int x_robot;
     int y_robot;
     int alpha_robot;
-    int robotPositionGap = -1;
     pair<int, int> enemyPosition = make_pair(-1, -1);
     int enemyPositionGap = -1;
     pair<int, int> beaconsPositions[3];
-    int beaconsRadius = BEACONS_RADIUS;
     bool lidarHealth = false;
     bool started = false;
-    bool beaconsMode = false;
+    bool beaconsMode = true;
     bool proximityLastRound = false;
+    bool triangulationMode = false;
 public:
     Localization(int x_robot, int y_robot, int alpha_robot, const char* ip = "127.0.0.1", int port = 8080) : TCPClient(ip, port) {
         this->x_robot = x_robot;
@@ -46,20 +47,22 @@ public:
     void setRobotPosition(int x, int y, int alpha);
     void setBeaconsMode(bool state);
     [[nodiscard]] bool getLidarHealth() const;
-    [[nodiscard]] vector<int> getRobotPosition() const;
     [[nodiscard]] bool getBeaconsMode() const;
     [[nodiscard]] vector<int> getAvoidance() const;
     [[nodiscard]] bool isStarted() const;
+    [[nodiscard]] bool isTriangulating() const;
     static pair<double, double> robotToCartesian(sl_lidar_response_measurement_node_hq_t node, int x_robot, int y_robot, int alpha_robot);
     static int distanceBetween(pair<double, double> pos1, pair<double, double> pos2);
     static bool isInsideMap(pair<double, double> position);
     static pair<int, int> getAveragePosition(const list<pair<double, double>> &positions);
     static int getMaxGap(const list<pair<double, double>>& positionList, pair<int, int> referencePosition);
-    static pair<int, int> getCircleCenter(list<pair<double, double>> detectedPositions, int radius);
     static vector<list<pair<double, double>>> getAgglomerates(list<pair<double, double>> &positionsList);
     list<pair<double, double>> getMostProbableAgglomerate(vector<list<pair<double, double>>> &agglomerated_points);
+    vector<int> determineRobotPosition(vector<pair<double, double>> beaconsDistanceAndAngleRelative, vector<bool> beaconsDetected);
+    vector<pair<double, double>> extractBeaconsMeasuredPoints(sl_lidar_response_measurement_node_hq_t nodes[NODES_LEN], size_t count);
     int getBeaconNumber(pair<double, double> position);
     void processPoints(sl_lidar_response_measurement_node_hq_t[NODES_LEN], size_t count);
+    void processTriangulation(const vector<pair<double, double>>& overallNearestBeaconDetectedPointRelative);
     void handleMessage(const string &message) override;
     void sendMessage(const string &recipient, const string &verb, const string &data);
     void sendProximityAlert(int distance, int theta);

src/main.cpp

@@ -53,14 +53,42 @@ int main(int argc, char* argv[]) {
             }
             //start scanning
             drv->startScan(false, true);
+            bool alreadyInTriangulationMode = false;
+            vector<pair<double, double>> overallNearestBeaconDetectedPointRelative(3, make_pair(-1, -1));
+            unsigned int measurementCounter = 0;
             while(localizer.isStarted()) {
-                //get scan data
+                //Detect a first triangulation round
+                if(localizer.isTriangulating()){
+                    if(!alreadyInTriangulationMode){
+                        alreadyInTriangulationMode = true;
+                        fill(overallNearestBeaconDetectedPointRelative.begin(), overallNearestBeaconDetectedPointRelative.end(), make_pair(-1, -1));
+                        measurementCounter = 0;
+                    }
+                }
+                //Get scan data
                 sl_lidar_response_measurement_node_hq_t nodes[8192];
                 size_t count = get_size(nodes);
                 op_result = drv->grabScanDataHq(nodes, count);
                 if (SL_IS_OK(op_result)) {
                     drv->ascendScanData(nodes, count);
-                    localizer.processPoints(nodes, count);
+                    if(localizer.isTriangulating()){
+                        //Triangulation mode
+                        measurementCounter++;
+                        vector<pair<double, double>> nearestBeaconDetectedPointRelative = localizer.extractBeaconsMeasuredPoints(nodes, count);
+                        for(unsigned int i = 0; i<3; i++){
+                            if((nearestBeaconDetectedPointRelative[i].first < overallNearestBeaconDetectedPointRelative[i].first && nearestBeaconDetectedPointRelative[i].first != -1) || overallNearestBeaconDetectedPointRelative[i].first == -1){
+                                overallNearestBeaconDetectedPointRelative[i] = nearestBeaconDetectedPointRelative[i];
+                            }
+                        }
+                    } else {
+                        //Normal operation mode
+                        localizer.processPoints(nodes, count);
+                    }
+                }
+                //Detect a last triangulation round
+                if(measurementCounter == TRIANGULATION_ROUNDS){
+                    localizer.processTriangulation(overallNearestBeaconDetectedPointRelative);
+                    alreadyInTriangulationMode = false;
                 }
                 if (stop_signal_received) {
                     break;