diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp index 848774ab4..1a938e228 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp @@ -54,6 +54,10 @@ namespace webots_ros2_control { rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init( const hardware_interface::HardwareInfo &info) override; +#if HARDWARE_INTERFACE_VERSION_MAJOR > 5 || (HARDWARE_INTERFACE_VERSION_MAJOR == 5 && HARDWARE_INTERFACE_VERSION_MINOR >= 3) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams ¶ms) override; +#endif rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( const rclcpp_lifecycle::State & /*previous_state*/) override; rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( diff --git a/webots_ros2_control/src/Ros2Control.cpp b/webots_ros2_control/src/Ros2Control.cpp index fb60b50fb..67c55c7e2 100644 --- a/webots_ros2_control/src/Ros2Control.cpp +++ b/webots_ros2_control/src/Ros2Control.cpp @@ -36,7 +36,56 @@ const double CONTROLLER_MANAGER_ALLOWED_SAMPLE_ERROR_MS = 1.0; namespace webots_ros2_control { -#if HARDWARE_INTERFACE_VERSION_MAJOR >= 4 && HARDWARE_INTERFACE_VERSION_MINOR >= 12 +#if HARDWARE_INTERFACE_VERSION_MAJOR > 5 || (HARDWARE_INTERFACE_VERSION_MAJOR == 5 && HARDWARE_INTERFACE_VERSION_MINOR >= 3) + class WebotsResourceManager : public hardware_interface::ResourceManager { + public: + WebotsResourceManager(webots_ros2_driver::WebotsNode *node) : + hardware_interface::ResourceManager(node->get_node_clock_interface(), node->get_node_logging_interface()), + mHardwareLoader("webots_ros2_control", "webots_ros2_control::Ros2ControlSystemInterface"), + mLogger(node->get_logger().get_child("WebotsResourceManager")) { + mNode = node; + } + + WebotsResourceManager(const WebotsResourceManager &) = delete; + + bool load_and_initialize_components(const hardware_interface::ResourceManagerParams ¶m) override { + components_are_loaded_and_initialized_ = true; + + std::vector controlHardware; + try { + controlHardware = hardware_interface::parse_control_resources_from_urdf(param.robot_description); + } catch (const std::runtime_error &ex) { + throw std::runtime_error("URDF cannot be parsed by a `ros2_control` component parser: " + std::string(ex.what())); + } + for (unsigned int i = 0; i < controlHardware.size(); i++) { + const std::string pluginName = controlHardware[i].hardware_plugin_name; + + std::unique_ptr webotsSystem; + try { + webotsSystem = std::unique_ptr( + mHardwareLoader.createUnmanagedInstance(pluginName)); + } catch (pluginlib::PluginlibException &ex) { + RCLCPP_ERROR(mLogger, "The plugin failed to load for some reason. Error: %s\n", ex.what()); + continue; + } + + webotsSystem->init(mNode, controlHardware[i]); + hardware_interface::HardwareComponentParams component_params; + component_params.hardware_info = controlHardware[i]; + component_params.clock = mNode->get_clock(); + component_params.logger = mNode->get_logger(); + import_component(std::move(webotsSystem), component_params); + } + + return components_are_loaded_and_initialized_; + } + + private: + webots_ros2_driver::WebotsNode *mNode; + pluginlib::ClassLoader mHardwareLoader; + rclcpp::Logger mLogger; + }; +#elif HARDWARE_INTERFACE_VERSION_MAJOR == 5 || (HARDWARE_INTERFACE_VERSION_MAJOR == 4 && HARDWARE_INTERFACE_VERSION_MINOR >= 12) class WebotsResourceManager : public hardware_interface::ResourceManager { public: WebotsResourceManager(webots_ros2_driver::WebotsNode *node) : @@ -118,7 +167,7 @@ namespace webots_ros2_control { } // Control Hardware -#if HARDWARE_INTERFACE_VERSION_MAJOR >= 4 && HARDWARE_INTERFACE_VERSION_MINOR >= 12 +#if HARDWARE_INTERFACE_VERSION_MAJOR > 4 || (HARDWARE_INTERFACE_VERSION_MAJOR == 4 && HARDWARE_INTERFACE_VERSION_MINOR >= 12) std::unique_ptr resourceManager = std::make_unique(node); #else diff --git a/webots_ros2_control/src/Ros2ControlSystem.cpp b/webots_ros2_control/src/Ros2ControlSystem.cpp index 155bda51c..df6c8c1a7 100644 --- a/webots_ros2_control/src/Ros2ControlSystem.cpp +++ b/webots_ros2_control/src/Ros2ControlSystem.cpp @@ -98,6 +98,16 @@ namespace webots_ros2_control { } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } +#if HARDWARE_INTERFACE_VERSION_MAJOR > 5 || (HARDWARE_INTERFACE_VERSION_MAJOR == 5 && HARDWARE_INTERFACE_VERSION_MINOR >= 3) + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2ControlSystem::on_init( + const hardware_interface::HardwareComponentInterfaceParams ¶ms) { + if (hardware_interface::SystemInterface::on_init(params) != + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } +#endif std::vector Ros2ControlSystem::export_state_interfaces() { std::vector interfaces;