Handle HW errors on read and write in CM by stopping controllers

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