From f9e0eac0680cc1cd2f5bea0e2d55199c8af1178a Mon Sep 17 00:00:00 2001
From: Matej Petrlik <matej.petrlik@fel.cvut.cz>
Date: Tue, 24 Oct 2023 14:09:10 +0200
Subject: [PATCH] fixed bug with multiple tf_sources publishing world_origin tf
 after estimator switch

---
 include/transform_manager/tf_source.h       |  6 ++++++
 src/transform_manager/transform_manager.cpp | 17 +++++++++--------
 2 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/include/transform_manager/tf_source.h b/include/transform_manager/tf_source.h
index 55bbeb49..e63f0cef 100644
--- a/include/transform_manager/tf_source.h
+++ b/include/transform_manager/tf_source.h
@@ -146,6 +146,12 @@ class TfSource {
   }
   /*//}*/
 
+  /*//{ getIsUtmSource() */
+  bool getIsUtmSource() {
+    return is_utm_source_;
+  }
+  /*//}*/
+
   /*//{ setIsUtmSource() */
   void setIsUtmSource(const bool is_utm_source) {
     is_utm_source_ = is_utm_source;
diff --git a/src/transform_manager/transform_manager.cpp b/src/transform_manager/transform_manager.cpp
index 3ef1723b..69825aa1 100644
--- a/src/transform_manager/transform_manager.cpp
+++ b/src/transform_manager/transform_manager.cpp
@@ -550,13 +550,13 @@ void TransformManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg)
 
     for (size_t i = 0; i < tf_sources_.size(); i++) {
 
-      // first check if tf source can publish utm origin and is not the switched from estimator
+      // first check if tf source can publish utm origin and is not the switched-from estimator
       if (tf_sources_.at(i)->getIsUtmBased() && tf_sources_.at(i)->getName() != last_estimator_name) {
 
         valid_utm_source_found     = true;
         potential_utm_source_index = i;
 
-        // check if switched to estimator is utm_based, if so, use it
+        // check if switched-to estimator is utm_based, if so, use it
         if (tf_sources_.at(i)->getIsUtmBased() && tf_sources_.at(i)->getName() == current_estimator_name) {
           potential_utm_source_index = i;
           break;
@@ -565,19 +565,20 @@ void TransformManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg)
     }
 
 
-    // if we found a valid utm source, use it, otherwise stay with the switched from estimator
+    // if we found a valid utm source, use it, otherwise stay with the switched-from estimator
     if (valid_utm_source_found) {
 
-      tf_sources_.at(potential_utm_source_index)->setIsUtmSource(true);
-      ROS_INFO("[%s]: setting is_utm_source of estimator %s to true", getPrintName().c_str(), current_estimator_name.c_str());
-
-      // stop previous estimator from publishing utm source
+      // stop all estimators from publishing utm source
       for (size_t i = 0; i < tf_sources_.size(); i++) {
-        if (tf_sources_.at(i)->getName() == last_estimator_name) {
+        if (tf_sources_.at(i)->getIsUtmSource()) {
           tf_sources_.at(i)->setIsUtmSource(false);
           ROS_INFO("[%s]: setting is_utm_source of estimator %s to false", getPrintName().c_str(), last_estimator_name.c_str());
         }
       }
+
+      tf_sources_.at(potential_utm_source_index)->setIsUtmSource(true);
+      ROS_INFO("[%s]: setting is_utm_source of estimator %s to true", getPrintName().c_str(), current_estimator_name.c_str());
+
     }
   }
   /*//}*/