fxd ctrl & trck swtching while rc joystic active

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