modelec/detection_adversaire
1
0
Fork 0
mirror of https://github.com/modelec/detection_adversaire.git synced 2026-01-18 16:17:23 +01:00
Code Issues Packages Projects Releases Wiki Activity

Functionnal ennemy position estimation

Browse Source
This commit is contained in:
Allan Cueff
2024-04-19 23:00:41 +02:00
parent a3ee256269
commit 67b9d2c4ae
2 changed files with 43 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
src/localization.cpp
View File

@@ -17,6 +17,11 @@ void Localization::setRobotPosition(int x, int y, int alpha) {
}
void Localization::setBeaconsMode(bool state){
this->beaconsMode = state;
}
vector<int> Localization::getRobotPosition() const {
vector<int> data;
data.push_back(this->x_robot);
@@ -26,6 +31,11 @@ vector<int> Localization::getRobotPosition() const {
}
bool Localization::getBeaconsMode() const{
return this->beaconsMode;
}
vector<int> Localization::getAvoidance() const {
vector<int> data;
data.push_back(this->enemyPosition.first);
@@ -67,7 +77,8 @@ int Localization::distanceBetween(pair<double, double> pos1, pair<double, double
bool Localization::isInsideMap(pair<double, double> 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<list<pair<double, double>>> Localization::getAgglomerates(list<pair<doubl
}
pair<int, int> Localization::getMostProbableAgglomerateAveragePos(vector<list<pair<double, double>>> &agglomerated_points) {
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);
for(unsigned int i=1; i<agglomerated_points.size(); i++){
pair<int, int>agglomerate_average = Localization::getAveragePosition(agglomerated_points[i]);
@@ -224,21 +236,28 @@ pair<int, int> Localization::getMostProbableAgglomerateAveragePos(vector<list<pa
if(distance < best_distance){
best_distance = distance;
most_probable_average = agglomerate_average;
most_probable_index = i;
}
}
return most_probable_average;
return agglomerated_points[most_probable_index];
}
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];
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<int, int> 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<list<pair<double, double>>> agglomerated_points = Localization::getAgglomerates(points_inside);
//Get most probable agglomerate average position
pair<int, int> averageDetection = Localization::getMostProbableAgglomerateAveragePos(agglomerated_points);
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;
@@ -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<string> 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<string> position = split(data, ",");
this->setRobotPosition(stoi(position[0]), stoi(position[1]), stoi(position[2]));
}
}
}

9
src/localization.h
View File

@@ -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 <iostream>
@@ -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<int> getRobotPosition() const;
[[nodiscard]] bool getBeaconsMode() const;
[[nodiscard]] vector<int> getAvoidance() const;
[[nodiscard]] bool isStarted() const;
static pair<double, double> 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<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);
pair<int, int> getMostProbableAgglomerateAveragePos(vector<list<pair<double, double>>> &agglomerated_points);
list<pair<double, double>> getMostProbableAgglomerate(vector<list<pair<double, double>>> &agglomerated_points);
int getBeaconNumber(pair<double, double> position);
void processPoints(sl_lidar_response_measurement_node_hq_t[NODES_LEN], size_t count);
void handleMessage(const string &message) override;