Add checks if hardware is initialized. (ros-controls#1054)

henrygerardmoore · Jul 18, 2023 · 6cd8191 · 6cd8191
1 parent aea4efe
commit 6cd8191
Showing 1 changed file with 70 additions and 16 deletions.
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -514,9 +514,18 @@ class ResourceStorage
     {
       check_for_duplicates(hardware_info);
       load_hardware<Actuator, ActuatorInterface>(hardware_info, actuator_loader_, container);
-      initialize_hardware(hardware_info, container.back());
-      import_state_interfaces(container.back());
-      import_command_interfaces(container.back());
+      if (initialize_hardware(hardware_info, container.back()))
+      {
+        import_state_interfaces(container.back());
+        import_command_interfaces(container.back());
+      }
+      else
+      {
+        RCUTILS_LOG_WARN_NAMED(
+          "resource_manager",
+          "Actuator hardware component '%s' from plugin '%s' failed to initialize.",
+          hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str());
+      }
     };
 
     if (hardware_info.is_async)
@@ -535,8 +544,17 @@ class ResourceStorage
     {
       check_for_duplicates(hardware_info);
       load_hardware<Sensor, SensorInterface>(hardware_info, sensor_loader_, container);
-      initialize_hardware(hardware_info, container.back());
-      import_state_interfaces(container.back());
+      if (initialize_hardware(hardware_info, container.back()))
+      {
+        import_state_interfaces(container.back());
+      }
+      else
+      {
+        RCUTILS_LOG_WARN_NAMED(
+          "resource_manager",
+          "Sensor hardware component '%s' from plugin '%s' failed to initialize.",
+          hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str());
+      }
     };
 
     if (hardware_info.is_async)
@@ -555,9 +573,18 @@ class ResourceStorage
     {
       check_for_duplicates(hardware_info);
       load_hardware<System, SystemInterface>(hardware_info, system_loader_, container);
-      initialize_hardware(hardware_info, container.back());
-      import_state_interfaces(container.back());
-      import_command_interfaces(container.back());
+      if (initialize_hardware(hardware_info, container.back()))
+      {
+        import_state_interfaces(container.back());
+        import_command_interfaces(container.back());
+      }
+      else
+      {
+        RCUTILS_LOG_WARN_NAMED(
+          "resource_manager",
+          "System hardware component '%s' from plugin '%s' failed to initialize.",
+          hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str());
+      }
     };
 
     if (hardware_info.is_async)
@@ -576,9 +603,18 @@ class ResourceStorage
     auto init_actuators = [&](auto & container)
     {
       container.emplace_back(Actuator(std::move(actuator)));
-      initialize_hardware(hardware_info, container.back());
-      import_state_interfaces(container.back());
-      import_command_interfaces(container.back());
+      if (initialize_hardware(hardware_info, container.back()))
+      {
+        import_state_interfaces(container.back());
+        import_command_interfaces(container.back());
+      }
+      else
+      {
+        RCUTILS_LOG_WARN_NAMED(
+          "resource_manager",
+          "Actuator hardware component '%s' from plugin '%s' failed to initialize.",
+          hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str());
+      }
     };
 
     if (hardware_info.is_async)
@@ -597,8 +633,17 @@ class ResourceStorage
     auto init_sensors = [&](auto & container)
     {
       container.emplace_back(Sensor(std::move(sensor)));
-      initialize_hardware(hardware_info, container.back());
-      import_state_interfaces(container.back());
+      if (initialize_hardware(hardware_info, container.back()))
+      {
+        import_state_interfaces(container.back());
+      }
+      else
+      {
+        RCUTILS_LOG_WARN_NAMED(
+          "resource_manager",
+          "Sensor hardware component '%s' from plugin '%s' failed to initialize.",
+          hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str());
+      }
     };
 
     if (hardware_info.is_async)
@@ -617,9 +662,18 @@ class ResourceStorage
     auto init_systems = [&](auto & container)
     {
       container.emplace_back(System(std::move(system)));
-      initialize_hardware(hardware_info, container.back());
-      import_state_interfaces(container.back());
-      import_command_interfaces(container.back());
+      if (initialize_hardware(hardware_info, container.back()))
+      {
+        import_state_interfaces(container.back());
+        import_command_interfaces(container.back());
+      }
+      else
+      {
+        RCUTILS_LOG_WARN_NAMED(
+          "resource_manager",
+          "System hardware component '%s' from plugin '%s' failed to initialize.",
+          hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str());
+      }
     };
 
     if (hardware_info.is_async)