added test for the deactivation in the control loops

controller_manager/CMakeLists.txt

@@ -179,6 +179,7 @@ if(BUILD_TESTING)
   )
   target_link_libraries(test_release_interfaces
     controller_manager
+    test_controller
     test_controller_with_interfaces
     ros2_control_test_assets::ros2_control_test_assets
   )

controller_manager/test/test_release_interfaces.cpp

@@ -20,6 +20,7 @@
 #include "controller_manager/controller_manager.hpp"
 #include "controller_manager_test_common.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "test_controller/test_controller.hpp"
 #include "test_controller_with_interfaces/test_controller_with_interfaces.hpp"
 
 using ::testing::_;
@@ -197,3 +198,137 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
       abstract_test_controller2.c->get_lifecycle_state().id());
   }
 }
+
+class TestReleaseExclusiveInterfaces
+: public ControllerManagerFixture<controller_manager::ControllerManager>
+{
+public:
+  TestReleaseExclusiveInterfaces()
+  : ControllerManagerFixture<controller_manager::ControllerManager>(
+      std::string(ros2_control_test_assets::urdf_head) +
+      std::string(ros2_control_test_assets::hardware_resources_with_exclusive_interface) +
+      std::string(ros2_control_test_assets::urdf_tail))
+  {
+  }
+};
+
+TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_deactivation_on_update_error)
+{
+  const std::string controller_type = test_controller::TEST_CONTROLLER_CLASS_NAME;
+
+  // Load two controllers of different names
+  const std::string controller_name1 = "test_controller1";
+  const std::string controller_name2 = "test_controller2";
+
+  auto test_controller1 = std::make_shared<test_controller::TestController>();
+  controller_interface::InterfaceConfiguration cmd_cfg = {
+    controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
+  controller_interface::InterfaceConfiguration state_cfg = {
+    controller_interface::interface_configuration_type::INDIVIDUAL,
+    {"joint1/position", "joint1/velocity"}};
+  test_controller1->set_command_interface_configuration(cmd_cfg);
+  test_controller1->set_state_interface_configuration(state_cfg);
+  cm_->add_controller(test_controller1, controller_name1, controller_type);
+
+  auto test_controller2 = std::make_shared<test_controller::TestController>();
+  cmd_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}};
+  state_cfg = {
+    controller_interface::interface_configuration_type::INDIVIDUAL,
+    {"joint2/position", "joint2/velocity"}};
+  test_controller2->set_command_interface_configuration(cmd_cfg);
+  test_controller2->set_state_interface_configuration(state_cfg);
+  cm_->add_controller(test_controller2, controller_name2, controller_type);
+  ASSERT_EQ(2u, cm_->get_loaded_controllers().size());
+  controller_manager::ControllerSpec abstract_test_controller1 = cm_->get_loaded_controllers()[0];
+  controller_manager::ControllerSpec abstract_test_controller2 = cm_->get_loaded_controllers()[1];
+
+  // Configure controllers
+  ASSERT_EQ(controller_interface::return_type::OK, cm_->configure_controller(controller_name1));
+  ASSERT_EQ(controller_interface::return_type::OK, cm_->configure_controller(controller_name2));
+
+  ASSERT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    abstract_test_controller1.c->get_lifecycle_state().id());
+  ASSERT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    abstract_test_controller2.c->get_lifecycle_state().id());
+
+  {  // Test starting the first and second controller
+    RCLCPP_INFO(cm_->get_logger(), "Starting controller #1 and #2");
+    std::vector<std::string> start_controllers = {controller_name1, controller_name2};
+    std::vector<std::string> stop_controllers = {};
+    auto switch_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
+    ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      abstract_test_controller1.c->get_lifecycle_state().id());
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      abstract_test_controller2.c->get_lifecycle_state().id());
+  }
+
+  // As the external command is Nan, the controller update cycle should return an error and
+  // deactivate the controllers
+  {
+    test_controller1->set_external_commands_for_testing({std::numeric_limits<double>::quiet_NaN()});
+    test_controller2->set_external_commands_for_testing({std::numeric_limits<double>::quiet_NaN()});
+    ControllerManagerRunner cm_runner(this);
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      abstract_test_controller1.c->get_lifecycle_state().id());
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      abstract_test_controller2.c->get_lifecycle_state().id());
+  }
+
+  {  // Test starting both controllers but only the second one will pass as the first one has the
+     // same external command
+    test_controller2->set_external_commands_for_testing({0.0});
+    RCLCPP_INFO(cm_->get_logger(), "Starting controller #1 and #2 again");
+    std::vector<std::string> start_controllers = {controller_name1, controller_name2};
+    std::vector<std::string> stop_controllers = {};
+    auto switch_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
+    ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      abstract_test_controller1.c->get_lifecycle_state().id())
+      << "Controller 1 current state is "
+      << abstract_test_controller1.c->get_lifecycle_state().label();
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      abstract_test_controller2.c->get_lifecycle_state().id());
+  }
+
+  {  // Test starting controller 1 and it should work as external command is valid now
+    test_controller1->set_external_commands_for_testing({0.0});
+    RCLCPP_INFO(cm_->get_logger(), "Starting controller #1");
+    std::vector<std::string> start_controllers = {controller_name1};
+    std::vector<std::string> stop_controllers = {};
+    auto switch_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
+    ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      abstract_test_controller1.c->get_lifecycle_state().id());
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      abstract_test_controller2.c->get_lifecycle_state().id());
+  }
+}