From f3d4494b08cf42104b6bf56b0e44c8154e6c3c9e Mon Sep 17 00:00:00 2001 From: Allan Cueff Date: Thu, 16 May 2024 01:04:13 +0200 Subject: [PATCH] evol Serge pilotable --- src/localization.cpp | 61 +++++++++++++++++++++++++++++--------------- src/localization.h | 5 +++- 2 files changed, 45 insertions(+), 21 deletions(-) diff --git a/src/localization.cpp b/src/localization.cpp index d65a08d..4fdd26e 100644 --- a/src/localization.cpp +++ b/src/localization.cpp @@ -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 Localization::getAvoidance() const { vector 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 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>> agglomerated_points = Localization::getAgglomerates(points_inside); - //Get most probable agglomerate average position - list> ennemyAgglomerate = Localization::getMostProbableAgglomerate(agglomerated_points); - pair 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 robotPos = this->determineRobotPosition(nearestBeaconDetectedPointRelative, beaconsDetected); - pair robotMeasuredPos{robotPos[0], robotPos[1]}; - pair 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>> agglomerated_points = Localization::getAgglomerates(points_inside); + //Get most probable agglomerate average position + list> ennemyAgglomerate = Localization::getMostProbableAgglomerate(agglomerated_points); + pair 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 robotPos = this->determineRobotPosition(nearestBeaconDetectedPointRelative, beaconsDetected); + pair robotMeasuredPos{robotPos[0], robotPos[1]}; + pair 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); diff --git a/src/localization.h b/src/localization.h index b272d33..8bb94dd 100644 --- a/src/localization.h +++ b/src/localization.h @@ -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 enemyPosition = make_pair(-1, -1); int enemyPositionGap = -1; pair 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 positions[3]); [[nodiscard]] bool getLidarHealth() const; [[nodiscard]] bool getBeaconsMode() const; + [[nodiscard]] bool getTableMode() const; [[nodiscard]] vector getAvoidance() const; [[nodiscard]] bool isStarted() const; [[nodiscard]] bool isTriangulating() const;