issue 815 hardware interface add return value

controller_manager/include/controller_manager/controller_manager.hpp

@@ -206,6 +206,9 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   void manage_switch();
 
+  CONTROLLER_MANAGER_LOCAL
+  std::vector<std::string> controllers_to_skip;
+
   /// Deactivate chosen controllers from real-time controller list.
   /**
    * Deactivate controllers with names \p controllers_to_deactivate from list \p rt_controller_list.

controller_manager/src/controller_manager.cpp

@@ -1970,8 +1970,8 @@ void ControllerManager::set_hardware_component_state_srv_cb(
       // the ternary operator is needed because label in State constructor cannot be an empty string
       request->target_state.label.empty() ? "-" : request->target_state.label);
     response->ok =
-      (resource_manager_->set_component_state(request->name, target_state) ==
-       hardware_interface::return_type::OK);
+      (resource_manager_->set_component_state(request->name, target_state) !=
+       hardware_interface::return_type::ERROR);
     hw_components_info = resource_manager_->get_components_status();
     response->state.id = hw_components_info[request->name].state.id();
     response->state.label = hw_components_info[request->name].state.label();
@@ -2002,6 +2002,7 @@ std::vector<std::string> ControllerManager::get_controller_names()
 void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
   auto [ok, failed_hardware_names] = resource_manager_->read(time, period);
+  controllers_to_skip.clear();
 
   if (!ok)
   {
@@ -2018,6 +2019,16 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration &
     deactivate_controllers(rt_controller_list, stop_request);
     // TODO(destogl): do auto-start of broadcasters
   }
+  else if (failed_hardware_names.size() > 0)
+  {
+    // Status is ok but some hardware is not ok (SKIPPED)
+    // Determine controllers to skip
+    for (const auto & hardware_name : failed_hardware_names)
+    {
+      auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name);
+      controllers_to_skip.insert(controllers_to_skip.end(), controllers.begin(), controllers.end());
+    }
+  }
 }
 
 controller_interface::return_type ControllerManager::update(
@@ -2048,7 +2059,15 @@ controller_interface::return_type ControllerManager::update(
         update_loop_counter_, controller_go ? "True" : "False",
         loaded_controller.info.name.c_str());
 
-      if (controller_go)
+      bool controller_skip =
+        (std::find(
+           controllers_to_skip.begin(), controllers_to_skip.end(), loaded_controller.info.name) !=
+         controllers_to_skip.end());
+      RCLCPP_DEBUG(
+        get_logger(), "Skip ?: controller_skip: '%s' controller_name: '%s'",
+        controller_skip ? "True" : "False", loaded_controller.info.name.c_str());
+
+      if (!controller_skip && controller_go)
       {
         auto controller_ret = loaded_controller.c->update(
           time, (controller_update_factor != 1u)

hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp

@@ -23,6 +23,7 @@ enum class return_type : std::uint8_t
 {
   OK = 0,
   ERROR = 1,
+  SKIPPED = 2,
 };
 
 }  // namespace hardware_interface

hardware_interface/src/resource_manager.cpp

@@ -1261,9 +1261,13 @@ HardwareReadWriteStatus ResourceManager::read(
     {
       if (component.read(time, period) != return_type::OK)
       {
-        read_write_status.ok = false;
         read_write_status.failed_hardware_names.push_back(component.get_name());
-        resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());
+        if (component.read(time, period) == return_type::ERROR)
+        {
+          read_write_status.ok = false;
+          resource_storage_->remove_all_hardware_interfaces_from_available_list(
+            component.get_name());
+        }
       }
     }
   };