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

rewrite to object oriented for definitive code (untested version)

Browse Source
This commit is contained in:
Allan Cueff
2024-03-25 01:34:14 +01:00
parent c1daf5418b
commit 80f38bfd9f
4 changed files with 199 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -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")

154
src/localization.cpp Normal file
View File

@@ -0,0 +1,154 @@
#include "localization.h"
pair<double, double> 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<double, double> position = make_pair(x_detected, y_detected);
return position;
}
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);
}
pair<int, int> Localization::averagePosition(list<pair<double, double>> 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<double, double> average = make_pair(x_average, y_average);
return average;
} else {
//TODO : pas de points => pas de moyenne
}
}
int Localization::getBeaconNumber(pair<double, double> 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<pair<double, double>> positionList, pair<int, int> 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<int, int> 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 {
//TODO : error
}
}
void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[NODES_LEN], int count){
list<pair<double, double>> points_inside;
list<pair<double, double>> beacons_points[3];
for (int pos = 0; pos < count; ++pos) {
pair<int, int> 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++){
pair<int, int>beaconDetectedPosition = 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<int, int> 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::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();
}

41
src/localization.h Normal file
View File

@@ -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 <iostream>
#include <cstdint>
#include <cmath>
#include <list>
#include <chrono>
#include <ctime>
#include <rplidar.h>
using namespace std;
class Localization {
private:
int x_robot;
int y_robot;
int alpha_robot;
int robotPositionGap;
pair<int, int> ennemyPosition;
int ennemyPositionGap;
uint64_t lastUpdate;
pair<int, int> beaconsPositions[3];
int beaconsRadius;
public:
pair<double, double> robotToCartesian(sl_lidar_response_measurement_node_hq_t node, int x_robot, int y_robot, int alpha_robot);
bool isInsideMap(pair<double, double> position);
pair<int, int> averagePosition(list<pair<double, double>> positions);
int getMaxGap(list<pair<double, double>> positionList, pair<int, int> referencePosition);
int getBeaconNumber(pair<double, double> position);
pair<int, int> getCircleCenter(list<pair<double, double>> detectedPositions, int radius);
void processPoints(sl_lidar_response_measurement_node_hq_t[NODES_LEN], int count);
};
#endif //LIDAR_LOCALIZATION_H

122
src/main.cpp
View File

@@ -1,20 +1,10 @@
#include <iostream>
#include <csignal>
#include <cmath>
#include <rplidar.h>
#include <list>
#include <chrono>
#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<sl::IChannel *> 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<pair<double, double>> 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<int, int> 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<pair<double, double>> 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<int, int> mediator_equation;
mediator_equation = make_pair(a, b);
mediators.emplace_back(mediator_equation);
}
}
cout << "Determined " << mediators.size() << " mediators" << endl;
/* GETTING MEDIATORS INTERSECTIONS COORDINATES */
list<pair<double, double>> intersections;
for (auto i = mediators.begin(); i != prev(mediators.end()); i++) {
for (auto j = next(i); j != mediators.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);
}
}
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::milliseconds>(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;