Handle HW errors on read and write in CM by stopping controllers (#742)

Add code for deactivating controller when hardware gets an error on read and write.
Fix misleading variable name in the tests.
Remove all interface from available list for hardware when an error happens.
Do some more variable renaming to the new nomenclature.

(cherry picked from commit ecf6c69)

controller_manager/CMakeLists.txt

@@ -102,6 +102,14 @@ if(BUILD_TESTING)
   target_link_libraries(test_controller_manager_with_namespace.cpp ${PROJECT_NAME} test_controller)
   ament_target_dependencies(test_controller_manager_with_namespace.cpp ros2_control_test_assets)
 
+  ament_add_gmock(
+    test_controller_manager_hardware_error_handling
+    test/test_controller_manager_hardware_error_handling.cpp
+  )
+  target_include_directories(test_controller_manager_hardware_error_handling PRIVATE include)
+  target_link_libraries(test_controller_manager_hardware_error_handling ${PROJECT_NAME} test_controller)
+  ament_target_dependencies(test_controller_manager_hardware_error_handling ros2_control_test_assets)
+
   ament_add_gmock(
     test_load_controller
     test/test_load_controller.cpp

controller_manager/include/controller_manager/controller_manager.hpp

@@ -124,17 +124,17 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::return_type configure_controller(const std::string & controller_name);
 
-  /// switch_controller Stops some controllers and start others.
+  /// switch_controller Deactivates some controllers and activates others.
   /**
-   * \param[in] start_controllers is a list of controllers to start
-   * \param[in] stop_controllers is a list of controllers to stop
+   * \param[in] activate_controllers is a list of controllers to activate.
+   * \param[in] deactivate_controllers is a list of controllers to deactivate.
    * \param[in] set level of strictness (BEST_EFFORT or STRICT)
    * \see Documentation in controller_manager_msgs/SwitchController.srv
    */
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::return_type switch_controller(
-    const std::vector<std::string> & start_controllers,
-    const std::vector<std::string> & stop_controllers, int strictness,
+    const std::vector<std::string> & activate_controllers,
+    const std::vector<std::string> & deactivate_controllers, int strictness,
     bool activate_asap = kWaitForAllResources,
     const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds(kInfiniteTimeout));
 
@@ -196,8 +196,18 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   void manage_switch();
 
+  /// Deactivate chosen controllers from real-time controller list.
+  /**
+   * Deactivate controllers with names \p controllers_to_deactivate from list \p rt_controller_list.
+   * The controller list will be iterated as many times as there are controller names.
+   *
+   * \param[in] rt_controller_list controllers in the real-time list.
+   * \param[in] controllers_to_deactivate names of the controller that have to be deactivated.
+   */
   CONTROLLER_MANAGER_PUBLIC
-  void deactivate_controllers();
+  void deactivate_controllers(
+    const std::vector<ControllerSpec> & rt_controller_list,
+    const std::vector<std::string> controllers_to_deactivate);
 
   /**
    * Switch chained mode for all the controllers with respect to the following cases:
@@ -211,11 +221,34 @@ class ControllerManager : public rclcpp::Node
   void switch_chained_mode(
     const std::vector<std::string> & chained_mode_switch_list, bool to_chained_mode);
 
+  /// Activate chosen controllers from real-time controller list.
+  /**
+   * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list.
+   * The controller list will be iterated as many times as there are controller names.
+   *
+   * \param[in] rt_controller_list controllers in the real-time list.
+   * \param[in] controllers_to_activate names of the controller that have to be activated.
+   */
   CONTROLLER_MANAGER_PUBLIC
-  void activate_controllers();
+  void activate_controllers(
+    const std::vector<ControllerSpec> & rt_controller_list,
+    const std::vector<std::string> controllers_to_activate);
 
+  /// Activate chosen controllers from real-time controller list.
+  /**
+   * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list.
+   * The controller list will be iterated as many times as there are controller names.
+   *
+   * *NOTE*: There is currently not difference to `activate_controllers` method.
+   * Check https://github.com/ros-controls/ros2_control/issues/263 for more information.
+   *
+   * \param[in] rt_controller_list controllers in the real-time list.
+   * \param[in] controllers_to_activate names of the controller that have to be activated.
+   */
   CONTROLLER_MANAGER_PUBLIC
-  void activate_controllers_asap();
+  void activate_controllers_asap(
+    const std::vector<ControllerSpec> & rt_controller_list,
+    const std::vector<std::string> controllers_to_activate);
 
   CONTROLLER_MANAGER_PUBLIC
   void list_controllers_srv_cb(
@@ -305,7 +338,7 @@ class ControllerManager : public rclcpp::Node
    *
    * For each controller the whole chain of following controllers is checked.
    *
-   * NOTE: The automatically adding of following controller into starting list is not implemented
+   * NOTE: The automatically adding of following controller into activate list is not implemented
    * yet.
    *
    * \param[in] controllers list with controllers.
@@ -329,7 +362,7 @@ class ControllerManager : public rclcpp::Node
    * - will be deactivated,
    * - and will not be activated.
    *
-   * NOTE: The automatically adding of preceding controllers into stopping list is not implemented
+   * NOTE: The automatically adding of preceding controllers into deactivate list is not implemented
    * yet.
    *
    * \param[in] controllers list with controllers.

controller_manager/src/controller_manager.cpp

@@ -831,6 +831,45 @@ controller_interface::return_type ControllerManager::switch_controller(
     {
       extract_interfaces_for_controller(controller, deactivate_command_interface_request_);
     }
+
+    // cache mapping between hardware and controllers for stopping when read/write error happens
+    // TODO(destogl): This caching approach is suboptimal because the cache can fast become
+    // outdated. Keeping it up to date is not easy because of stopping controllers from multiple
+    // threads maybe we should not at all cache this but always search for the related controllers
+    // to a hardware when error in hardware happens
+    if (in_activate_list)
+    {
+      std::vector<std::string> interface_names = {};
+
+      auto command_interface_config = controller.c->command_interface_configuration();
+      if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
+      {
+        interface_names = resource_manager_->available_command_interfaces();
+      }
+      if (
+        command_interface_config.type ==
+        controller_interface::interface_configuration_type::INDIVIDUAL)
+      {
+        interface_names = command_interface_config.names;
+      }
+
+      std::vector<std::string> interfaces = {};
+      auto state_interface_config = controller.c->state_interface_configuration();
+      if (state_interface_config.type == controller_interface::interface_configuration_type::ALL)
+      {
+        interfaces = resource_manager_->available_state_interfaces();
+      }
+      if (
+        state_interface_config.type ==
+        controller_interface::interface_configuration_type::INDIVIDUAL)
+      {
+        interfaces = state_interface_config.names;
+      }
+
+      interface_names.insert(interface_names.end(), interfaces.begin(), interfaces.end());
+
+      resource_manager_->cache_controller_to_hardware(controller.info.name, interface_names);
+    }
   }
 
   if (activate_request_.empty() && deactivate_request_.empty())
@@ -853,6 +892,7 @@ controller_interface::return_type ControllerManager::switch_controller(
       return controller_interface::return_type::ERROR;
     }
   }
+
   // start the atomic controller switching
   switch_params_.strictness = strictness;
   switch_params_.activate_asap = activate_asap;
@@ -977,31 +1017,34 @@ void ControllerManager::manage_switch()
     RCLCPP_ERROR(get_logger(), "Error while performing mode switch.");
   }
 
-  deactivate_controllers();
+  std::vector<ControllerSpec> & rt_controller_list =
+    rt_controllers_wrapper_.update_and_get_used_by_rt_list();
+
+  deactivate_controllers(rt_controller_list, deactivate_request_);
 
   switch_chained_mode(to_chained_mode_request_, true);
   switch_chained_mode(from_chained_mode_request_, false);
 
   // activate controllers once the switch is fully complete
   if (!switch_params_.activate_asap)
   {
-    activate_controllers();
+    activate_controllers(rt_controller_list, activate_request_);
   }
   else
   {
     // activate controllers as soon as their required joints are done switching
-    activate_controllers_asap();
+    activate_controllers_asap(rt_controller_list, activate_request_);
   }
 
   // TODO(destogl): move here "do_switch = false"
 }
 
-void ControllerManager::deactivate_controllers()
+void ControllerManager::deactivate_controllers(
+  const std::vector<ControllerSpec> & rt_controller_list,
+  const std::vector<std::string> controllers_to_deactivate)
 {
-  std::vector<ControllerSpec> & rt_controller_list =
-    rt_controllers_wrapper_.update_and_get_used_by_rt_list();
-  // stop controllers
-  for (const auto & request : deactivate_request_)
+  // deactivate controllers
+  for (const auto & request : controllers_to_deactivate)
   {
     auto found_it = std::find_if(
       rt_controller_list.begin(), rt_controller_list.end(),
@@ -1010,7 +1053,7 @@ void ControllerManager::deactivate_controllers()
     {
       RCLCPP_ERROR(
         get_logger(),
-        "Got request to stop controller '%s' but it is not in the realtime controller list",
+        "Got request to deactivate controller '%s' but it is not in the realtime controller list",
         request.c_str());
       continue;
     }
@@ -1086,11 +1129,11 @@ void ControllerManager::switch_chained_mode(
   }
 }
 
-void ControllerManager::activate_controllers()
+void ControllerManager::activate_controllers(
+  const std::vector<ControllerSpec> & rt_controller_list,
+  const std::vector<std::string> controllers_to_activate)
 {
-  std::vector<ControllerSpec> & rt_controller_list =
-    rt_controllers_wrapper_.update_and_get_used_by_rt_list();
-  for (const auto & request : activate_request_)
+  for (const auto & request : controllers_to_activate)
   {
     auto found_it = std::find_if(
       rt_controller_list.begin(), rt_controller_list.end(),
@@ -1197,10 +1240,12 @@ void ControllerManager::activate_controllers()
   switch_params_.do_switch = false;
 }
 
-void ControllerManager::activate_controllers_asap()
+void ControllerManager::activate_controllers_asap(
+  const std::vector<ControllerSpec> & rt_controller_list,
+  const std::vector<std::string> controllers_to_activate)
 {
   //  https://github.com/ros-controls/ros2_control/issues/263
-  activate_controllers();
+  activate_controllers(rt_controller_list, controllers_to_activate);
 }
 
 void ControllerManager::list_controllers_srv_cb(
@@ -1547,6 +1592,20 @@ void ControllerManager::list_hardware_components_srv_cb(
       hwi.name = interface;
       hwi.is_available = resource_manager_->command_interface_is_available(interface);
       hwi.is_claimed = resource_manager_->command_interface_is_claimed(interface);
+      // TODO(destogl): Add here mapping to controller that has claimed or
+      // can be claiming this interface
+      // Those should be two variables
+      // if (hwi.is_claimed)
+      // {
+      //   for (const auto & controller : controllers_that_use_interface(interface))
+      //   {
+      //     if (is_controller_active(controller))
+      //     {
+      //       hwi.is_claimed_by = controller;
+      //     }
+      //   }
+      // }
+      // hwi.is_used_by = controllers_that_use_interface(interface);
       component.command_interfaces.push_back(hwi);
     }
 
@@ -1645,7 +1704,23 @@ std::vector<std::string> ControllerManager::get_controller_names()
 
 void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  resource_manager_->read(time, period);
+  auto [ok, failed_hardware_names] = resource_manager_->read(time, period);
+
+  if (!ok)
+  {
+    std::vector<std::string> stop_request = {};
+    // Determine controllers to stop
+    for (const auto & hardware_name : failed_hardware_names)
+    {
+      auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name);
+      stop_request.insert(stop_request.end(), controllers.begin(), controllers.end());
+    }
+
+    std::vector<ControllerSpec> & rt_controller_list =
+      rt_controllers_wrapper_.update_and_get_used_by_rt_list();
+    deactivate_controllers(rt_controller_list, stop_request);
+    // TODO(destogl): do auto-start of broadcasters
+  }
 }
 
 controller_interface::return_type ControllerManager::update(
@@ -1699,7 +1774,23 @@ controller_interface::return_type ControllerManager::update(
 
 void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  resource_manager_->write(time, period);
+  auto [ok, failed_hardware_names] = resource_manager_->write(time, period);
+
+  if (!ok)
+  {
+    std::vector<std::string> stop_request = {};
+    // Determine controllers to stop
+    for (const auto & hardware_name : failed_hardware_names)
+    {
+      auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name);
+      stop_request.insert(stop_request.end(), controllers.begin(), controllers.end());
+    }
+
+    std::vector<ControllerSpec> & rt_controller_list =
+      rt_controllers_wrapper_.update_and_get_used_by_rt_list();
+    deactivate_controllers(rt_controller_list, stop_request);
+    // TODO(destogl): do auto-start of broadcasters
+  }
 }
 
 std::vector<ControllerSpec> &

controller_manager/test/controller_manager_test_common.hpp

@@ -105,7 +105,6 @@ class ControllerManagerFixture : public ::testing::Test
     const std::future_status expected_future_status = std::future_status::timeout,
     const controller_interface::return_type expected_return = controller_interface::return_type::OK)
   {
-    // First activation not possible because controller not configured
     auto switch_future = std::async(
       std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
       start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0));

controller_manager/test/test_controller/test_controller.cpp

@@ -14,6 +14,7 @@
 
 #include "test_controller.hpp"
 
+#include <limits>
 #include <memory>
 #include <string>
 
@@ -25,6 +26,7 @@ TestController::TestController()
 : controller_interface::ControllerInterface(),
   cmd_iface_cfg_{controller_interface::interface_configuration_type::NONE}
 {
+  set_first_command_interface_value_to = std::numeric_limits<double>::quiet_NaN();
 }
 
 controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const
@@ -62,12 +64,22 @@ controller_interface::return_type TestController::update(
 {
   ++internal_counter;
 
-  for (size_t i = 0; i < command_interfaces_.size(); ++i)
+  // set value to hardware to produce and test different behaviors there
+  if (!std::isnan(set_first_command_interface_value_to))
   {
-    RCLCPP_INFO(
-      get_node()->get_logger(), "Setting value of command interface '%s' to %f",
-      command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]);
-    command_interfaces_[i].set_value(external_commands_for_testing_[i]);
+    command_interfaces_[0].set_value(set_first_command_interface_value_to);
+    // reset to be easier to test
+    set_first_command_interface_value_to = std::numeric_limits<double>::quiet_NaN();
+  }
+  else
+  {
+    for (size_t i = 0; i < command_interfaces_.size(); ++i)
+    {
+      RCLCPP_INFO(
+        get_node()->get_logger(), "Setting value of command interface '%s' to %f",
+        command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]);
+      command_interfaces_[i].set_value(external_commands_for_testing_[i]);
+    }
   }
 
   return controller_interface::return_type::OK;

controller_manager/test/test_controller/test_controller.hpp

@@ -74,6 +74,9 @@ class TestController : public controller_interface::ControllerInterface
   controller_interface::InterfaceConfiguration state_iface_cfg_;
 
   std::vector<double> external_commands_for_testing_;
+  // enables external setting of values to command interfaces - used for simulation of hardware
+  // errors
+  double set_first_command_interface_value_to;
 };
 
 }  // namespace test_controller