From f41125aa7edd4d908ca7f4c5f5a52cf4151829f5 Mon Sep 17 00:00:00 2001 From: bap-0-1 Date: Wed, 8 May 2024 19:13:40 +0200 Subject: [PATCH] =?UTF-8?q?Revert=20"retour=20=C3=A0=20spam=20proximity=20?= =?UTF-8?q?TCP"?= 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