diff --git a/src/localization.cpp b/src/localization.cpp index 929dba5..d65a08d 100644 --- a/src/localization.cpp +++ b/src/localization.cpp @@ -305,6 +305,7 @@ void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[N list> points_inside; vector> nearestBeaconDetectedPointRelative(3, make_pair(-1, -1)); vector beaconsDetected(3, false); + pair proximityValues = {-1, -1}; bool proximityAlert = false; bool positionIncorrect = false; for (int pos = 0; pos < count; ++pos) { @@ -313,13 +314,19 @@ void Localization::processPoints(sl_lidar_response_measurement_node_hq_t nodes[N //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 + //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); + 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 { @@ -334,6 +341,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