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