From dcf0a58e6f38113615722ca5b8a7457efa37c03d Mon Sep 17 00:00:00 2001 From: Allan Cueff Date: Wed, 8 May 2024 14:34:17 +0200 Subject: [PATCH] =?UTF-8?q?ajout=20lox=20proximit=C3=A9=20+=20envoi=20d'al?= =?UTF-8?q?erte=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 }