Included ResourceManager in Ros2ControlSystem::init()

Author: ralphieraccoon

webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp

@@ -22,6 +22,7 @@
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/resource_manager.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "rclcpp/macros.hpp"
 #include "webots_ros2_driver/PluginInterface.hpp"
@@ -50,7 +51,7 @@ namespace webots_ros2_control {
   class Ros2ControlSystem : public Ros2ControlSystemInterface {
   public:
     Ros2ControlSystem();
-    void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) override;
+    void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info, const hardware_interface::ResourceManager &resource) override;
 
     rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init(
       const hardware_interface::HardwareInfo &info) override;

webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp

@@ -23,14 +23,15 @@
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/resource_manager.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "webots_ros2_driver/PluginInterface.hpp"
 #include "webots_ros2_driver/WebotsNode.hpp"
 
 namespace webots_ros2_control {
   class Ros2ControlSystemInterface : public hardware_interface::SystemInterface {
   public:
-    virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) = 0;
+    virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info, const hardware_interface::ResourceManager &resource) = 0;
   };
 }  // namespace webots_ros2_control
 

webots_ros2_control/src/Ros2Control.cpp

@@ -87,7 +87,7 @@ namespace webots_ros2_control {
       auto webotsSystem = std::unique_ptr<webots_ros2_control::Ros2ControlSystemInterface>(
         mHardwareLoader->createUnmanagedInstance(hardwareType));
 #endif
-      webotsSystem->init(mNode, controlHardware[i]);
+      webotsSystem->init(mNode, controlHardware[i], *resourceManager);
       resourceManager->import_component(std::move(webotsSystem), controlHardware[i]);
 
 // Configure and activate all components

webots_ros2_control/src/Ros2ControlSystem.cpp

@@ -31,7 +31,7 @@ namespace webots_ros2_control {
   Ros2ControlSystem::Ros2ControlSystem() {
     mNode = NULL;
   }
-  void Ros2ControlSystem::init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) {
+  void Ros2ControlSystem::init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info, const hardware_interface::ResourceManager &resource) {
     mNode = node;
     for (hardware_interface::ComponentInfo component : info.joints) {
       Joint joint;
@@ -64,14 +64,16 @@ namespace webots_ros2_control {
 
       // Configure the command interface
       for (hardware_interface::InterfaceInfo commandInterface : component.command_interfaces) {
-        if (commandInterface.name == "position")
-          joint.controlPosition = true;
-        else if (commandInterface.name == "velocity")
-          joint.controlVelocity = true;
-        else if (commandInterface.name == "effort")
-          joint.controlEffort = true;
-        else
-          throw std::runtime_error("Invalid hardware info name `" + commandInterface.name + "`");
+        if (resource.command_interface_is_claimed(commandInterface.name)) {
+          if (commandInterface.name == "position")
+            joint.controlPosition = true;
+          else if (commandInterface.name == "velocity")
+            joint.controlVelocity = true;
+          else if (commandInterface.name == "effort")
+            joint.controlEffort = true;
+          else
+            throw std::runtime_error("Invalid hardware info name `" + commandInterface.name + "`");
+        }
       }
       if (joint.motor && joint.controlVelocity && !joint.controlPosition) {
         wb_motor_set_position(joint.motor, INFINITY);