From 80f38bfd9f7683ba43f465e909268434e7ba7f55 Mon Sep 17 00:00:00 2001 From: Allan Cueff Date: Mon, 25 Mar 2024 01:34:14 +0100 Subject: [PATCH] rewrite to object oriented for definitive code (untested version) --- CMakeLists.txt | 2 +- src/localization.cpp | 154 +++++++++++++++++++++++++++++++++++++++++++ src/localization.h | 41 ++++++++++++ src/main.cpp | 122 +--------------------------------- 4 files changed, 199 insertions(+), 120 deletions(-) create mode 100644 src/localization.cpp create mode 100644 src/localization.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 8cf5af0..ef21c11 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED TRUE) SET(CMAKE_CXX_FLAGS -pthread) include_directories(include) -add_executable(${PROJECT_NAME} src/main.cpp) +add_executable(${PROJECT_NAME} src/main.cpp src/localization.h src/localization.cpp) set(RPLIDAR_SDK_PATH "/home/modelec/rplidar_sdk") diff --git a/src/localization.cpp b/src/localization.cpp new file mode 100644 index 0000000..743b1bd --- /dev/null +++ b/src/localization.cpp @@ -0,0 +1,154 @@ +#include "localization.h" + + +pair Localization::robotToCartesian(sl_lidar_response_measurement_node_hq_t node, int x_robot, int y_robot, int alpha_robot){ + double alpha_lidar = 2.f * M_PI - (double) node.angle_z_q14 * 0.5f * M_PI / 16384.f; + double alpha = alpha_robot + alpha_lidar; + if (alpha > 2.f * M_PI) { + alpha = alpha - 2.f * M_PI; + } else if (alpha < 0) { + alpha = alpha + 2.f * M_PI; + } + double x_detected = x_robot + (double) node.dist_mm_q2 / 4.0f * cos(alpha); + double y_detected = y_robot + (double) node.dist_mm_q2 / 4.0f * sin(alpha); + pair position = make_pair(x_detected, y_detected); + return position; +} + + +bool Localization::isInsideMap(pair pos){ + return (pos.first < MAX_TABLE_X && pos.first > 0.f && pos.second < MAX_TABLE_Y && pos.second > 0.f); +} + + +pair Localization::averagePosition(list> positions){ + double total_x = 0; + double total_y = 0; + int n = 0; + for (auto iter = positions.begin(); iter != positions.end(); iter++) { + n++; + total_x += (*iter).first; + total_y += (*iter).second; + } + if (n > 0) { + int x_average = (int) total_x / n; + int y_average = (int) total_y / n; + pair average = make_pair(x_average, y_average); + return average; + } else { + //TODO : pas de points => pas de moyenne + } +} + + +int Localization::getBeaconNumber(pair position){ + for(int n=0; n<3; n++){ + if(sqrt(pow(this->beaconsPositions[n].first - position.first, 2) + pow(this->beaconsPositions[n].second - position.second, 2)) < BEACON_DETECT_RANGE){ + return n; + } + } + return -1; +} + + +int getMaxGap(list> positionList, pair referencePosition){ + int maxGap = 0; + for (auto iter = positionList.begin(); iter != positionList.end(); iter++) { + int gap = sqrt(pow((*iter).first - referencePosition.first, 2) + pow((*iter).second - referencePosition.second, 2)); + if(gap > maxGap){ + maxGap = gap; + } + } + return maxGap; +} + + +pair getCircleCenter(list> detectedPositions, int radius){ + // Implementing https://math.stackexchange.com/a/1781546 + list> 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 { + //TODO : error + } +} + + +void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[NODES_LEN], int count){ + list> points_inside; + list> beacons_points[3]; + for (int pos = 0; pos < count; ++pos) { + pair position = this->robotToCartesian(nodes[pos], this->x_robot, this->y_robot, this->alpha_robot); + if(this->isInsideMap(position)){ + points_inside.emplace_back(position); + } else { + int beaconNumber = this->getBeaconNumber(position); + if(beaconNumber != -1){ + beacons_points[beaconNumber].emplace_back(position); + } + } + } + int xBeaconsShift = 0; + int yBeaconsShift = 0; + for(int i=0; i<3; i++){ + pairbeaconDetectedPosition = this->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; + } + //TODO remove false detections + //TODO check agglomeration of points and keep only the one next to the ennemy previous position => replace points_inside by agglomerate points in next lines + pair averageDetection = this->averagePosition(points_inside); + int maxGap = this->getMaxGap(points_inside, averageDetection); + //All writes at the same time to prevent inconsistent data + this->x_robot = this->x_robot + xBeaconsShift; + this->y_robot = this->y_robot + yBeaconsShift; + this->ennemyPosition = averageDetection; + this->ennemyPositionGap = maxGap; + this->lastUpdate = (uint64_t)chrono::duration_cast(chrono::system_clock::now().time_since_epoch()).count(); +} \ No newline at end of file diff --git a/src/localization.h b/src/localization.h new file mode 100644 index 0000000..85f8e32 --- /dev/null +++ b/src/localization.h @@ -0,0 +1,41 @@ +#ifndef LIDAR_LOCALIZATION_H +#define LIDAR_LOCALIZATION_H + +#define NODES_LEN 8192 +#define MAX_TABLE_X 3000 +#define MAX_TABLE_Y 2000 +#define BEACON_DETECT_RANGE 100 + +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +class Localization { +private: + int x_robot; + int y_robot; + int alpha_robot; + int robotPositionGap; + pair ennemyPosition; + int ennemyPositionGap; + uint64_t lastUpdate; + pair beaconsPositions[3]; + int beaconsRadius; +public: + pair robotToCartesian(sl_lidar_response_measurement_node_hq_t node, int x_robot, int y_robot, int alpha_robot); + bool isInsideMap(pair position); + pair averagePosition(list> positions); + int getMaxGap(list> positionList, pair referencePosition); + int getBeaconNumber(pair position); + pair getCircleCenter(list> detectedPositions, int radius); + void processPoints(sl_lidar_response_measurement_node_hq_t[NODES_LEN], int count); +}; + + +#endif //LIDAR_LOCALIZATION_H \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 90a5c14..8f5ad4e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,20 +1,10 @@ -#include #include -#include -#include -#include -#include +#include "localization.h" #ifndef get_size #define get_size(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) #endif -#define X_ROBOT 1000.f -#define Y_ROBOT 1000.f -#define ALPHA_ROBOT 0.f -#define MAX_X 3000.f -#define MAX_Y 2000.f - bool stop_signal_received; void stop_loop(int) { @@ -24,6 +14,7 @@ void stop_loop(int) { using namespace std; int main() { + Localization localizer; sl::Result channel = sl::createSerialPortChannel("/dev/ttyUSB0", 115200); sl::ILidarDriver *drv = *sl::createLidarDriver(); auto res = drv->connect(*channel); @@ -37,114 +28,7 @@ int main() { op_result = drv->grabScanDataHq(nodes, count); if (SL_IS_OK(op_result)) { drv->ascendScanData(nodes, count); - /* GETTING POINTS INSIDE OF MAP ONLY */ - list> points_inside; - for (int pos = 0; pos < (int) count; ++pos) { - double alpha_lidar = 2.f * M_PI - (double) nodes[pos].angle_z_q14 * 0.5f * M_PI / 16384.f; - double alpha = ALPHA_ROBOT + alpha_lidar; - if (alpha > 2.f * M_PI) { - alpha = alpha - 2.f * M_PI; - } else if (alpha < 0) { - alpha = alpha + 2.f * M_PI; - } - double x_detected = X_ROBOT + (double) nodes[pos].dist_mm_q2 / 4.0f * cos(alpha); - double y_detected = Y_ROBOT + (double) nodes[pos].dist_mm_q2 / 4.0f * sin(alpha); - if (x_detected < MAX_X && x_detected > 0.f && y_detected < MAX_Y && y_detected > 0.f && - nodes[pos].quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT != 0) { - pair position_detected; - position_detected = make_pair(x_detected, y_detected); - points_inside.emplace_back(position_detected); - } - } - /* FILTERING POINTS */ - auto it = points_inside.begin(); - double min_x = 0; - double max_x = 0; - double min_y = 0; - double max_y = 0; - while (it != points_inside.end()) { - double distance_from_prev; - double distance_to_next; - if (it == points_inside.begin()) { - distance_from_prev = sqrt(pow((*prev(points_inside.end())).first - (*it).first, 2) + - pow((*prev(points_inside.end())).second - (*it).second, 2)); - distance_to_next = sqrt( - pow((*next(it)).first - (*it).first, 2) + pow((*next(it)).second - (*it).second, 2)); - } else if (it == prev(points_inside.end())) { - distance_from_prev = sqrt( - pow((*prev(it)).first - (*it).first, 2) + pow((*prev(it)).second - (*it).second, 2)); - distance_to_next = sqrt(pow((*points_inside.begin()).first - (*it).first, 2) + - pow((*points_inside.begin()).second - (*it).second, 2)); - } else { - distance_from_prev = sqrt( - pow((*prev(it)).first - (*it).first, 2) + pow((*prev(it)).second - (*it).second, 2)); - distance_to_next = sqrt( - pow((*next(it)).first - (*it).first, 2) + pow((*next(it)).second - (*it).second, 2)); - } - if (distance_from_prev > 100.f && distance_to_next > 100.f) { - cout << "False detection : x : " << (*it).first << " y : " << (*it).second << endl; - points_inside.erase(it++); - } else { - if (min_x > (*it).first || min_x == 0) { - min_x = (*it).first; - } - if (max_x < (*it).first || max_x == 0) { - max_x = (*it).first; - } - if (min_y > (*it).second || min_y == 0) { - min_y = (*it).second; - } - if (max_y < (*it).second || max_y == 0) { - max_y = (*it).second; - } - it++; - } - } - cout << "Detected " << points_inside.size() << " correct points this round max_x " << max_x << " max_y " - << max_y << " min_x " << min_x << " min_y " << min_y << endl; - /* GETTING MEDIATORS CARTESIAN EQUATIONS */ - list> mediators; - for (auto i = points_inside.begin(); i != prev(points_inside.end()); i++) { - for (auto j = next(i); j != points_inside.end(); j++) { - double x_middle = ((*i).first + (*j).first) / 2.f; - double y_middle = ((*i).second + (*j).second) / 2.f; - double a = ((*i).first - (*j).first) / ((*j).second - (*i).second); - double b = y_middle - a * x_middle; - pair mediator_equation; - mediator_equation = make_pair(a, b); - mediators.emplace_back(mediator_equation); - } - } - cout << "Determined " << mediators.size() << " mediators" << endl; - /* GETTING MEDIATORS INTERSECTIONS COORDINATES */ - list> intersections; - for (auto i = mediators.begin(); i != prev(mediators.end()); i++) { - for (auto j = next(i); j != mediators.end(); j++) { - pair 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); - } - } - cout << "Calculated " << intersections.size() << " intersections" << endl; - /* ESTIMATING ENEMY POS */ - double total_x = 0; - double total_y = 0; - unsigned int n = 0; - for (auto iter = intersections.begin(); iter != intersections.end(); iter++) { - if ((*iter).first > min_x && (*iter).first < max_x && (*iter).second > min_y && - (*iter).second < max_y) { - n++; - total_x += (*iter).first; - total_y += (*iter).second; - } - } - if (n > 0) { - cout << "Detected enemy position : x : " << total_x / (double) n << " y : " << total_y / (double) n - << " from " << n << " points at " << chrono::duration_cast(chrono::high_resolution_clock::now().time_since_epoch()).count() << "ms since epoch" << endl; - } else { - cout << "Could not precisely detect enemy pos"; - } + localizer.processPoints(nodes, count); } if (stop_signal_received) { break;