diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 96c87f5806..73daddd23d 100644 --- 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(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(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(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)