Fix the controller deactivation on the control cycles returning ERROR

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/src/controller_manager.cpp

@@ -170,6 +170,45 @@ void controller_chain_spec_cleanup(
   }
   ctrl_chain_spec.erase(controller);
 }
+
+void extract_command_interfaces_for_controller(
+  const controller_manager::ControllerSpec & ctrl,
+  const hardware_interface::ResourceManager & resource_manager,
+  std::vector<std::string> & request_interface_list)
+{
+  auto command_interface_config = ctrl.c->command_interface_configuration();
+  std::vector<std::string> command_interface_names = {};
+  if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
+  {
+    command_interface_names = resource_manager.available_command_interfaces();
+  }
+  if (
+    command_interface_config.type == controller_interface::interface_configuration_type::INDIVIDUAL)
+  {
+    command_interface_names = command_interface_config.names;
+  }
+  request_interface_list.insert(
+    request_interface_list.end(), command_interface_names.begin(), command_interface_names.end());
+};
+
+void get_controller_list_command_interfaces(
+  const std::vector<std::string> & controllers_list,
+  const std::vector<controller_manager::ControllerSpec> & controllers_spec,
+  const hardware_interface::ResourceManager & resource_manager,
+  std::vector<std::string> & request_interface_list)
+{
+  for (const auto & controller_name : controllers_list)
+  {
+    auto found_it = std::find_if(
+      controllers_spec.begin(), controllers_spec.end(),
+      std::bind(controller_name_compare, std::placeholders::_1, controller_name));
+    if (found_it != controllers_spec.end())
+    {
+      extract_command_interfaces_for_controller(
+        *found_it, resource_manager, request_interface_list);
+    }
+  }
+}
 }  // namespace
 
 namespace controller_manager
@@ -1224,33 +1263,15 @@ controller_interface::return_type ControllerManager::switch_controller(
       activate_request_.erase(activate_list_it);
     }
 
-    const auto extract_interfaces_for_controller =
-      [this](const ControllerSpec ctrl, std::vector<std::string> & request_interface_list)
-    {
-      auto command_interface_config = ctrl.c->command_interface_configuration();
-      std::vector<std::string> command_interface_names = {};
-      if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
-      {
-        command_interface_names = resource_manager_->available_command_interfaces();
-      }
-      if (
-        command_interface_config.type ==
-        controller_interface::interface_configuration_type::INDIVIDUAL)
-      {
-        command_interface_names = command_interface_config.names;
-      }
-      request_interface_list.insert(
-        request_interface_list.end(), command_interface_names.begin(),
-        command_interface_names.end());
-    };
-
     if (in_activate_list)
     {
-      extract_interfaces_for_controller(controller, activate_command_interface_request_);
+      extract_command_interfaces_for_controller(
+        controller, *resource_manager_, activate_command_interface_request_);
     }
     if (in_deactivate_list)
     {
-      extract_interfaces_for_controller(controller, deactivate_command_interface_request_);
+      extract_command_interfaces_for_controller(
+        controller, *resource_manager_, deactivate_command_interface_request_);
     }
 
     // cache mapping between hardware and controllers for stopping when read/write error happens
@@ -2176,6 +2197,8 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration &
     std::vector<std::string> stop_request = {};
     std::string failed_hardware_string;
     failed_hardware_string.reserve(500);
+    std::vector<std::string> failed_controller_interfaces;
+    failed_controller_interfaces.reserve(500);
     // Determine controllers to stop
     for (const auto & hardware_name : failed_hardware_names)
     {
@@ -2200,8 +2223,21 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration &
       "Deactivating following controllers as their hardware components read cycle resulted in an "
       "error: [ %s]",
       stop_request_string.c_str());
+
     std::vector<ControllerSpec> & rt_controller_list =
       rt_controllers_wrapper_.update_and_get_used_by_rt_list();
+    get_controller_list_command_interfaces(
+      stop_request, rt_controller_list, *resource_manager_, failed_controller_interfaces);
+    if (!failed_controller_interfaces.empty())
+    {
+      if (!(resource_manager_->prepare_command_mode_switch({}, failed_controller_interfaces) &&
+            resource_manager_->perform_command_mode_switch({}, failed_controller_interfaces)))
+      {
+        RCLCPP_ERROR(
+          get_logger(),
+          "Error while attempting mode switch when deactivating controllers in read cycle!");
+      }
+    }
     deactivate_controllers(rt_controller_list, stop_request);
     // TODO(destogl): do auto-start of broadcasters
   }
@@ -2336,6 +2372,21 @@ controller_interface::return_type ControllerManager::update(
       get_logger(), "Deactivating following controllers as their update resulted in an error :%s",
       failed_controllers.c_str());
 
+    std::vector<std::string> failed_controller_interfaces;
+    failed_controller_interfaces.reserve(500);
+    get_controller_list_command_interfaces(
+      failed_controllers_list, rt_controller_list, *resource_manager_,
+      failed_controller_interfaces);
+    if (!failed_controller_interfaces.empty())
+    {
+      if (!(resource_manager_->prepare_command_mode_switch({}, failed_controller_interfaces) &&
+            resource_manager_->perform_command_mode_switch({}, failed_controller_interfaces)))
+      {
+        RCLCPP_ERROR(
+          get_logger(),
+          "Error while attempting mode switch when deactivating controllers in update cycle!");
+      }
+    }
     deactivate_controllers(rt_controller_list, failed_controllers_list);
   }
 
@@ -2357,6 +2408,8 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration
     std::vector<std::string> stop_request = {};
     std::string failed_hardware_string;
     failed_hardware_string.reserve(500);
+    std::vector<std::string> failed_controller_interfaces;
+    failed_controller_interfaces.reserve(500);
     // Determine controllers to stop
     for (const auto & hardware_name : failed_hardware_names)
     {
@@ -2384,6 +2437,18 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration
       stop_request_string.c_str());
     std::vector<ControllerSpec> & rt_controller_list =
       rt_controllers_wrapper_.update_and_get_used_by_rt_list();
+    get_controller_list_command_interfaces(
+      stop_request, rt_controller_list, *resource_manager_, failed_controller_interfaces);
+    if (!failed_controller_interfaces.empty())
+    {
+      if (!(resource_manager_->prepare_command_mode_switch({}, failed_controller_interfaces) &&
+            resource_manager_->perform_command_mode_switch({}, failed_controller_interfaces)))
+      {
+        RCLCPP_ERROR(
+          get_logger(),
+          "Error while attempting mode switch when deactivating controllers in write cycle!");
+      }
+    }
     deactivate_controllers(rt_controller_list, stop_request);
     // TODO(destogl): do auto-start of broadcasters
   }

controller_manager/test/test_controller/test_controller.cpp

@@ -137,6 +137,11 @@ std::vector<double> TestController::get_state_interface_data() const
   return state_intr_data;
 }
 
+void TestController::set_external_commands_for_testing(const std::vector<double> & commands)
+{
+  external_commands_for_testing_ = commands;
+}
+
 }  // namespace test_controller
 
 #include "pluginlib/class_list_macros.hpp"

controller_manager/test/test_controller/test_controller.hpp

@@ -70,6 +70,8 @@ class TestController : public controller_interface::ControllerInterface
 
   const std::string & getRobotDescription() const;
 
+  void set_external_commands_for_testing(const std::vector<double> & commands);
+
   unsigned int internal_counter = 0;
   bool simulate_cleanup_failure = false;
   // Variable where we store when cleanup was called, pointer because the controller

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());
+  }
+}

hardware_interface_testing/CMakeLists.txt

@@ -23,7 +23,8 @@ endforeach()
 add_library(test_components SHARED
 test/test_components/test_actuator.cpp
 test/test_components/test_sensor.cpp
-test/test_components/test_system.cpp)
+test/test_components/test_system.cpp
+test/test_components/test_actuator_exclusive_interfaces.cpp)
 ament_target_dependencies(test_components hardware_interface pluginlib ros2_control_test_assets)
 install(TARGETS test_components
 DESTINATION lib