diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp index 52c084a..1d71dfd 100644 --- 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 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 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 index 28b96e6..bf4efec 100644 --- 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 index d916756..e3a1839 100644 --- 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;