From dcf0a58e6f38113615722ca5b8a7457efa37c03d Mon Sep 17 00:00:00 2001 From: Allan Cueff Date: Wed, 8 May 2024 14:34:17 +0200 Subject: [PATCH 1/6] =?UTF-8?q?ajout=20lox=20proximit=C3=A9=20+=20envoi=20?= =?UTF-8?q?d'alerte=20une=20seule=20fois=20par=20round?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/localization.cpp | 36 +++++++++++++++++++++++++----------- 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/src/localization.cpp b/src/localization.cpp index 3b992aa..5c40af6 100644 --- a/src/localization.cpp +++ b/src/localization.cpp @@ -308,20 +308,27 @@ void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[N vector beaconsDetected(3, false); bool proximityAlert = false; bool positionIncorrect = false; + pair proximityValues = {-1, -1}; 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) { - proximityAlert = true; - if (this->proximityLastRound) { - int angle_radians = Localization::rplidarToTrigoRadians(((double) nodes[pos].angle_z_q14 * 90.f) / 16384.f); - this->sendProximityAlert((int) ((double) nodes[pos].dist_mm_q2 / 4.0f), angle_radians); - } - } //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)) { + //Checking for direct proximity + if((double)nodes[pos].dist_mm_q2/4.0f < PROXIMITY_ALERT_RANGE) { + proximityAlert = true; + if (this->proximityLastRound) { + if(proximityValues.first == -1 || proximityValues.first > (int) ((double) nodes[pos].dist_mm_q2 / 4.0f)){ + proximityValues.first = (int) ((double) nodes[pos].dist_mm_q2 / 4.0f); + proximityValues.second = Localization::rplidarToTrigoRadians(((double) nodes[pos].angle_z_q14 * 90.f) / 16384.f);; + } + } + #ifdef LIDAR_LOG_DEBUG_MODE + long epoch = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + cout << "### localization.cpp:processPoints:329:T" << epoch << " >> detected proximity point - distance=" << proximityValues.first << " angle=" << proximityValues.second << endl; + #endif + } points_inside.emplace_back(position); } else { if (this->getBeaconsMode()) { @@ -335,6 +342,9 @@ 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 @@ -370,7 +380,7 @@ void Localization::processTriangulation(const vector>& overall } #ifdef LIDAR_LOG_DEBUG_MODE long epoch = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - cout << "### localization.cpp:processTriangulation:373:T" << epoch << " >> detected beacons >> " << beaconsDetected[0] << ";" << beaconsDetected[1] << ";" << beaconsDetected[2] << endl; + cout << "### localization.cpp:processTriangulation:383:T" << epoch << " >> detected beacons >> " << beaconsDetected[0] << ";" << beaconsDetected[1] << ";" << beaconsDetected[2] << endl; #endif vector robotPos = this->determineRobotPosition(overallNearestBeaconDetectedPointRelative, beaconsDetected); this->sendMessage("strat", "set pos", to_string(robotPos[0]) + ',' + to_string(robotPos[1]) + ',' + to_string(robotPos[2])); @@ -416,7 +426,7 @@ void Localization::handleMessage(const std::string &message) { vector position = split(data, ","); #ifdef LIDAR_LOG_DEBUG_MODE long epoch = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - cout << "### localization.cpp:handleMessage:419:T" << epoch << " >> got position TCP message >> " << position[0] << ";" << position[1] << ";" << position[2] << endl; + cout << "### localization.cpp:handleMessage:429:T" << epoch << " >> got position TCP message >> " << position[0] << ";" << position[1] << ";" << position[2] << endl; #endif this->setRobotPosition(stoi(position[0]), stoi(position[1]), stoi(position[2])); } @@ -440,7 +450,7 @@ void Localization::handleMessage(const std::string &message) { this->triangulationMode = true; #ifdef LIDAR_LOG_DEBUG_MODE long epoch = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - cout << "### localization.cpp:handleMessage:443:T" << epoch << " >> got triangulation TCP message" << endl; + cout << "### localization.cpp:handleMessage:453:T" << epoch << " >> got triangulation TCP message" << endl; #endif } } @@ -449,4 +459,8 @@ void Localization::handleMessage(const std::string &message) { void Localization::sendProximityAlert(int distance, int angle) { this->sendMessage("all", "stop proximity", to_string(distance) + "," + to_string(angle)); + #ifdef LIDAR_LOG_DEBUG_MODE + long epoch = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + cout << "### localization.cpp:sendProximityAlert:464:T" << epoch << " >> sent proximity message - distance=" << distance << " angle=" << angle << endl; + #endif } From 852e5b63ea3dfc32c930e27d17ab080366d5e909 Mon Sep 17 00:00:00 2001 From: Allan Cueff Date: Wed, 8 May 2024 17:03:51 +0200 Subject: [PATCH 2/6] valeurs grande table --- src/localization.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/localization.h b/src/localization.h index 703938a..4ce651d 100644 --- a/src/localization.h +++ b/src/localization.h @@ -2,7 +2,7 @@ #define LIDAR_LOCALIZATION_H #define NODES_LEN 8192 -#define MAX_TABLE_X 1500 +#define MAX_TABLE_X 3000 #define MAX_TABLE_Y 2000 #define BEACON_DETECT_RANGE 100 #define PROXIMITY_ALERT_RANGE 500 @@ -11,8 +11,8 @@ #define BEACONS_RADIUS 50 #define TRIANGULATION_ROUNDS 3 #define POSITION_CORRECT_RANGE 25 -#define YELLOW_TEAM_BEACONS_POS {make_pair(1,2), make_pair(3,4), make_pair(5,6)} -#define BLUE_TEAM_BEACONS_POS {make_pair(-94,72), make_pair(-94,1928), make_pair(1594,1000)} +#define YELLOW_TEAM_BEACONS_POS {make_pair(3094,72), make_pair(3094,1928), make_pair(-94,1000)} +#define BLUE_TEAM_BEACONS_POS {make_pair(-94,72), make_pair(-94,1928), make_pair(3094,1000)} #define LIDAR_LOG_DEBUG_MODE From 4226d562e399eb9e14b6fa997598830c11dddf22 Mon Sep 17 00:00:00 2001 From: Allan Cueff Date: Wed, 8 May 2024 18:47:35 +0200 Subject: [PATCH 3/6] =?UTF-8?q?retour=20=C3=A0=20spam=20proximity=20TCP?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/localization.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/localization.cpp b/src/localization.cpp index 5c40af6..ad610d9 100644 --- a/src/localization.cpp +++ b/src/localization.cpp @@ -319,10 +319,7 @@ void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[N if((double)nodes[pos].dist_mm_q2/4.0f < PROXIMITY_ALERT_RANGE) { proximityAlert = true; if (this->proximityLastRound) { - if(proximityValues.first == -1 || proximityValues.first > (int) ((double) nodes[pos].dist_mm_q2 / 4.0f)){ - proximityValues.first = (int) ((double) nodes[pos].dist_mm_q2 / 4.0f); - proximityValues.second = Localization::rplidarToTrigoRadians(((double) nodes[pos].angle_z_q14 * 90.f) / 16384.f);; - } + this->sendProximityAlert((int) ((double) nodes[pos].dist_mm_q2 / 4.0f), Localization::rplidarToTrigoRadians(((double) nodes[pos].angle_z_q14 * 90.f) / 16384.f)); } #ifdef LIDAR_LOG_DEBUG_MODE long epoch = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); @@ -342,9 +339,6 @@ 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 From dbc2c590117893e3760c738a397235a035c35f6a Mon Sep 17 00:00:00 2001 From: bap-0-1 Date: Wed, 8 May 2024 19:09:50 +0200 Subject: [PATCH 4/6] Revert "valeurs grande table" This reverts commit 852e5b63ea3dfc32c930e27d17ab080366d5e909. --- src/localization.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/localization.h b/src/localization.h index 4ce651d..703938a 100644 --- a/src/localization.h +++ b/src/localization.h @@ -2,7 +2,7 @@ #define LIDAR_LOCALIZATION_H #define NODES_LEN 8192 -#define MAX_TABLE_X 3000 +#define MAX_TABLE_X 1500 #define MAX_TABLE_Y 2000 #define BEACON_DETECT_RANGE 100 #define PROXIMITY_ALERT_RANGE 500 @@ -11,8 +11,8 @@ #define BEACONS_RADIUS 50 #define TRIANGULATION_ROUNDS 3 #define POSITION_CORRECT_RANGE 25 -#define YELLOW_TEAM_BEACONS_POS {make_pair(3094,72), make_pair(3094,1928), make_pair(-94,1000)} -#define BLUE_TEAM_BEACONS_POS {make_pair(-94,72), make_pair(-94,1928), make_pair(3094,1000)} +#define YELLOW_TEAM_BEACONS_POS {make_pair(1,2), make_pair(3,4), make_pair(5,6)} +#define BLUE_TEAM_BEACONS_POS {make_pair(-94,72), make_pair(-94,1928), make_pair(1594,1000)} #define LIDAR_LOG_DEBUG_MODE From 515497ac55831698630dcc5825a48bf3a2c1f967 Mon Sep 17 00:00:00 2001 From: bap-0-1 Date: Wed, 8 May 2024 19:13:09 +0200 Subject: [PATCH 5/6] Revert "Revert "valeurs grande table"" This reverts commit dbc2c590117893e3760c738a397235a035c35f6a. --- src/localization.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/localization.h b/src/localization.h index 703938a..4ce651d 100644 --- a/src/localization.h +++ b/src/localization.h @@ -2,7 +2,7 @@ #define LIDAR_LOCALIZATION_H #define NODES_LEN 8192 -#define MAX_TABLE_X 1500 +#define MAX_TABLE_X 3000 #define MAX_TABLE_Y 2000 #define BEACON_DETECT_RANGE 100 #define PROXIMITY_ALERT_RANGE 500 @@ -11,8 +11,8 @@ #define BEACONS_RADIUS 50 #define TRIANGULATION_ROUNDS 3 #define POSITION_CORRECT_RANGE 25 -#define YELLOW_TEAM_BEACONS_POS {make_pair(1,2), make_pair(3,4), make_pair(5,6)} -#define BLUE_TEAM_BEACONS_POS {make_pair(-94,72), make_pair(-94,1928), make_pair(1594,1000)} +#define YELLOW_TEAM_BEACONS_POS {make_pair(3094,72), make_pair(3094,1928), make_pair(-94,1000)} +#define BLUE_TEAM_BEACONS_POS {make_pair(-94,72), make_pair(-94,1928), make_pair(3094,1000)} #define LIDAR_LOG_DEBUG_MODE From f41125aa7edd4d908ca7f4c5f5a52cf4151829f5 Mon Sep 17 00:00:00 2001 From: bap-0-1 Date: Wed, 8 May 2024 19:13:40 +0200 Subject: [PATCH 6/6] =?UTF-8?q?Revert=20"retour=20=C3=A0=20spam=20proximit?= =?UTF-8?q?y=20TCP"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit 4226d562e399eb9e14b6fa997598830c11dddf22. --- src/localization.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/localization.cpp b/src/localization.cpp index ad610d9..5c40af6 100644 --- a/src/localization.cpp +++ b/src/localization.cpp @@ -319,7 +319,10 @@ void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[N if((double)nodes[pos].dist_mm_q2/4.0f < PROXIMITY_ALERT_RANGE) { proximityAlert = true; if (this->proximityLastRound) { - this->sendProximityAlert((int) ((double) nodes[pos].dist_mm_q2 / 4.0f), Localization::rplidarToTrigoRadians(((double) nodes[pos].angle_z_q14 * 90.f) / 16384.f)); + if(proximityValues.first == -1 || proximityValues.first > (int) ((double) nodes[pos].dist_mm_q2 / 4.0f)){ + proximityValues.first = (int) ((double) nodes[pos].dist_mm_q2 / 4.0f); + proximityValues.second = Localization::rplidarToTrigoRadians(((double) nodes[pos].angle_z_q14 * 90.f) / 16384.f);; + } } #ifdef LIDAR_LOG_DEBUG_MODE long epoch = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); @@ -339,6 +342,9 @@ 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