Fix the failing prepare_command_mode_switch check when HW goes to error

Author: saikishor

hardware_interface/src/resource_manager.cpp

@@ -665,6 +665,7 @@ class ResourceStorage
   {
     auto interfaces = hardware.export_state_interfaces();
     const auto interface_names = add_state_interfaces(interfaces);
+    hardware_info_map_[hardware.get_name()].state_interfaces = interface_names;
 
     RCLCPP_WARN_EXPRESSION(
       get_logger(), interface_names.empty(),
@@ -1304,6 +1305,22 @@ class ResourceStorage
     init_systems(systems_);
   }
 
+  bool is_hardware_command_interface(const std::string & interface_name) const
+  {
+    // Check if the interface is a command interface of any hardware component
+    for (const auto & [hw_name, hw_info] : hardware_info_map_)
+    {
+      const auto & command_interfaces = hw_info.command_interfaces;
+      if (
+        std::find(command_interfaces.begin(), command_interfaces.end(), interface_name) !=
+        command_interfaces.end())
+      {
+        return true;
+      }
+    }
+    return false;
+  }
+
   void clear()
   {
     actuators_.clear();
@@ -2020,20 +2037,30 @@ bool ResourceManager::prepare_command_mode_switch(
   // Check if interfaces are available
   std::stringstream ss_not_available;
   ss_not_available << "Not available: " << std::endl << "[" << std::endl;
-  auto check_available = [&](const std::vector<std::string> & list_to_check)
+  auto check_available =
+    [&](const std::vector<std::string> & list_to_check, bool check_only_if_cmd_itf_available)
   {
     bool all_available = true;
     for (const auto & interface : list_to_check)
     {
+      /// When the hardware goes is in error, the command interfaces are made unavailable to avoid
+      /// starting any interfaces, so for the stop interfaces we will have to check if the
+      /// interfaces belong to the hardware when they are unavailable to avoid deactivation
       if (!command_interface_is_available(interface))
       {
+        if (
+          !check_only_if_cmd_itf_available &&
+          resource_storage_->is_hardware_command_interface(interface))
+        {
+          continue;
+        }
         all_available = false;
         ss_not_available << " " << interface << std::endl;
       }
     }
     return all_available;
   };
-  if (!(check_available(start_interfaces) && check_available(stop_interfaces)))
+  if (!(check_available(start_interfaces, true) && check_available(stop_interfaces, false)))
   {
     ss_not_available << "]" << std::endl;
     RCLCPP_ERROR(