From 460c1a6f8e521e2fa24fd0c16ded9f5f91b2ca98 Mon Sep 17 00:00:00 2001 From: ralphieraccoon Date: Fri, 1 Sep 2023 18:54:00 +0100 Subject: [PATCH 1/5] Now we can switch controllers. --- .../webots_ros2_control/Ros2ControlSystem.hpp | 3 ++ webots_ros2_control/src/Ros2ControlSystem.cpp | 37 +++++++++++++++++++ 2 files changed, 40 insertions(+) diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp index 848774ab4..cafd60db6 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp @@ -61,6 +61,9 @@ namespace webots_ros2_control { std::vector export_state_interfaces() override; std::vector export_command_interfaces() override; + + hardware_interface::return_type perform_command_mode_switch(const std::vector& start_interfaces, const std::vector& stop_interfaces) override; + hardware_interface::return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; hardware_interface::return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; diff --git a/webots_ros2_control/src/Ros2ControlSystem.cpp b/webots_ros2_control/src/Ros2ControlSystem.cpp index 8771c64bd..81ea150a5 100644 --- a/webots_ros2_control/src/Ros2ControlSystem.cpp +++ b/webots_ros2_control/src/Ros2ControlSystem.cpp @@ -133,6 +133,43 @@ namespace webots_ros2_control { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } + hardware_interface::return_type Ros2ControlSystem::perform_command_mode_switch( + const std::vector &start_interfaces, const std::vector &stop_interfaces) { + for (Joint &joint : mJoints) { + for (const std::string &interface_name : stop_interfaces) { + // Clear joint control method bits corresponding to stop interfaces + if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_POSITION)) { + joint.controlPosition = false; + } + if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_VELOCITY)) { + joint.controlVelocity = false; + } + if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_EFFORT)) { + joint.controlEffort = false; + } + } + + // Set joint control method bits corresponding to start interfaces + for (const std::string &interface_name : start_interfaces) { + if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_POSITION)) { + joint.controlPosition = true; + } + if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_VELOCITY)) { + joint.controlVelocity = true; + } + if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_EFFORT)) { + joint.controlEffort = true; + } + } + if (joint.motor && joint.controlVelocity && !joint.controlPosition) { + wb_motor_set_position(joint.motor, INFINITY); + wb_motor_set_velocity(joint.motor, 0.0); + } + } + + return hardware_interface::return_type::OK; + } + hardware_interface::return_type Ros2ControlSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { static double lastReadTime = 0; From 4b2aa16dfe973601a117f0d816006052622b8d21 Mon Sep 17 00:00:00 2001 From: RalphieRaccoon Date: Thu, 5 Oct 2023 18:12:24 +0100 Subject: [PATCH 2/5] Made Ros2ControlSystem.hpp clang-format compliant. --- .../include/webots_ros2_control/Ros2ControlSystem.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp index cafd60db6..d0ae50ba2 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp @@ -62,7 +62,8 @@ namespace webots_ros2_control { std::vector export_state_interfaces() override; std::vector export_command_interfaces() override; - hardware_interface::return_type perform_command_mode_switch(const std::vector& start_interfaces, const std::vector& stop_interfaces) override; + hardware_interface::return_type perform_command_mode_switch(const std::vector &start_interfaces, + const std::vector &stop_interfaces) override; hardware_interface::return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; hardware_interface::return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; From 9c56d77eaf3578ece9febbc51d93941daf9837d4 Mon Sep 17 00:00:00 2001 From: RalphieRaccoon Date: Tue, 14 Nov 2023 18:59:17 +0000 Subject: [PATCH 3/5] Included ResourceManager in Ros2ControlSystem::init() --- .../webots_ros2_control/Ros2ControlSystem.hpp | 3 ++- .../Ros2ControlSystemInterface.hpp | 3 ++- webots_ros2_control/src/Ros2Control.cpp | 2 +- webots_ros2_control/src/Ros2ControlSystem.cpp | 20 ++++++++++--------- 4 files changed, 16 insertions(+), 12 deletions(-) diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp index d0ae50ba2..ab1731a7c 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp +++ b/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; diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp index 56e69a76e..25b86048c 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp @@ -23,6 +23,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 "webots_ros2_driver/PluginInterface.hpp" #include "webots_ros2_driver/WebotsNode.hpp" @@ -30,7 +31,7 @@ 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 diff --git a/webots_ros2_control/src/Ros2Control.cpp b/webots_ros2_control/src/Ros2Control.cpp index fca501e86..a0453dbed 100644 --- a/webots_ros2_control/src/Ros2Control.cpp +++ b/webots_ros2_control/src/Ros2Control.cpp @@ -87,7 +87,7 @@ namespace webots_ros2_control { auto webotsSystem = std::unique_ptr( 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 diff --git a/webots_ros2_control/src/Ros2ControlSystem.cpp b/webots_ros2_control/src/Ros2ControlSystem.cpp index 81ea150a5..0e899768f 100644 --- a/webots_ros2_control/src/Ros2ControlSystem.cpp +++ b/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); From 98b67072157866c090d1684c9b873b897ce57997 Mon Sep 17 00:00:00 2001 From: RalphieRaccoon Date: Tue, 14 Nov 2023 19:05:46 +0000 Subject: [PATCH 4/5] Made clang-format compliant. --- webots_ros2_control/src/Ros2ControlSystem.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/webots_ros2_control/src/Ros2ControlSystem.cpp b/webots_ros2_control/src/Ros2ControlSystem.cpp index 0e899768f..ec8e85018 100644 --- a/webots_ros2_control/src/Ros2ControlSystem.cpp +++ b/webots_ros2_control/src/Ros2ControlSystem.cpp @@ -31,7 +31,8 @@ namespace webots_ros2_control { Ros2ControlSystem::Ros2ControlSystem() { mNode = NULL; } - void Ros2ControlSystem::init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info, const hardware_interface::ResourceManager &resource) { + 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; From ea27318579ecfa018c8b160e6e5e9280b75c49dc Mon Sep 17 00:00:00 2001 From: RalphieRaccoon Date: Tue, 14 Nov 2023 19:08:02 +0000 Subject: [PATCH 5/5] Ros2ControlSystem.hpp Clang-format compliant. --- .../include/webots_ros2_control/Ros2ControlSystem.hpp | 5 +++-- .../webots_ros2_control/Ros2ControlSystemInterface.hpp | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp index ab1731a7c..0f496933a 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp @@ -21,8 +21,8 @@ #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/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/macros.hpp" #include "webots_ros2_driver/PluginInterface.hpp" @@ -51,7 +51,8 @@ namespace webots_ros2_control { class Ros2ControlSystem : public Ros2ControlSystemInterface { public: Ros2ControlSystem(); - void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info, const hardware_interface::ResourceManager &resource) 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; diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp index 25b86048c..49799a2b8 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp @@ -22,8 +22,8 @@ #include #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/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "webots_ros2_driver/PluginInterface.hpp" #include "webots_ros2_driver/WebotsNode.hpp" @@ -31,7 +31,8 @@ namespace webots_ros2_control { class Ros2ControlSystemInterface : public hardware_interface::SystemInterface { public: - virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info, const hardware_interface::ResourceManager &resource) = 0; + virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info, + const hardware_interface::ResourceManager &resource) = 0; }; } // namespace webots_ros2_control