From 67b9d2c4aed70f0c0363466231d5706fc40787b0 Mon Sep 17 00:00:00 2001 From: Allan Cueff Date: Fri, 19 Apr 2024 23:00:41 +0200 Subject: [PATCH] Functionnal ennemy position estimation --- src/localization.cpp | 43 ++++++++++++++++++++++++++++++++++++------- src/localization.h | 9 +++++++-- 2 files changed, 43 insertions(+), 9 deletions(-) diff --git a/src/localization.cpp b/src/localization.cpp index 20c03c3..2d19068 100644 --- a/src/localization.cpp +++ b/src/localization.cpp @@ -17,6 +17,11 @@ void Localization::setRobotPosition(int x, int y, int alpha) { } +void Localization::setBeaconsMode(bool state){ + this->beaconsMode = state; +} + + vector Localization::getRobotPosition() const { vector data; data.push_back(this->x_robot); @@ -26,6 +31,11 @@ vector Localization::getRobotPosition() const { } +bool Localization::getBeaconsMode() const{ + return this->beaconsMode; +} + + vector Localization::getAvoidance() const { vector data; data.push_back(this->enemyPosition.first); @@ -67,7 +77,8 @@ int Localization::distanceBetween(pair pos1, pair pos) { - return (pos.first < MAX_TABLE_X && pos.first > 0.f && pos.second < MAX_TABLE_Y && pos.second > 0.f); + //Trigger used here to prevent border false detections due to + return (pos.first + BORDER_DETECT_TRIGGER < MAX_TABLE_X && pos.first - BORDER_DETECT_TRIGGER > 0.f && pos.second + BORDER_DETECT_TRIGGER < MAX_TABLE_Y && pos.second - BORDER_DETECT_TRIGGER > 0.f); } @@ -214,9 +225,10 @@ vector>> Localization::getAgglomerates(list Localization::getMostProbableAgglomerateAveragePos(vector>> &agglomerated_points) { +list> Localization::getMostProbableAgglomerate(vector>> &agglomerated_points) { pair last_enemy_pos = this->enemyPosition; pair 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); for(unsigned int i=1; iagglomerate_average = Localization::getAveragePosition(agglomerated_points[i]); @@ -224,21 +236,28 @@ pair Localization::getMostProbableAgglomerateAveragePos(vector> points_inside; list> beacons_points[3]; + bool proximityAlert = false; for (int pos = 0; pos < count; ++pos) { //checking measurement quality if(nodes[pos].quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT != 0){ //Checking for direct proximity - if((double)nodes[pos].dist_mm_q2/4.0f < PROXIMITY_ALERT_RANGE){ - this->sendProximityAlert((int)((double)nodes[pos].dist_mm_q2/4.0f), (int)(((double)nodes[pos].angle_z_q14 * 90.f) / 16384.f)); + 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_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); + } } //Select points inside map and next to beacons pair position = Localization::robotToCartesian(nodes[pos], this->x_robot, this->y_robot, this->alpha_robot); @@ -255,7 +274,9 @@ void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[N //Get agglomerates without solo points vector>> agglomerated_points = Localization::getAgglomerates(points_inside); //Get most probable agglomerate average position - pair averageDetection = Localization::getMostProbableAgglomerateAveragePos(agglomerated_points); + list> ennemyAgglomerate = Localization::getMostProbableAgglomerate(agglomerated_points); + pair averageDetection = Localization::getAveragePosition(ennemyAgglomerate); + int maxGap = Localization::getMaxGap(ennemyAgglomerate, averageDetection); //Measure beacons shift int xBeaconsShift = 0; int yBeaconsShift = 0; @@ -267,12 +288,13 @@ void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[N yBeaconsShift = yBeaconsShift + yShift / 3; }*/ - int maxGap = Localization::getMaxGap(points_inside, averageDetection); //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; this->enemyPosition = averageDetection; this->enemyPositionGap = maxGap; + this->proximityLastRound = proximityAlert; } @@ -314,6 +336,13 @@ void Localization::handleMessage(const std::string &message) { vector position = split(data, ","); this->setRobotPosition(stoi(position[0]), stoi(position[1]), stoi(position[2])); } + if (contains(verb, "set beacon mode")) { + this->setBeaconsMode(stoi(data)); + } + if (contains(verb, "set beacon position")) { + vector position = split(data, ","); + this->setRobotPosition(stoi(position[0]), stoi(position[1]), stoi(position[2])); + } } } diff --git a/src/localization.h b/src/localization.h index f8ac357..9da1884 100644 --- a/src/localization.h +++ b/src/localization.h @@ -6,7 +6,8 @@ #define MAX_TABLE_Y 2000 #define BEACON_DETECT_RANGE 100 #define PROXIMITY_ALERT_RANGE 250 -#define AGGLOMERATES_TRIGGER 100 +#define BORDER_DETECT_TRIGGER 50 +#define AGGLOMERATES_TRIGGER 250 #define BEACONS_RADIUS 50 #include @@ -33,6 +34,8 @@ private: int beaconsRadius = BEACONS_RADIUS; bool lidarHealth = false; bool started = false; + bool beaconsMode = false; + bool proximityLastRound = 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; @@ -41,8 +44,10 @@ public: }; void setLidarHealth(bool ok); void setRobotPosition(int x, int y, int alpha); + void setBeaconsMode(bool state); [[nodiscard]] bool getLidarHealth() const; [[nodiscard]] vector getRobotPosition() const; + [[nodiscard]] bool getBeaconsMode() const; [[nodiscard]] vector getAvoidance() const; [[nodiscard]] bool isStarted() const; static pair robotToCartesian(sl_lidar_response_measurement_node_hq_t node, int x_robot, int y_robot, int alpha_robot); @@ -52,7 +57,7 @@ public: static int getMaxGap(const list>& positionList, pair referencePosition); static pair getCircleCenter(list> detectedPositions, int radius); static vector>> getAgglomerates(list> &positionsList); - pair getMostProbableAgglomerateAveragePos(vector>> &agglomerated_points); + list> getMostProbableAgglomerate(vector>> &agglomerated_points); int getBeaconNumber(pair position); void processPoints(sl_lidar_response_measurement_node_hq_t[NODES_LEN], size_t count); void handleMessage(const string &message) override;