[ResourceManager] Add interfaces only if hardware is successfully ini…

…tialized.
ros-controls · Jun 14, 2023 · 9e630a6 · 9e630a6
1 parent 99deef8
commit 9e630a6
Showing 1 changed file with 28 additions and 16 deletions.
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -514,9 +514,11 @@ 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());
+      }
     };
 
     if (hardware_info.is_async)
@@ -535,8 +537,10 @@ 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());
+      }
     };
 
     if (hardware_info.is_async)
@@ -555,9 +559,11 @@ 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());
+      }
     };
 
     if (hardware_info.is_async)
@@ -576,9 +582,11 @@ 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());
+      }
     };
 
     if (hardware_info.is_async)
@@ -597,8 +605,10 @@ 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());
+      }
     };
 
     if (hardware_info.is_async)
@@ -617,9 +627,11 @@ 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());
+      }
     };
 
     if (hardware_info.is_async)