From 81a9fe7bd869f24c12c3a58cdc608ef700a81ed7 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 11 Aug 2023 12:27:21 +0100 Subject: [PATCH] Add checks if hardware is initialized. (backport #1054) (#1081) --- hardware_interface/src/resource_manager.cpp | 134 ++++++++++++++++---- 1 file changed, 109 insertions(+), 25 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 7082b7ea23..98f2727613 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -484,54 +484,138 @@ class ResourceStorage // TODO(destogl): Propagate "false" up, if happens in initialize_hardware void load_and_initialize_actuator(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, actuator_loader_, actuators_); - initialize_hardware(hardware_info, actuators_.back()); - import_state_interfaces(actuators_.back()); - import_command_interfaces(actuators_.back()); + auto load_and_init_actuators = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, actuator_loader_, container); + 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_class_type.c_str()); + } + }; + + load_and_init_actuators(actuators_); } void load_and_initialize_sensor(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, sensor_loader_, sensors_); - initialize_hardware(hardware_info, sensors_.back()); - import_state_interfaces(sensors_.back()); + auto load_and_init_sensors = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, sensor_loader_, container); + 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_class_type.c_str()); + } + }; + + load_and_init_sensors(sensors_); } void load_and_initialize_system(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, system_loader_, systems_); - initialize_hardware(hardware_info, systems_.back()); - import_state_interfaces(systems_.back()); - import_command_interfaces(systems_.back()); + auto load_and_init_systems = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, system_loader_, container); + 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_class_type.c_str()); + } + }; + + load_and_init_systems(systems_); } void initialize_actuator( std::unique_ptr actuator, const HardwareInfo & hardware_info) { - this->actuators_.emplace_back(Actuator(std::move(actuator))); - initialize_hardware(hardware_info, actuators_.back()); - import_state_interfaces(actuators_.back()); - import_command_interfaces(actuators_.back()); + auto init_actuators = [&](auto & container) + { + container.emplace_back(Actuator(std::move(actuator))); + 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_class_type.c_str()); + } + }; + + init_actuators(actuators_); } void initialize_sensor( std::unique_ptr sensor, const HardwareInfo & hardware_info) { - this->sensors_.emplace_back(Sensor(std::move(sensor))); - initialize_hardware(hardware_info, sensors_.back()); - import_state_interfaces(sensors_.back()); + auto init_sensors = [&](auto & container) + { + container.emplace_back(Sensor(std::move(sensor))); + 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_class_type.c_str()); + } + }; + + init_sensors(sensors_); } void initialize_system( std::unique_ptr system, const HardwareInfo & hardware_info) { - this->systems_.emplace_back(System(std::move(system))); - initialize_hardware(hardware_info, systems_.back()); - import_state_interfaces(systems_.back()); - import_command_interfaces(systems_.back()); + auto init_systems = [&](auto & container) + { + container.emplace_back(System(std::move(system))); + 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_class_type.c_str()); + } + }; + + init_systems(systems_); } // hardware plugins