ControlManager: disabled ctrl and trckr switching during rc mode

ctu-mrs · Apr 24, 2024 · 296df24 · 296df24
1 parent b013e67
commit 296df24
Show file tree

Hide file tree

Showing 3 changed files with 77 additions and 1 deletion.
diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp
@@ -4098,7 +4098,8 @@ void ControlManager::callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg) {
     }
   }
 
-  // | ------------------------ rc eland ------------------------ |
+  // | ----------------- RC escalating failsafe ----------------- |
+
   if (_rc_escalating_failsafe_enabled_) {
 
     if (_rc_escalating_failsafe_channel_ >= int(rc->channels.size())) {
@@ -8006,6 +8007,12 @@ 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
@@ -8140,6 +8147,12 @@ 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

diff --git a/test/control_manager/rc_remote_control/config/custom_config.yaml b/test/control_manager/rc_remote_control/config/custom_config.yaml
@@ -1,5 +1,13 @@
 mrs_uav_managers:
 
+  estimation_manager:
+
+    state_estimators: [
+    "gps_baro",
+    ]
+
+    initial_state_estimator: "gps_baro"
+
   constraint_manager:
 
     default_constraints:

diff --git a/test/include/remote_control_test.h b/test/include/remote_control_test.h
@@ -566,6 +566,61 @@ bool RemoteControlTest::test() {
 
   deactivate();
 
+  // | ------- test switching of controllers and trackers ------- |
+
+  activate();
+
+  {
+    bool success = testMoveForward();
+
+    if (!success) {
+      ROS_ERROR("[%s]: forward motion test failed", ros::this_node::getName().c_str());
+      return false;
+    }
+  }
+
+  {
+    auto [success, message] = uh_->switchController("Se3Controller");
+
+    if (success) {
+      ROS_ERROR("[%s]: controller switched, this should not be allowed: '%s'", ros::this_node::getName().c_str(), message.c_str());
+      return false;
+    }
+  }
+
+  sleep(2.0);
+
+  {
+    bool success = testMoveForward();
+
+    if (!success) {
+      ROS_ERROR("[%s]: forward motion test failed", ros::this_node::getName().c_str());
+      return false;
+    }
+  }
+
+  {
+    auto [success, message] = uh_->switchController("MpcController");
+
+    if (success) {
+      ROS_ERROR("[%s]: controller switched, this should not be allowed: '%s'", ros::this_node::getName().c_str(), message.c_str());
+      return false;
+    }
+  }
+
+  sleep(2.0);
+
+  {
+    bool success = testMoveForward();
+
+    if (!success) {
+      ROS_ERROR("[%s]: forward motion test failed", ros::this_node::getName().c_str());
+      return false;
+    }
+  }
+
+  deactivate();
+
   // | ------------- activate the remote controller ------------- |
 
   return true;