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

code détection angle robot

Browse Source
This commit is contained in:
Allan Cueff
2024-05-07 23:51:54 +02:00
parent 3f56d17aef
commit cc543f3b68
2 changed files with 31 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
src/localization.cpp
View File

@@ -214,13 +214,31 @@ vector<pair<double,double>> Localization::intersectionBetweenCircles(pair<double
pair<double,double> Localization::intersectionBetweenLines(pair<double,double> l1, pair<double,double> l2){
pair<double, double> intersection;
double x_intersect = (l2.second - l1.second) / (l1.first - l2.first);
double y_intersect = l1.first * x_intersect + l1.second;
return make_pair(x_intersect, y_intersect);
}
int Localization::determineRobotOrientation(pair<int,int> robotPos, pair<int,int> beaconPos, int beaconOrientation){
int angle = -1;
if(beaconPos.first <= robotPos.first && beaconPos.second <= robotPos.second){
angle = (int)(100*(double)atan(robotPos.first - beaconPos.first));
} else if(beaconPos.first <= robotPos.first && beaconPos.second >= robotPos.second){
angle = (int)(100*(M_PI/2 + (double)atan(beaconPos.second - robotPos.second)));
} else if(beaconPos.first >= robotPos.first && beaconPos.second >= robotPos.second){
angle = (int)(100*(M_PI+(double)atan(beaconPos.first - robotPos.first)));
} else if(beaconPos.first >= robotPos.first && beaconPos.second <= robotPos.second){
angle = (int)(100*(3*M_PI/2+(double)atan(robotPos.second - beaconPos.second)));
}
int alpha = angle - beaconOrientation;
if(alpha < 0){
alpha = (int)(alpha + 200*M_PI);
}
return alpha;
}
list<pair<double, double>> Localization::getMostProbableAgglomerate(vector<list<pair<double, double>>> &agglomerated_points) {
pair<int ,int> last_enemy_pos = this->enemyPosition;
unsigned int most_probable_index = 0;
@@ -262,8 +280,6 @@ vector<pair<double, int>> Localization::extractBeaconsMeasuredPoints(sl_lidar_re
vector<int> Localization::determineRobotPosition(vector<pair<double, int>> beaconsDistanceAndAngleRelative, vector<bool> beaconsDetected) {
pair<int, int>* beaconsPos = this->beaconsPositions;
for(unsigned int i=0; i<3; i++){
}
list<pair<double, double>> circlesIntersectionsEquations;
if(beaconsDetected[0] && beaconsDetected[1]){
vector<pair<double,double>> intersectCircles = Localization::intersectionBetweenCircles(beaconsPos[0], beaconsDistanceAndAngleRelative[0].first + (double)BEACONS_RADIUS, beaconsPos[1], beaconsDistanceAndAngleRelative[1].first + (double)BEACONS_RADIUS);
@@ -288,7 +304,17 @@ vector<int> Localization::determineRobotPosition(vector<pair<double, int>> beaco
}
}
pair<int, int> robotPos = Localization::getAveragePosition(intersections);
int angle = -1;
int n = 0;
int angle = 0;
for(unsigned int i=0; i<3; i++){
if(beaconsDetected[i]){
n++;
angle += Localization::determineRobotOrientation(robotPos, beaconsPos[i], beaconsDistanceAndAngleRelative[i].second);
}
}
if(n != 0){
angle = angle/n;
}
vector<int> result{robotPos.first, robotPos.second, angle};
return result;
}

1
src/localization.h
View File

@@ -66,6 +66,7 @@ public:
static pair<double, double> lineEquationFromPoints(pair<double, double> p1, pair<double, double> p2);
static vector<pair<double,double>> intersectionBetweenCircles(pair<double,double> c1, double r1, pair<double,double> c2, double r2);
static pair<double,double> intersectionBetweenLines(pair<double,double> l1, pair<double,double> l2);
static int determineRobotOrientation(pair<int, int> robotPos, pair<int, int> beaconPos, int beaconOrientation);
list<pair<double, double>> getMostProbableAgglomerate(vector<list<pair<double, double>>> &agglomerated_points);
vector<int> determineRobotPosition(vector<pair<double, int>> beaconsDistanceAndAngleRelative, vector<bool> beaconsDetected);
vector<pair<double, int>> extractBeaconsMeasuredPoints(sl_lidar_response_measurement_node_hq_t nodes[NODES_LEN], size_t count);