Set hw interfaces to unavailable on deactivated hardware

ros-controls · May 15, 2023 · a385eeb · a385eeb
1 parent d2b0d8f
commit a385eeb
Showing 1 changed file with 38 additions and 29 deletions.
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -166,39 +166,18 @@ class ResourceStorage
             hardware.get_name().c_str(), interface.c_str());
         }
       }
-
-      // add command interfaces to available list
-      for (const auto & interface : hardware_info_map_[hardware.get_name()].command_interfaces)
-      {
-        // TODO(destogl): check if interface should be available on configure
-        auto found_it = std::find(
-          available_command_interfaces_.begin(), available_command_interfaces_.end(), interface);
-
-        if (found_it == available_command_interfaces_.end())
-        {
-          available_command_interfaces_.emplace_back(interface);
-          RCUTILS_LOG_DEBUG_NAMED(
-            "resource_manager", "(hardware '%s'): '%s' command interface added into available list",
-            hardware.get_name().c_str(), interface.c_str());
-        }
-        else
-        {
-          // TODO(destogl): do here error management if interfaces are only partially added into
-          // "available" list - this should never be the case!
-          RCUTILS_LOG_WARN_NAMED(
-            "resource_manager",
-            "(hardware '%s'): '%s' command interface already in available list."
-            " This can happen due to multiple calls to 'configure'",
-            hardware.get_name().c_str(), interface.c_str());
-        }
-      }
     }
     return result;
   }
 
   void remove_all_hardware_interfaces_from_available_list(const std::string & hardware_name)
   {
-    // remove all command interfaces from available list
+    remove_all_state_interfaces_from_available_list(hardware_name);
+    remove_all_command_interfaces_from_available_list(hardware_name);
+  }
+
+  void remove_all_command_interfaces_from_available_list(const std::string & hardware_name)
+  {
     for (const auto & interface : hardware_info_map_[hardware_name].command_interfaces)
     {
       auto found_it = std::find(
@@ -222,7 +201,10 @@ class ResourceStorage
           hardware_name.c_str(), interface.c_str());
       }
     }
-    // remove all state interfaces from available list
+  }
+
+  void remove_all_state_interfaces_from_available_list(const std::string & hardware_name)
+  {
     for (const auto & interface : hardware_info_map_[hardware_name].state_interfaces)
     {
       auto found_it = std::find(
@@ -288,7 +270,31 @@ class ResourceStorage
 
     if (result)
     {
-      // TODO(destogl): make all command interfaces available (currently are all available)
+      // add command interfaces to available list
+      for (const auto & interface : hardware_info_map_[hardware.get_name()].command_interfaces)
+      {
+        // TODO(destogl): check if interface should be available on configure
+        auto found_it = std::find(
+          available_command_interfaces_.begin(), available_command_interfaces_.end(), interface);
+
+        if (found_it == available_command_interfaces_.end())
+        {
+          available_command_interfaces_.emplace_back(interface);
+          RCUTILS_LOG_DEBUG_NAMED(
+            "resource_manager", "(hardware '%s'): '%s' command interface added into available list",
+            hardware.get_name().c_str(), interface.c_str());
+        }
+        else
+        {
+          // TODO(destogl): do here error management if interfaces are only partially added into
+          // "available" list - this should never be the case!
+          RCUTILS_LOG_WARN_NAMED(
+            "resource_manager",
+            "(hardware '%s'): '%s' command interface already in available list."
+            " This can happen due to multiple calls to 'configure'",
+            hardware.get_name().c_str(), interface.c_str());
+        }
+      }
     }
 
     return result;
@@ -303,6 +309,9 @@ class ResourceStorage
 
     if (result)
     {
+      RCUTILS_LOG_INFO_NAMED(
+        "resource_manager", "Hardware '%s' has been deactivated", hardware.get_name().c_str());
+      remove_all_command_interfaces_from_available_list(hardware.get_name());
       // TODO(destogl): make all command interfaces unavailable that should be present only
       // when active (currently are all available) also at inactive
     }