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

evol Serge pilotable

Browse Source
This commit is contained in:
Allan Cueff
2024-05-16 01:04:13 +02:00
parent da3cd388ca
commit f3d4494b08
2 changed files with 45 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
src/localization.cpp
View File

@@ -37,11 +37,21 @@ void Localization::setBeaconsMode(bool state){
}
void Localization::setTableMode(bool state){
this->tableMode = state;
}
bool Localization::getBeaconsMode() const{
return this->beaconsMode;
}
bool Localization::getTableMode() const{
return this->tableMode;
}
vector<int> Localization::getAvoidance() const {
vector<int> data;
data.push_back(this->enemyPosition.first);
@@ -313,9 +323,9 @@ void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[N
if(nodes[pos].quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT != 0){
//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);
if (Localization::isInsideMap(position)) {
if (Localization::isInsideMap(position) || !this->getTableMode()) {
//Checking for direct proximity
if((double)nodes[pos].dist_mm_q2/4.0f < PROXIMITY_ALERT_RANGE) {
if((double)nodes[pos].dist_mm_q2/4.0f < this->proximityAlertRange) {
proximityAlert = true;
if (this->proximityLastRound) {
if(proximityValues.first == -1 || proximityValues.first > (int) ((double) nodes[pos].dist_mm_q2 / 4.0f)){
@@ -344,24 +354,26 @@ void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[N
if(this->proximityLastRound && proximityAlert){
this->sendProximityAlert(proximityValues.first, proximityValues.second);
}
//Get agglomerates without solo points
vector<list<pair<double, double>>> agglomerated_points = Localization::getAgglomerates(points_inside);
//Get most probable agglomerate average position
list<pair<double, double>> ennemyAgglomerate = Localization::getMostProbableAgglomerate(agglomerated_points);
pair<int,int> averageDetection = Localization::getAveragePosition(ennemyAgglomerate);
int maxGap = Localization::getMaxGap(ennemyAgglomerate, averageDetection);
this->enemyPosition = averageDetection;
this->enemyPositionGap = maxGap;
//Determine approximative robot position from beacons
if(this->getBeaconsMode()){
vector<int> robotPos = this->determineRobotPosition(nearestBeaconDetectedPointRelative, beaconsDetected);
pair<int, int> robotMeasuredPos{robotPos[0], robotPos[1]};
pair<int, int> robotOdometryPos{this->x_robot, this->y_robot};
int positionSwitch = Localization::distanceBetween(robotMeasuredPos, robotOdometryPos);
if(positionSwitch > POSITION_CORRECT_RANGE){
positionIncorrect = true;
if(this->positionIncorrectLastRound){
this->sendMessage("strat", "stop recalibrate", to_string(positionSwitch));
if(this->getTableMode()) {
//Get agglomerates without solo points
vector<list<pair<double, double>>> agglomerated_points = Localization::getAgglomerates(points_inside);
//Get most probable agglomerate average position
list<pair<double, double>> ennemyAgglomerate = Localization::getMostProbableAgglomerate(agglomerated_points);
pair<int, int> averageDetection = Localization::getAveragePosition(ennemyAgglomerate);
int maxGap = Localization::getMaxGap(ennemyAgglomerate, averageDetection);
this->enemyPosition = averageDetection;
this->enemyPositionGap = maxGap;
//Determine approximative robot position from beacons
if (this->getBeaconsMode()) {
vector<int> robotPos = this->determineRobotPosition(nearestBeaconDetectedPointRelative, beaconsDetected);
pair<int, int> robotMeasuredPos{robotPos[0], robotPos[1]};
pair<int, int> robotOdometryPos{this->x_robot, this->y_robot};
int positionSwitch = Localization::distanceBetween(robotMeasuredPos, robotOdometryPos);
if (positionSwitch > POSITION_CORRECT_RANGE) {
positionIncorrect = true;
if (this->positionIncorrectLastRound) {
this->sendMessage("strat", "stop recalibrate", to_string(positionSwitch));
}
}
}
}
@@ -433,6 +445,11 @@ void Localization::handleMessage(const std::string &message) {
//Enable or disable beacons triangulation. This must be put to 0 if no beacons are on the table
this->setBeaconsMode(stoi(data));
}
if (contains(verb, "set table")) {
//Enable or disable table mode. Set this to 0 to use the robot in another environment than the match table
this->setTableMode(stoi(data));
this->setBeaconsMode(false);
}
if (contains(verb, "set team")) {
//Update beacons position
if(stoi(data) == 0){
@@ -443,6 +460,10 @@ void Localization::handleMessage(const std::string &message) {
this->setBeaconsPosition(positions);
}
}
if (contains(verb, "set range")) {
//Updates proximity alert range
this->proximityAlertRange = stoi(data);
}
if (contains(verb, "get pos")) {
//ONLY IF ROBOT IS STOPPED : measure robot position from multiple lidar runs.
this->setBeaconsMode(true);

5
src/localization.h
View File

@@ -5,7 +5,6 @@
#define MAX_TABLE_X 3000 //Attention : mode demi table
#define MAX_TABLE_Y 2000
#define BEACON_DETECT_RANGE 100
#define PROXIMITY_ALERT_RANGE 450
#define BORDER_DETECT_TRIGGER 50
#define AGGLOMERATES_TRIGGER 250
#define BEACONS_RADIUS 50
@@ -39,9 +38,11 @@ private:
pair<int, int> enemyPosition = make_pair(-1, -1);
int enemyPositionGap = -1;
pair<int, int> beaconsPositions[3];
int proximityAlertRange = 450;
bool lidarHealth = false;
bool started = false;
bool beaconsMode = true;
bool tableMode = true;
bool proximityLastRound = false;
bool positionIncorrectLastRound = false;
bool triangulationMode = false;
@@ -53,10 +54,12 @@ public:
};
void setLidarHealth(bool ok);
void setRobotPosition(int x, int y, int alpha);
void setTableMode(bool state);
void setBeaconsMode(bool state);
void setBeaconsPosition(pair<int, int> positions[3]);
[[nodiscard]] bool getLidarHealth() const;
[[nodiscard]] bool getBeaconsMode() const;
[[nodiscard]] bool getTableMode() const;
[[nodiscard]] vector<int> getAvoidance() const;
[[nodiscard]] bool isStarted() const;
[[nodiscard]] bool isTriangulating() const;