From bb1a7e744d7f9f0c5ad477e82d34c8ec85961e91 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Thu, 29 Aug 2024 13:08:18 +0200
Subject: [PATCH] fxd ctrl & trck swtching while rc joystic active

---
 src/control_manager/control_manager.cpp | 38 +++++++++++++++++--------
 1 file changed, 26 insertions(+), 12 deletions(-)

diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp
index fc32d1a..3d354cb 100644
--- a/src/control_manager/control_manager.cpp
+++ b/src/control_manager/control_manager.cpp
@@ -4181,6 +4181,19 @@ bool ControlManager::callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_m
     return true;
   }
 
+  if (rc_goto_active_) {
+
+    std::stringstream ss;
+    ss << "can not switch tracker, RC joystick is active";
+
+    res.message = ss.str();
+    res.success = false;
+
+    ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+
+    return true;
+  }
+
   auto [success, response] = switchTracker(req.value);
 
   res.success = success;
@@ -4212,6 +4225,19 @@ bool ControlManager::callbackSwitchController(mrs_msgs::String::Request& req, mr
     return true;
   }
 
+  if (rc_goto_active_) {
+
+    std::stringstream ss;
+    ss << "can not switch controller, RC joystick is active";
+
+    res.message = ss.str();
+    res.success = false;
+
+    ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+
+    return true;
+  }
+
   auto [success, response] = switchController(req.value);
 
   res.success = success;
@@ -8075,12 +8101,6 @@ std::tuple<bool, std::string> ControlManager::switchTracker(const std::string& t
     return std::tuple(false, ss.str());
   }
 
-  if (rc_goto_active_) {
-    ss << "can not switch tracker, the RC joystick is active";
-    ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
-    return std::tuple(false, ss.str());
-  }
-
   auto new_tracker_idx = idxInVector(tracker_name, _tracker_names_);
 
   // check if the tracker exists
@@ -8215,12 +8235,6 @@ std::tuple<bool, std::string> ControlManager::switchController(const std::string
     return std::tuple(false, ss.str());
   }
 
-  if (rc_goto_active_) {
-    ss << "can not switch controller, the RC joystick is active";
-    ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
-    return std::tuple(false, ss.str());
-  }
-
   auto new_controller_idx = idxInVector(controller_name, _controller_names_);
 
   // check if the controller exists