From fc424e36117b6acbd8f33b2691008d7a40920132 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 12 Dec 2023 09:24:26 +0100 Subject: [PATCH 1/5] first preperations for storing command-/Systeminterfaces in Components * adjusted handles * add component parser --- .../include/hardware_interface/handle.hpp | 110 +++++++++--------- .../hardware_interface/hardware_info.hpp | 23 +++- .../hardware_interface/system_interface.hpp | 53 ++++++--- .../types/hardware_component_type_values.hpp | 28 +++++ hardware_interface/src/component_parser.cpp | 11 +- .../include/transmission_interface/handle.hpp | 8 +- 6 files changed, 147 insertions(+), 86 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/types/hardware_component_type_values.hpp diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index ec3d6a6d3b..9831c732fc 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include #include #include @@ -25,40 +26,62 @@ namespace hardware_interface { /// A handle used to get and set a value on a given interface. -class ReadOnlyHandle +class Handle { public: - ReadOnlyHandle( + [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] Handle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) { } - explicit ReadOnlyHandle(const std::string & interface_name) + explicit Handle(const InterfaceDescription & interface_description) + : interface_description_(interface_description), + prefix_name_(interface_description.get_prefix_name()), + interface_name_(interface_description.get_interface_type()), + value_(std::numeric_limits::quiet_NaN()), + value_ptr_(&value_) + { + } + + [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] + + explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - explicit ReadOnlyHandle(const char * interface_name) + [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] + + explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; + Handle(const Handle & other) = default; - ReadOnlyHandle(ReadOnlyHandle && other) = default; + Handle(Handle && other) = default; - ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + Handle & operator=(const Handle & other) = default; - ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + Handle & operator=(Handle && other) = default; - virtual ~ReadOnlyHandle() = default; + virtual ~Handle() = default; /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } - const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } + const std::string & get_component_type() const + { + return interface_description_.get_component_type(); + } + + const std::string get_name() const + { + // deprecated is going to be replaced by interface_description_.get_name() + return prefix_name_ + "/" + interface_name_; + } const std::string & get_interface_name() const { return interface_name_; } @@ -69,7 +92,14 @@ class ReadOnlyHandle return get_name(); } - const std::string & get_prefix_name() const { return prefix_name_; } + // Only used for hw side. LoanedStateInterface does not expose this to controllers + void set_value(const double & value) { *value_ptr_ = value; } + + const std::string & get_prefix_name() const + { + // deprecated is going to be replaced by interface_description_.get_prefix_name() + return prefix_name_; + } double get_value() const { @@ -78,49 +108,21 @@ class ReadOnlyHandle } protected: + InterfaceDescription interface_description_; + // deprecated is going to be replaced by InterfaceDescription std::string prefix_name_; + // deprecated is going to be replaced by InterfaceDescription std::string interface_name_; + double value_; + // deprecated is going to be replaced by value_ double * value_ptr_; }; -class ReadWriteHandle : public ReadOnlyHandle -{ -public: - ReadWriteHandle( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) - { - } - - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} - - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} - - ReadWriteHandle(const ReadWriteHandle & other) = default; - - ReadWriteHandle(ReadWriteHandle && other) = default; - - ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; - - ReadWriteHandle & operator=(ReadWriteHandle && other) = default; - - virtual ~ReadWriteHandle() = default; - - void set_value(double value) - { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; - } -}; - -class StateInterface : public ReadOnlyHandle +class StateInterface : public Handle { public: - explicit StateInterface( - const InterfaceDescription & interface_description, double * value_ptr = nullptr) - : ReadOnlyHandle( - interface_description.prefix_name, interface_description.interface_info.name, value_ptr) + explicit StateInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } @@ -128,16 +130,14 @@ class StateInterface : public ReadOnlyHandle StateInterface(StateInterface && other) = default; - using ReadOnlyHandle::ReadOnlyHandle; + using Handle::Handle; }; -class CommandInterface : public ReadWriteHandle +class CommandInterface : public Handle { public: - explicit CommandInterface( - const InterfaceDescription & interface_description, double * value_ptr = nullptr) - : ReadWriteHandle( - interface_description.prefix_name, interface_description.interface_info.name, value_ptr) + explicit CommandInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } /// CommandInterface copy constructor is actively deleted. @@ -148,9 +148,11 @@ class CommandInterface : public ReadWriteHandle */ CommandInterface(const CommandInterface & other) = delete; + CommandInterface & operator=(const Handle & other) = delete; + CommandInterface(CommandInterface && other) = default; - using ReadWriteHandle::ReadWriteHandle; + using Handle::Handle; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index be5ba036f1..9c783aaa1b 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -108,9 +108,15 @@ struct TransmissionInfo */ struct InterfaceDescription { - InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in) - : prefix_name(prefix_name_in), interface_info(interface_info_in) + InterfaceDescription() {} + InterfaceDescription( + const std::string & prefix_name_in, const std::string & component_type_in, + const InterfaceInfo & interface_info_in) + : prefix_name(prefix_name_in), + component_type(component_type_in), + interface_info(interface_info_in) { + name = get_prefix_name() + "/" + get_interface_type(); } /** @@ -123,14 +129,23 @@ struct InterfaceDescription */ std::string component_type; + /** + * Full qualified name of the Interface + */ + std::string name; + /** * Information about the Interface type (position, velocity,...) as well as limits and so on. */ InterfaceInfo interface_info; - std::string get_name() const { return prefix_name + "/" + interface_info.name; } + const std::string & get_prefix_name() const { return prefix_name; } + + const std::string & get_component_type() const { return component_type; } + + const std::string & get_name() const { return name; } - std::string get_interface_type() const { return interface_info.name; } + const std::string & get_interface_type() const { return interface_info.name; } }; /// This structure stores information about hardware defined in a robot's URDF. diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index af6f350c72..4625b6102a 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -19,11 +19,13 @@ #include #include #include +#include #include #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_component_type_values.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" @@ -139,8 +141,17 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI for (const auto & descr : joint_states_descriptions_) { - joint_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); - state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()])); + state_interfaces.emplace_back(StateInterface(descr)); + } + + for (const auto & descr : sensor_states_descriptions_) + { + state_interfaces.emplace_back(StateInterface(descr)); + } + + for (const auto & descr : gpio_states_descriptions_) + { + state_interfaces.emplace_back(StateInterface(descr)); } return state_interfaces; @@ -162,8 +173,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI for (const auto & descr : joint_commands_descriptions_) { - joint_commands_[descr.get_name()] = std::numeric_limits::quiet_NaN(); - command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()])); + command_interfaces.emplace_back(CommandInterface(descr)); + } + + for (const auto & descr : gpio_commands_descriptions_) + { + command_interfaces.emplace_back(CommandInterface(descr)); } return command_interfaces; @@ -251,52 +266,52 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI double joint_state_get_value(const InterfaceDescription & interface_descr) const { - return joint_states_.at(interface_descr.get_name()); + return joint_states_.at(interface_descr.get_name()).get_value(); } void joint_state_set_value(const InterfaceDescription & interface_descr, const double & value) { - joint_states_[interface_descr.get_name()] = value; + joint_states_.at(interface_descr.get_name()).set_value(value); } double joint_command_get_value(const InterfaceDescription & interface_descr) const { - return joint_commands_.at(interface_descr.get_name()); + return joint_commands_.at(interface_descr.get_name()).get_value(); } void joint_command_set_value(const InterfaceDescription & interface_descr, const double & value) { - joint_commands_[interface_descr.get_name()] = value; + joint_commands_.at(interface_descr.get_name()).set_value(value); } double sensor_state_get_value(const InterfaceDescription & interface_descr) const { - return sensor_states_.at(interface_descr.get_name()); + return sensor_states_.at(interface_descr.get_name()).get_value(); } void sensor_state_set_value(const InterfaceDescription & interface_descr, const double & value) { - sensor_states_[interface_descr.get_name()] = value; + sensor_states_.at(interface_descr.get_name()).set_value(value); } double gpio_state_get_value(const InterfaceDescription & interface_descr) const { - return gpio_states_.at(interface_descr.get_name()); + return gpio_states_.at(interface_descr.get_name()).get_value(); } void gpio_state_set_value(const InterfaceDescription & interface_descr, const double & value) { - gpio_states_[interface_descr.get_name()] = value; + gpio_states_.at(interface_descr.get_name()).set_value(value); } double gpio_command_get_value(const InterfaceDescription & interface_descr) const { - return gpio_commands_.at(interface_descr.get_name()); + return gpio_commands_.at(interface_descr.get_name()).get_value(); } void gpio_commands_set_value(const InterfaceDescription & interface_descr, const double & value) { - gpio_commands_[interface_descr.get_name()] = value; + gpio_commands_.at(interface_descr.get_name()).set_value(value); } protected: @@ -310,13 +325,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::vector gpio_commands_descriptions_; private: - std::map joint_states_; - std::map joint_commands_; + std::map joint_states_; + std::map joint_commands_; - std::map sensor_states_; + std::map sensor_states_; - std::map gpio_states_; - std::map gpio_commands_; + std::map gpio_states_; + std::map gpio_commands_; rclcpp_lifecycle::State lifecycle_state_; }; diff --git a/hardware_interface/include/hardware_interface/types/hardware_component_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_component_type_values.hpp new file mode 100644 index 0000000000..a49b7e3b25 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_component_type_values.hpp @@ -0,0 +1,28 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_COMPONENT_TYPE_VALUES_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_COMPONENT_TYPE_VALUES_HPP_ + +namespace hardware_interface +{ +/// Constant defining joint component +constexpr char HW_JOINT_COMPONENT[] = "joint"; +/// Constant defining sensor component +constexpr char HW_SENSOR_COMPONENT[] = "sensor"; +/// Constant defining gpio component +constexpr char HW_GPIO_COMPONENT[] = "gpio"; +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_COMPONENT_TYPE_VALUES_HPP_ diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 2f90e9e26d..9b339931c5 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -23,6 +23,7 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_component_type_values.hpp" namespace { @@ -632,7 +633,7 @@ std::vector parse_joint_state_interface_descriptions_from_ for (const auto & state_interface : joint.state_interfaces) { joint_state_interface_descriptions.emplace_back( - InterfaceDescription(joint.name, state_interface)); + InterfaceDescription(joint.name, HW_JOINT_COMPONENT, state_interface)); } } return joint_state_interface_descriptions; @@ -649,7 +650,7 @@ std::vector parse_sensor_state_interface_descriptions_from for (const auto & state_interface : sensor.state_interfaces) { sensor_state_interface_descriptions.emplace_back( - InterfaceDescription(sensor.name, state_interface)); + InterfaceDescription(sensor.name, HW_SENSOR_COMPONENT, state_interface)); } } return sensor_state_interface_descriptions; @@ -666,7 +667,7 @@ std::vector parse_gpio_state_interface_descriptions_from_h for (const auto & state_interface : gpio.state_interfaces) { gpio_state_interface_descriptions.emplace_back( - InterfaceDescription(gpio.name, state_interface)); + InterfaceDescription(gpio.name, HW_GPIO_COMPONENT, state_interface)); } } return gpio_state_interface_descriptions; @@ -685,7 +686,7 @@ std::vector parse_joint_command_interface_descriptions_fro for (const auto & command_interface : joint.command_interfaces) { gpio_state_intejoint_command_interface_descriptionsrface_descriptions.emplace_back( - InterfaceDescription(joint.name, command_interface)); + InterfaceDescription(joint.name, HW_JOINT_COMPONENT, command_interface)); } } return gpio_state_intejoint_command_interface_descriptionsrface_descriptions; @@ -702,7 +703,7 @@ std::vector parse_gpio_command_interface_descriptions_from for (const auto & command_interface : gpio.command_interfaces) { gpio_command_interface_descriptions.emplace_back( - InterfaceDescription(gpio.name, command_interface)); + InterfaceDescription(gpio.name, HW_GPIO_COMPONENT, command_interface)); } } return gpio_command_interface_descriptions; diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index bc1c0a78d8..7696db03d9 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -22,17 +22,17 @@ namespace transmission_interface { /** A handle used to get and set a value on a given actuator interface. */ -class ActuatorHandle : public hardware_interface::ReadWriteHandle +class ActuatorHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public hardware_interface::ReadWriteHandle +class JointHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; } // namespace transmission_interface From 55a91752ae6d1261b19c41fed8962ca139dcf3ad Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 12 Dec 2023 11:59:01 +0100 Subject: [PATCH 2/5] added methods in resource manager to import descriptions --- .../hardware_interface/resource_manager.hpp | 2 + .../include/hardware_interface/system.hpp | 6 ++ .../hardware_interface/system_interface.hpp | 89 ++++++++++++++++++- hardware_interface/src/resource_manager.cpp | 66 +++++++++++++- hardware_interface/src/system.cpp | 10 +++ 5 files changed, 165 insertions(+), 8 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 92bde14817..39f3bb246b 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -99,6 +99,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool is_urdf_already_loaded() const; + bool component_creates_loaned_state(const std::string & key); + /// Claim a state interface given its key. /** * The resource is claimed as long as being in scope. diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index ece14f814d..aebdfe952d 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -71,6 +71,12 @@ class System final HARDWARE_INTERFACE_PUBLIC std::vector export_command_interfaces(); + HARDWARE_INTERFACE_PUBLIC + std::vector export_state_interface_descriptions(); + + HARDWARE_INTERFACE_PUBLIC + std::vector export_command_interface_descriptions(); + HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( const std::vector & start_interfaces, diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 4625b6102a..72563aafaf 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -104,27 +104,108 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; - add_state_interface_descriptions(); - add_command_interface_descriptions(); + create_state_interfaces(); + create_command_interfaces(); return CallbackReturn::SUCCESS; }; - virtual void add_state_interface_descriptions() + virtual void create_state_interfaces() { + // get the InterfaceDescriptions from the HardwareInfo joint_states_descriptions_ = parse_joint_state_interface_descriptions_from_hardware_info(info_); gpio_states_descriptions_ = parse_gpio_state_interface_descriptions_from_hardware_info(info_); sensor_states_descriptions_ = parse_sensor_state_interface_descriptions_from_hardware_info(info_); + + // create the StateInterfaces based on the before acquired InterfaceDescriptions + // create StateInterfaces for all joints + for (const auto & description : joint_states_descriptions_) + { + joint_states_.insert(std::make_pair(description.get_name(), StateInterface(description))); + } + // create StateInterfaces for all sensors + for (const auto & description : sensor_states_descriptions_) + { + sensor_states_.insert(std::make_pair(description.get_name(), StateInterface(description))); + } + // create StateInterfaces for all gpios + for (const auto & description : gpio_states_descriptions_) + { + gpio_states_.insert(std::make_pair(description.get_name(), StateInterface(description))); + } } - virtual void add_command_interface_descriptions() + virtual void create_command_interfaces() { + // get the InterfaceDescriptions from the HardwareInfo joint_commands_descriptions_ = parse_joint_command_interface_descriptions_from_hardware_info(info_); gpio_commands_descriptions_ = parse_gpio_command_interface_descriptions_from_hardware_info(info_); + + // create the CommandInterfaces based on the before acquired InterfaceDescriptions + // create CommandInterfaces for all joints + for (const auto & description : joint_commands_descriptions_) + { + joint_commands_.insert(std::make_pair(description.get_name(), CommandInterface(description))); + } + // create CommandInterfaces for all gpios + for (const auto & description : gpio_commands_descriptions_) + { + gpio_commands_.insert(std::make_pair(description.get_name(), CommandInterface(description))); + } } + /** + * Exports all information about the available StateInterfaces for this hardware interface. + * + * \return vector of InterfaceDescription + */ + virtual std::vector export_state_interface_descriptions() + { + std::vector state_interface_descriptions; + state_interface_descriptions.reserve( + joint_states_descriptions_.size() + sensor_states_descriptions_.size() + + gpio_states_descriptions_.size()); + + // append InterfaceDescription of joint_states + state_interface_descriptions.insert( + state_interface_descriptions.end(), joint_states_descriptions_.begin(), + joint_states_descriptions_.end()); + // append InterfaceDescription of sensor_states + state_interface_descriptions.insert( + state_interface_descriptions.end(), sensor_states_descriptions_.begin(), + sensor_states_descriptions_.end()); + // append InterfaceDescription of gpio_states + state_interface_descriptions.insert( + state_interface_descriptions.end(), gpio_states_descriptions_.begin(), + gpio_states_descriptions_.end()); + + return state_interface_descriptions; + } + + /** + * Exports all information about the available CommandInterfaces for this hardware interface. + * + * \return vector of InterfaceDescription + */ + virtual std::vector export_command_interface_descriptions() + { + std::vector command_interface_descriptions; + command_interface_descriptions.reserve( + joint_commands_descriptions_.size() + gpio_commands_descriptions_.size()); + + // append InterfaceDescription of joint_commands + command_interface_descriptions.insert( + command_interface_descriptions.end(), joint_commands_descriptions_.begin(), + joint_commands_descriptions_.end()); + // append InterfaceDescription of gpio_commands + command_interface_descriptions.insert( + command_interface_descriptions.end(), gpio_commands_descriptions_.begin(), + gpio_commands_descriptions_.end()); + + return command_interface_descriptions; + } /// Exports all state interfaces for this hardware interface. /** * The state interfaces have to be created and transferred according diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5c5d5af3e6..7cfdfc238e 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -446,6 +446,23 @@ class ResourceStorage available_state_interfaces_.capacity() + interface_names.size()); } + template + void import_state_interface_descriptions(HardwareT & hardware) + { + auto interface_descriptions = hardware.export_state_interface_descriptions(); + std::vector interface_names; + interface_names.reserve(interface_descriptions.size()); + for (auto & description : interface_descriptions) + { + auto key = description.get_name(); + state_interface_descriptions_map_.emplace(std::make_pair(key, description)); + interface_names.push_back(key); + } + hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; + available_state_interfaces_.reserve( + available_state_interfaces_.capacity() + interface_names.size()); + } + template void import_command_interfaces(HardwareT & hardware) { @@ -453,6 +470,14 @@ class ResourceStorage hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } + template + void import_command_interface_descriptions(HardwareT & hardware) + { + auto interface_descriptions = hardware.export_command_interface_descriptions(); + hardware_info_map_[hardware.get_name()].command_interfaces = + add_command_interface_descriptions(interface_descriptions); + } + /// Adds exported command interfaces into internal storage. /** * Add command interfaces to the internal storage. Command interfaces exported from hardware or @@ -481,6 +506,24 @@ class ResourceStorage return interface_names; } + std::vector add_command_interface_descriptions( + std::vector & interface_descriptions) + { + std::vector interface_names; + interface_names.reserve(interface_descriptions.size()); + for (auto & description : interface_descriptions) + { + auto key = description.get_name(); + command_interface_descriptions_map_.emplace(std::make_pair(key, description)); + claimed_command_interface_map_.emplace(std::make_pair(key, false)); + interface_names.push_back(key); + } + available_command_interfaces_.reserve( + available_command_interfaces_.capacity() + interface_names.size()); + + return interface_names; + } + /// Removes command interfaces from internal storage. /** * Command interface are removed from the maps with theirs storage and their claimed status. @@ -575,8 +618,8 @@ class ResourceStorage load_hardware(hardware_info, system_loader_, container); if (initialize_hardware(hardware_info, container.back())) { - import_state_interfaces(container.back()); - import_command_interfaces(container.back()); + import_state_interface_descriptions(container.back()); + import_command_interface_descriptions(container.back()); } else { @@ -664,8 +707,8 @@ class ResourceStorage container.emplace_back(System(std::move(system))); if (initialize_hardware(hardware_info, container.back())) { - import_state_interfaces(container.back()); - import_command_interfaces(container.back()); + import_state_interface_descriptions(container.back()); + import_command_interface_descriptions(container.back()); } else { @@ -709,8 +752,10 @@ class ResourceStorage /// Storage of all available state interfaces std::map state_interface_map_; + std::map state_interface_descriptions_map_; /// Storage of all available command interfaces std::map command_interface_map_; + std::map command_interface_descriptions_map_; /// Vectors with interfaces available to controllers (depending on hardware component state) std::vector available_state_interfaces_; @@ -796,6 +841,13 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } +bool ResourceManager::component_creates_loaned_state(const std::string & key) +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->state_interface_descriptions_map_.find(key) != + resource_storage_->state_interface_descriptions_map_.end(); +} + // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) { @@ -805,6 +857,12 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string & } std::lock_guard guard(resource_interfaces_lock_); + // TODO(Manuel) just a hack for testing have to be removed later on when all the components create + // the loans. + if (component_creates_loaned_state(key)) + { + // TOD(Manuel) implement exporting of loans by components + } return LoanedStateInterface(resource_storage_->state_interface_map_.at(key)); } diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index f8703a47bc..aff18c32b6 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -184,6 +184,16 @@ const rclcpp_lifecycle::State & System::error() return impl_->get_state(); } +std::vector System::export_state_interface_descriptions() +{ + return impl_->export_state_interface_descriptions(); +} + +std::vector System::export_command_interface_descriptions() +{ + return impl_->export_command_interface_descriptions(); +} + std::vector System::export_state_interfaces() { return impl_->export_state_interfaces(); From d33d67b5069f70e6af5635bcefa7e74aee084fbb Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 15 Dec 2023 09:22:16 +0100 Subject: [PATCH 3/5] add export_of_description and creation of loans for system --- .../include/hardware_interface/actuator.hpp | 8 + .../hardware_interface/actuator_interface.hpp | 20 ++ .../include/hardware_interface/sensor.hpp | 4 + .../hardware_interface/sensor_interface.hpp | 12 + .../include/hardware_interface/system.hpp | 8 + .../hardware_interface/system_interface.hpp | 16 +- hardware_interface/src/actuator.cpp | 10 + hardware_interface/src/resource_manager.cpp | 219 +++++++++++++----- hardware_interface/src/sensor.cpp | 5 + hardware_interface/src/system.cpp | 10 + 10 files changed, 258 insertions(+), 54 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 4082863370..f5de610117 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -21,6 +21,8 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" #include "rclcpp/duration.hpp" @@ -70,6 +72,12 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC std::vector export_command_interfaces(); + HARDWARE_INTERFACE_PUBLIC + LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name); + + HARDWARE_INTERFACE_PUBLIC + LoanedStateInterface create_loaned_state_interface(const std::string & interface_name); + HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( const std::vector & start_interfaces, diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index abfd8eb45a..0716cdd4b2 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -15,12 +15,15 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ +#include #include #include #include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -122,6 +125,16 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual std::vector export_command_interfaces() = 0; + virtual LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name) + { + return LoanedCommandInterface(joint_commands_.at(interface_name)); + } + + virtual LoanedStateInterface create_loaned_state_interface(const std::string & interface_name) + { + return LoanedStateInterface(joint_states_.at(interface_name)); + } + /// Prepare for a new command interface switch. /** * Prepare for any mode-switching required by the new command interface combination. @@ -204,6 +217,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod protected: HardwareInfo info_; + std::vector joint_states_descriptions_; + std::vector joint_commands_descriptions_; + +private: + std::map joint_states_; + std::map joint_commands_; + rclcpp_lifecycle::State lifecycle_state_; }; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 5d0677c587..c9384c5246 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -22,6 +22,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" #include "rclcpp/duration.hpp" @@ -68,6 +69,9 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC std::vector export_state_interfaces(); + HARDWARE_INTERFACE_PUBLIC + LoanedStateInterface create_loaned_state_interface(const std::string & interface_name); + HARDWARE_INTERFACE_PUBLIC std::string get_name() const; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 14a59e4588..a3d45c6dfe 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -15,12 +15,14 @@ #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ +#include #include #include #include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -111,6 +113,11 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual std::vector export_state_interfaces() = 0; + virtual LoanedStateInterface create_loaned_state_interface(const std::string & interface_name) + { + return LoanedStateInterface(sensor_states_.at(interface_name)); + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -143,6 +150,11 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: HardwareInfo info_; + std::vector sensor_states_descriptions_; + +private: + std::map sensor_states_; + rclcpp_lifecycle::State lifecycle_state_; }; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index aebdfe952d..85eea550d4 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -22,6 +22,8 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" #include "rclcpp/duration.hpp" @@ -77,6 +79,12 @@ class System final HARDWARE_INTERFACE_PUBLIC std::vector export_command_interface_descriptions(); + HARDWARE_INTERFACE_PUBLIC + LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name); + + HARDWARE_INTERFACE_PUBLIC + LoanedStateInterface create_loaned_state_interface(const std::string & interface_name); + HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( const std::vector & start_interfaces, diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 72563aafaf..7b54495a81 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -25,6 +25,8 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_component_type_values.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -161,7 +163,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of InterfaceDescription */ - virtual std::vector export_state_interface_descriptions() + virtual std::vector export_state_interface_descriptions() const { std::vector state_interface_descriptions; state_interface_descriptions.reserve( @@ -189,7 +191,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of InterfaceDescription */ - virtual std::vector export_command_interface_descriptions() + virtual std::vector export_command_interface_descriptions() const { std::vector command_interface_descriptions; command_interface_descriptions.reserve( @@ -265,6 +267,16 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return command_interfaces; } + virtual LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name) + { + return LoanedCommandInterface(joint_commands_.at(interface_name)); + } + + virtual LoanedStateInterface create_loaned_state_interface(const std::string & interface_name) + { + return LoanedStateInterface(joint_states_.at(interface_name)); + } + /// Prepare for a new command interface switch. /** * Prepare for any mode-switching required by the new command interface combination. diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 694e92355e..bfde7d92b5 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -198,6 +198,16 @@ std::vector Actuator::export_command_interfaces() return impl_->export_command_interfaces(); } +LoanedCommandInterface Actuator::create_loaned_command_interface(const std::string & interface_name) +{ + return impl_->create_loaned_command_interface(interface_name); +} + +LoanedStateInterface Actuator::create_loaned_state_interface(const std::string & interface_name) +{ + return impl_->create_loaned_state_interface(interface_name); +} + return_type Actuator::prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 7cfdfc238e..b267890334 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -89,10 +89,11 @@ class ResourceStorage { } + // TODO(Manuel): review regarding simplifications and checking for duplicates template - void load_hardware( + std::string load_hardware( const HardwareInfo & hardware_info, pluginlib::ClassLoader & loader, - std::vector & container) + std::map & container) { RCUTILS_LOG_INFO_NAMED( "resource_manager", "Loading hardware '%s' ", hardware_info.name.c_str()); @@ -100,17 +101,21 @@ class ResourceStorage auto interface = std::unique_ptr( loader.createUnmanagedInstance(hardware_info.hardware_plugin_name)); HardwareT hardware(std::move(interface)); - container.emplace_back(std::move(hardware)); // initialize static data about hardware component to reduce later calls HardwareComponentInfo component_info; component_info.name = hardware_info.name; component_info.type = hardware_info.type; component_info.plugin_name = hardware_info.hardware_plugin_name; component_info.is_async = hardware_info.is_async; - hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); hardware_used_by_controllers_.insert( std::make_pair(component_info.name, std::vector())); + + // TODO(Manuel): review regarding simplifications and checking for duplicates + std::string hw_name = hardware.get_name(); + assert(hw_name == component_info.name); + container.insert(std::make_pair(hw_name, std::move(hardware))); + return hw_name; } template @@ -446,16 +451,17 @@ class ResourceStorage available_state_interfaces_.capacity() + interface_names.size()); } - template - void import_state_interface_descriptions(HardwareT & hardware) + void import_system_state_interface_descriptions(System & hardware) { auto interface_descriptions = hardware.export_state_interface_descriptions(); std::vector interface_names; interface_names.reserve(interface_descriptions.size()); + std::string hw_name = hardware.get_name(); for (auto & description : interface_descriptions) { auto key = description.get_name(); - state_interface_descriptions_map_.emplace(std::make_pair(key, description)); + state_interface_descriptions_.emplace(std::make_pair(key, description)); + state_interface_to_component_name_.emplace(std::make_pair(key, hw_name)); interface_names.push_back(key); } hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; @@ -514,7 +520,7 @@ class ResourceStorage for (auto & description : interface_descriptions) { auto key = description.get_name(); - command_interface_descriptions_map_.emplace(std::make_pair(key, description)); + command_interface_descriptions_.emplace(std::make_pair(key, description)); claimed_command_interface_map_.emplace(std::make_pair(key, false)); interface_names.push_back(key); } @@ -551,16 +557,18 @@ class ResourceStorage } // TODO(destogl): Propagate "false" up, if happens in initialize_hardware + // TODO(Manuel): review regarding simplifications and checking for duplicates void load_and_initialize_actuator(const HardwareInfo & hardware_info) { 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())) + std::string loaded_hw = + load_hardware(hardware_info, actuator_loader_, container); + if (initialize_hardware(hardware_info, container.at(loaded_hw))) { - import_state_interfaces(container.back()); - import_command_interfaces(container.back()); + import_state_interfaces(container.at(loaded_hw)); + import_command_interfaces(container.at(loaded_hw)); } else { @@ -581,15 +589,17 @@ class ResourceStorage } } + // TODO(Manuel): review regarding simplifications and checking for duplicates void load_and_initialize_sensor(const HardwareInfo & hardware_info) { 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())) + std::string loaded_hw = + load_hardware(hardware_info, sensor_loader_, container); + if (initialize_hardware(hardware_info, container.at(loaded_hw))) { - import_state_interfaces(container.back()); + import_state_interfaces(container.at(loaded_hw)); } else { @@ -610,16 +620,18 @@ class ResourceStorage } } + // TODO(Manuel): review regarding simplifications and checking for duplicates void load_and_initialize_system(const HardwareInfo & hardware_info) { 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())) + std::string loaded_hw = + load_hardware(hardware_info, system_loader_, container); + if (initialize_hardware(hardware_info, container.at(loaded_hw))) { - import_state_interface_descriptions(container.back()); - import_command_interface_descriptions(container.back()); + import_system_state_interface_descriptions(container.at(loaded_hw)); + import_command_interface_descriptions(container.at(loaded_hw)); } else { @@ -640,16 +652,19 @@ class ResourceStorage } } + // TODO(Manuel): review regarding simplifications and checking for duplicates void initialize_actuator( std::unique_ptr actuator, const HardwareInfo & hardware_info) { auto init_actuators = [&](auto & container) { - container.emplace_back(Actuator(std::move(actuator))); - if (initialize_hardware(hardware_info, container.back())) + Actuator act(std::move(actuator)); + std::string hw_name = act.get_name(); + container.insert(std::make_pair(hw_name, std::move(act))); + if (initialize_hardware(hardware_info, container.at(hw_name))) { - import_state_interfaces(container.back()); - import_command_interfaces(container.back()); + import_state_interfaces(container.at(hw_name)); + import_command_interfaces(container.at(hw_name)); } else { @@ -675,10 +690,12 @@ class ResourceStorage { auto init_sensors = [&](auto & container) { - container.emplace_back(Sensor(std::move(sensor))); - if (initialize_hardware(hardware_info, container.back())) + Sensor sens(std::move(sensor)); + std::string hw_name = sens.get_name(); + container.insert(std::make_pair(hw_name, std::move(sens))); + if (initialize_hardware(hardware_info, container.at(hw_name))) { - import_state_interfaces(container.back()); + import_state_interfaces(container.at(hw_name)); } else { @@ -704,11 +721,13 @@ class ResourceStorage { auto init_systems = [&](auto & container) { - container.emplace_back(System(std::move(system))); - if (initialize_hardware(hardware_info, container.back())) + System sys(std::move(system)); + std::string hw_name = sys.get_name(); + container.insert(std::make_pair(hw_name, std::move(sys))); + if (initialize_hardware(hardware_info, container.at(hw_name))) { - import_state_interface_descriptions(container.back()); - import_command_interface_descriptions(container.back()); + import_system_state_interface_descriptions(container.at(hw_name)); + import_command_interface_descriptions(container.at(hw_name)); } else { @@ -729,18 +748,114 @@ class ResourceStorage } } + template + std::pair find_component( + const std::string & state_interface_name, Container & components) + { + auto found_component_it = components.find(state_interface_name); + return {found_component_it != components.end(), found_component_it}; + } + + template + std::pair find_component_name_for_interface( + const std::string & interface_name, Container & components) + { + auto component_name_it = components.find(interface_name); + if (component_name_it != components.end()) + { + return {true, component_name_it->second}; + } + return {false, ""}; + } + + LoanedStateInterface get_loand_state_interface(const std::string state_interface_name) + { + auto [found, component_name] = + find_component_name_for_interface(state_interface_name, state_interface_to_component_name_); + if (!found) + { + // TODO(Manuel) we should probably throw + } + + auto [found_act, actutator] = find_component(component_name, actuators_); + if (found_act) + { + return actutator->second.create_loaned_state_interface(state_interface_name); + } + auto [found_sens, sensor] = find_component(component_name, sensors_); + if (found_sens) + { + return sensor->second.create_loaned_state_interface(state_interface_name); + } + auto [found_sys, system] = find_component(component_name, systems_); + if (found_sys) + { + return system->second.create_loaned_state_interface(state_interface_name); + } + auto [found_async_act, async_acturator] = find_component(component_name, async_actuators_); + if (found_async_act) + { + return async_acturator->second.create_loaned_state_interface(state_interface_name); + } + auto [found_async_sens, asysnc_sensor] = find_component(component_name, async_sensors_); + if (found_async_sens) + { + return asysnc_sensor->second.create_loaned_state_interface(state_interface_name); + } + auto [found_async_sys, async_sys] = find_component(component_name, async_systems_); + // Inverted so that compiler is not complaining that we don't return anything... + if (!found_async_sys) + { + // TODO(Manuel) we should probably throw here something + } + return async_sys->second.create_loaned_state_interface(state_interface_name); + } + + LoanedCommandInterface get_loaned_command_interface(const std::string command_interface_name) + { + auto [found, component_name] = find_component_name_for_interface( + command_interface_name, command_interface_to_component_name_); + if (!found) + { + // TODO(Manuel) we should probably throw + } + + auto [found_act, actutator] = find_component(component_name, actuators_); + if (found_act) + { + return actutator->second.create_loaned_command_interface(command_interface_name); + } + auto [found_sys, system] = find_component(component_name, systems_); + if (found_sys) + { + return system->second.create_loaned_command_interface(command_interface_name); + } + auto [found_async_act, async_acturator] = find_component(component_name, async_actuators_); + if (found_async_act) + { + return async_acturator->second.create_loaned_command_interface(command_interface_name); + } + auto [found_async_sys, async_sys] = find_component(component_name, async_systems_); + // Inverted so that compiler is not complaining that we don't return anything... + if (!found_async_sys) + { + // TODO(Manuel) we should probably throw here something + } + return async_sys->second.create_loaned_command_interface(command_interface_name); + } + // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; pluginlib::ClassLoader system_loader_; - std::vector actuators_; - std::vector sensors_; - std::vector systems_; + std::map actuators_; + std::map sensors_; + std::map systems_; - std::vector async_actuators_; - std::vector async_sensors_; - std::vector async_systems_; + std::map async_actuators_; + std::map async_sensors_; + std::map async_systems_; std::unordered_map hardware_info_map_; @@ -752,10 +867,12 @@ class ResourceStorage /// Storage of all available state interfaces std::map state_interface_map_; - std::map state_interface_descriptions_map_; + std::map state_interface_descriptions_; + std::map state_interface_to_component_name_; /// Storage of all available command interfaces std::map command_interface_map_; - std::map command_interface_descriptions_map_; + std::map command_interface_descriptions_; + std::map command_interface_to_component_name_; /// Vectors with interfaces available to controllers (depending on hardware component state) std::vector available_state_interfaces_; @@ -844,8 +961,8 @@ bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; bool ResourceManager::component_creates_loaned_state(const std::string & key) { std::lock_guard guard(resource_interfaces_lock_); - return resource_storage_->state_interface_descriptions_map_.find(key) != - resource_storage_->state_interface_descriptions_map_.end(); + return resource_storage_->state_interface_descriptions_.find(key) != + resource_storage_->state_interface_descriptions_.end(); } // CM API: Called in "update"-thread @@ -861,7 +978,7 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string & // the loans. if (component_creates_loaned_state(key)) { - // TOD(Manuel) implement exporting of loans by components + return resource_storage_->get_loand_state_interface(key); } return LoanedStateInterface(resource_storage_->state_interface_map_.at(key)); } @@ -1109,7 +1226,7 @@ std::unordered_map ResourceManager::get_comp { auto loop_and_get_state = [&](auto & container) { - for (auto & component : container) + for (const auto & [key, component] : container) { resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); } @@ -1148,7 +1265,7 @@ bool ResourceManager::prepare_command_mode_switch( return ss.str(); }; - for (auto & component : resource_storage_->actuators_) + for (auto & [key, component] : resource_storage_->actuators_) { if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) { @@ -1158,7 +1275,7 @@ bool ResourceManager::prepare_command_mode_switch( return false; } } - for (auto & component : resource_storage_->systems_) + for (auto & [key, component] : resource_storage_->systems_) { if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) { @@ -1176,7 +1293,7 @@ bool ResourceManager::perform_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) { - for (auto & component : resource_storage_->actuators_) + for (auto & [key, component] : resource_storage_->actuators_) { if (return_type::OK != component.perform_command_mode_switch(start_interfaces, stop_interfaces)) { @@ -1186,7 +1303,7 @@ bool ResourceManager::perform_command_mode_switch( return false; } } - for (auto & component : resource_storage_->systems_) + for (auto & [key, component] : resource_storage_->systems_) { if (return_type::OK != component.perform_command_mode_switch(start_interfaces, stop_interfaces)) { @@ -1245,13 +1362,11 @@ return_type ResourceManager::set_component_state( auto find_set_component_state = [&](auto action, auto & components) { - auto found_component_it = std::find_if( - components.begin(), components.end(), - [&](const auto & component) { return component.get_name() == component_name; }); + auto found_component_it = components.find(component_name); if (found_component_it != components.end()) { - if (action(*found_component_it, target_state)) + if (action(found_component_it->second, target_state)) { result = return_type::OK; } @@ -1311,7 +1426,7 @@ HardwareReadWriteStatus ResourceManager::read( auto read_components = [&](auto & components) { - for (auto & component : components) + for (auto & [key, component] : components) { auto ret_val = component.read(time, period); if (ret_val == return_type::ERROR) @@ -1351,7 +1466,7 @@ HardwareReadWriteStatus ResourceManager::write( auto write_components = [&](auto & components) { - for (auto & component : components) + for (auto & [key, component] : components) { auto ret_val = component.write(time, period); if (ret_val == return_type::ERROR) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index ade9b8781a..72ee37d5f1 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -189,6 +189,11 @@ std::vector Sensor::export_state_interfaces() return impl_->export_state_interfaces(); } +LoanedStateInterface Sensor::create_loaned_state_interface(const std::string & interface_name) +{ + return impl_->create_loaned_state_interface(interface_name); +} + std::string Sensor::get_name() const { return impl_->get_name(); } const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_state(); } diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index aff18c32b6..f9a27c5bd4 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -204,6 +204,16 @@ std::vector System::export_command_interfaces() return impl_->export_command_interfaces(); } +LoanedCommandInterface System::create_loaned_command_interface(const std::string & interface_name) +{ + return impl_->create_loaned_command_interface(interface_name); +} + +LoanedStateInterface System::create_loaned_state_interface(const std::string & interface_name) +{ + return impl_->create_loaned_state_interface(interface_name); +} + return_type System::prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) From 700d04f9ff4a017e2be7c9893bb7517014be6bc3 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 15 Dec 2023 14:45:37 +0100 Subject: [PATCH 4/5] ! there is a bug with multiple cleanupcalls! add exporting for all components, creatons of loans --- .../include/hardware_interface/actuator.hpp | 9 +- .../hardware_interface/actuator_interface.hpp | 46 ++++++- .../hardware_interface/hardware_info.hpp | 20 +-- .../hardware_interface/resource_manager.hpp | 4 +- .../include/hardware_interface/sensor.hpp | 3 + .../hardware_interface/sensor_interface.hpp | 20 +++ .../include/hardware_interface/system.hpp | 3 +- .../hardware_interface/system_interface.hpp | 116 +++++++++--------- hardware_interface/src/actuator.cpp | 15 ++- hardware_interface/src/resource_manager.cpp | 95 +++++++++++--- hardware_interface/src/sensor.cpp | 5 + hardware_interface/src/system.cpp | 5 +- 12 files changed, 248 insertions(+), 93 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index f5de610117..69bd17c930 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -73,7 +73,14 @@ class Actuator final std::vector export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC - LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name); + std::vector export_state_interface_descriptions(); + + HARDWARE_INTERFACE_PUBLIC + std::vector export_command_interface_descriptions(); + + HARDWARE_INTERFACE_PUBLIC + LoanedCommandInterface create_loaned_command_interface( + const std::string & interface_name, std::function && release_callback); HARDWARE_INTERFACE_PUBLIC LoanedStateInterface create_loaned_state_interface(const std::string & interface_name); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 0716cdd4b2..ffbb9b0c99 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "hardware_interface/handle.hpp" @@ -125,9 +126,30 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual std::vector export_command_interfaces() = 0; - virtual LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name) + /** + * Exports all information about the available StateInterfaces for this hardware interface. + * + * \return vector of InterfaceDescription + */ + virtual std::vector export_state_interface_descriptions() const + { + return joint_states_descriptions_; + } + + /** + * Exports all information about the available CommandInterfaces for this hardware interface. + * + * \return vector of InterfaceDescription + */ + virtual std::vector export_command_interface_descriptions() const + { + return joint_commands_descriptions_; + } + + virtual LoanedCommandInterface create_loaned_command_interface( + const std::string & interface_name, std::function && release_callback) { - return LoanedCommandInterface(joint_commands_.at(interface_name)); + return LoanedCommandInterface(joint_commands_.at(interface_name), std::move(release_callback)); } virtual LoanedStateInterface create_loaned_state_interface(const std::string & interface_name) @@ -215,6 +237,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + double joint_state_get_value(const InterfaceDescription & interface_descr) const + { + return joint_states_.at(interface_descr.get_name()).get_value(); + } + + void joint_state_set_value(const InterfaceDescription & interface_descr, const double & value) + { + joint_states_.at(interface_descr.get_name()).set_value(value); + } + + double joint_command_get_value(const InterfaceDescription & interface_descr) const + { + return joint_commands_.at(interface_descr.get_name()).get_value(); + } + + void joint_command_set_value(const InterfaceDescription & interface_descr, const double & value) + { + joint_commands_.at(interface_descr.get_name()).set_value(value); + } + protected: HardwareInfo info_; std::vector joint_states_descriptions_; diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 9c783aaa1b..8d4ebf0bf1 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -114,9 +114,9 @@ struct InterfaceDescription const InterfaceInfo & interface_info_in) : prefix_name(prefix_name_in), component_type(component_type_in), - interface_info(interface_info_in) + interface_info(interface_info_in), + name(get_prefix_name() + "/" + get_interface_type()) { - name = get_prefix_name() + "/" + get_interface_type(); } /** @@ -130,22 +130,22 @@ struct InterfaceDescription std::string component_type; /** - * Full qualified name of the Interface + * Information about the Interface type (position, velocity,...) as well as limits and so on. */ - std::string name; + InterfaceInfo interface_info; /** - * Information about the Interface type (position, velocity,...) as well as limits and so on. + * Full qualified name of the Interface */ - InterfaceInfo interface_info; + std::string name; - const std::string & get_prefix_name() const { return prefix_name; } + std::string get_prefix_name() const { return prefix_name; } - const std::string & get_component_type() const { return component_type; } + std::string get_component_type() const { return component_type; } - const std::string & get_name() const { return name; } + std::string get_name() const { return name; } - const std::string & get_interface_type() const { return interface_info.name; } + std::string get_interface_type() const { return interface_info.name; } }; /// This structure stores information about hardware defined in a robot's URDF. diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 39f3bb246b..2ddba7e098 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -99,7 +99,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool is_urdf_already_loaded() const; - bool component_creates_loaned_state(const std::string & key); + bool component_creates_loaned_state_interface(const std::string & key); + + bool component_creates_loaned_command_interface(const std::string & key); /// Claim a state interface given its key. /** diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index c9384c5246..099572ba7e 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -69,6 +69,9 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC std::vector export_state_interfaces(); + HARDWARE_INTERFACE_PUBLIC + std::vector export_state_interface_descriptions(); + HARDWARE_INTERFACE_PUBLIC LoanedStateInterface create_loaned_state_interface(const std::string & interface_name); diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index a3d45c6dfe..4a757693a6 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -113,6 +113,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual std::vector export_state_interfaces() = 0; + /** + * Exports all information about the available StateInterfaces for this hardware interface. + * + * \return vector of InterfaceDescription + */ + virtual std::vector export_state_interface_descriptions() const + { + return sensor_states_descriptions_; + } + virtual LoanedStateInterface create_loaned_state_interface(const std::string & interface_name) { return LoanedStateInterface(sensor_states_.at(interface_name)); @@ -148,6 +158,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + double sensor_state_get_value(const InterfaceDescription & interface_descr) const + { + return sensor_states_.at(interface_descr.get_name()).get_value(); + } + + void sensor_state_set_value(const InterfaceDescription & interface_descr, const double & value) + { + sensor_states_.at(interface_descr.get_name()).set_value(value); + } + protected: HardwareInfo info_; std::vector sensor_states_descriptions_; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 85eea550d4..0a87af4169 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -80,7 +80,8 @@ class System final std::vector export_command_interface_descriptions(); HARDWARE_INTERFACE_PUBLIC - LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name); + LoanedCommandInterface create_loaned_command_interface( + const std::string & interface_name, std::function && release_callback); HARDWARE_INTERFACE_PUBLIC LoanedStateInterface create_loaned_state_interface(const std::string & interface_name); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 7b54495a81..29bee17dc6 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -158,56 +158,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI } } - /** - * Exports all information about the available StateInterfaces for this hardware interface. - * - * \return vector of InterfaceDescription - */ - virtual std::vector export_state_interface_descriptions() const - { - std::vector state_interface_descriptions; - state_interface_descriptions.reserve( - joint_states_descriptions_.size() + sensor_states_descriptions_.size() + - gpio_states_descriptions_.size()); - - // append InterfaceDescription of joint_states - state_interface_descriptions.insert( - state_interface_descriptions.end(), joint_states_descriptions_.begin(), - joint_states_descriptions_.end()); - // append InterfaceDescription of sensor_states - state_interface_descriptions.insert( - state_interface_descriptions.end(), sensor_states_descriptions_.begin(), - sensor_states_descriptions_.end()); - // append InterfaceDescription of gpio_states - state_interface_descriptions.insert( - state_interface_descriptions.end(), gpio_states_descriptions_.begin(), - gpio_states_descriptions_.end()); - - return state_interface_descriptions; - } - - /** - * Exports all information about the available CommandInterfaces for this hardware interface. - * - * \return vector of InterfaceDescription - */ - virtual std::vector export_command_interface_descriptions() const - { - std::vector command_interface_descriptions; - command_interface_descriptions.reserve( - joint_commands_descriptions_.size() + gpio_commands_descriptions_.size()); - - // append InterfaceDescription of joint_commands - command_interface_descriptions.insert( - command_interface_descriptions.end(), joint_commands_descriptions_.begin(), - joint_commands_descriptions_.end()); - // append InterfaceDescription of gpio_commands - command_interface_descriptions.insert( - command_interface_descriptions.end(), gpio_commands_descriptions_.begin(), - gpio_commands_descriptions_.end()); - - return command_interface_descriptions; - } /// Exports all state interfaces for this hardware interface. /** * The state interfaces have to be created and transferred according @@ -267,9 +217,61 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return command_interfaces; } - virtual LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name) + /** + * Exports all information about the available StateInterfaces for this hardware interface. + * + * \return vector of InterfaceDescription + */ + virtual std::vector export_state_interface_descriptions() const + { + std::vector state_interface_descriptions; + state_interface_descriptions.reserve( + joint_states_descriptions_.size() + sensor_states_descriptions_.size() + + gpio_states_descriptions_.size()); + + // append InterfaceDescription of joint_states + state_interface_descriptions.insert( + state_interface_descriptions.end(), joint_states_descriptions_.begin(), + joint_states_descriptions_.end()); + // append InterfaceDescription of sensor_states + state_interface_descriptions.insert( + state_interface_descriptions.end(), sensor_states_descriptions_.begin(), + sensor_states_descriptions_.end()); + // append InterfaceDescription of gpio_states + state_interface_descriptions.insert( + state_interface_descriptions.end(), gpio_states_descriptions_.begin(), + gpio_states_descriptions_.end()); + + return state_interface_descriptions; + } + + /** + * Exports all information about the available CommandInterfaces for this hardware interface. + * + * \return vector of InterfaceDescription + */ + virtual std::vector export_command_interface_descriptions() const + { + std::vector command_interface_descriptions; + command_interface_descriptions.reserve( + joint_commands_descriptions_.size() + gpio_commands_descriptions_.size()); + + // append InterfaceDescription of joint_commands + command_interface_descriptions.insert( + command_interface_descriptions.end(), joint_commands_descriptions_.begin(), + joint_commands_descriptions_.end()); + // append InterfaceDescription of gpio_commands + command_interface_descriptions.insert( + command_interface_descriptions.end(), gpio_commands_descriptions_.begin(), + gpio_commands_descriptions_.end()); + + return command_interface_descriptions; + } + + virtual LoanedCommandInterface create_loaned_command_interface( + const std::string & interface_name, std::function && release_callback) { - return LoanedCommandInterface(joint_commands_.at(interface_name)); + return LoanedCommandInterface(joint_commands_.at(interface_name), std::move(release_callback)); } virtual LoanedStateInterface create_loaned_state_interface(const std::string & interface_name) @@ -316,7 +318,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return return_type::OK; } - /// Read the current state values from the actuator. + /// Read the current state values from the system. /** * The data readings from the physical hardware has to be updated * and reflected accordingly in the exported state interfaces. @@ -328,7 +330,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - /// Write the current command values to the actuator. + /// Write the current command values to the system. /** * The physical hardware shall be updated with the latest value from * the exported command interfaces. @@ -339,19 +341,19 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - /// Get name of the actuator hardware. + /// Get name of the system hardware. /** * \return name. */ virtual std::string get_name() const { return info_.name; } - /// Get life-cycle state of the actuator hardware. + /// Get life-cycle state of the system hardware. /** * \return state. */ const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } - /// Set life-cycle state of the actuator hardware. + /// Set life-cycle state of the system hardware. /** * \return state. */ diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index bfde7d92b5..ea7d01834f 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -198,9 +198,20 @@ std::vector Actuator::export_command_interfaces() return impl_->export_command_interfaces(); } -LoanedCommandInterface Actuator::create_loaned_command_interface(const std::string & interface_name) +std::vector Actuator::export_state_interface_descriptions() { - return impl_->create_loaned_command_interface(interface_name); + return impl_->export_state_interface_descriptions(); +} + +std::vector Actuator::export_command_interface_descriptions() +{ + return impl_->export_command_interface_descriptions(); +} + +LoanedCommandInterface Actuator::create_loaned_command_interface( + const std::string & interface_name, std::function && release_callback) +{ + return impl_->create_loaned_command_interface(interface_name, std::move(release_callback)); } LoanedStateInterface Actuator::create_loaned_state_interface(const std::string & interface_name) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index b267890334..ff07da6766 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -36,6 +36,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" +#include "rclcpp/rclcpp.hpp" #include "rcutils/logging_macros.h" namespace hardware_interface @@ -272,6 +273,9 @@ class ResourceStorage template bool cleanup_hardware(HardwareT & hardware) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("resource_manager"), "Cleanup component {" << hardware.get_name() << "}"); + bool result = trigger_and_print_hardware_state_transition( std::bind(&HardwareT::cleanup, &hardware), "cleanup", hardware.get_name(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); @@ -341,6 +345,8 @@ class ResourceStorage using lifecycle_msgs::msg::State; bool result = false; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("resource_manager"), "Found component {" << hardware.get_name() << "}"); switch (target_state.id()) { @@ -451,7 +457,8 @@ class ResourceStorage available_state_interfaces_.capacity() + interface_names.size()); } - void import_system_state_interface_descriptions(System & hardware) + template + void import_state_interface_descriptions(HardwareT & hardware) { auto interface_descriptions = hardware.export_state_interface_descriptions(); std::vector interface_names; @@ -462,9 +469,22 @@ class ResourceStorage auto key = description.get_name(); state_interface_descriptions_.emplace(std::make_pair(key, description)); state_interface_to_component_name_.emplace(std::make_pair(key, hw_name)); + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("resource_manager"), + "Importing StateInterface {" << key << "} for (hardware " << hw_name << "): "); interface_names.push_back(key); } - hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; + for (const auto & name : interface_names) + { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("resource_manager"), "StateInterfaces {" << name << "}"); + } + + hardware_info_map_[hw_name].state_interfaces = interface_names; + for (const auto & name : hardware_info_map_[hw_name].state_interfaces) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("resource_manager"), "value={" << name << "}"); + } available_state_interfaces_.reserve( available_state_interfaces_.capacity() + interface_names.size()); } @@ -473,7 +493,14 @@ class ResourceStorage void import_command_interfaces(HardwareT & hardware) { auto interfaces = hardware.export_command_interfaces(); - hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); + auto interfaces_names = add_command_interfaces(interfaces); + hardware_info_map_[hardware.get_name()].command_interfaces = interfaces_names; + // TODO(Manuel) we have to consider reference interfaces from controllers... + std::string hw_name = hardware.get_name(); + for (const auto & interface_name : interfaces_names) + { + command_interface_to_component_name_.emplace(std::make_pair(interface_name, hw_name)); + } } template @@ -512,6 +539,7 @@ class ResourceStorage return interface_names; } + // TODO(Manuel) we have to consider reference interfaces from controllers... std::vector add_command_interface_descriptions( std::vector & interface_descriptions) { @@ -630,7 +658,7 @@ class ResourceStorage load_hardware(hardware_info, system_loader_, container); if (initialize_hardware(hardware_info, container.at(loaded_hw))) { - import_system_state_interface_descriptions(container.at(loaded_hw)); + import_state_interface_descriptions(container.at(loaded_hw)); import_command_interface_descriptions(container.at(loaded_hw)); } else @@ -726,7 +754,7 @@ class ResourceStorage container.insert(std::make_pair(hw_name, std::move(sys))); if (initialize_hardware(hardware_info, container.at(hw_name))) { - import_system_state_interface_descriptions(container.at(hw_name)); + import_state_interface_descriptions(container.at(hw_name)); import_command_interface_descriptions(container.at(hw_name)); } else @@ -774,7 +802,10 @@ class ResourceStorage find_component_name_for_interface(state_interface_name, state_interface_to_component_name_); if (!found) { - // TODO(Manuel) we should probably throw + throw std::runtime_error( + std::string("While getting LoanedStateInterface from component: Could not find a " + "component name for StateInterface:{'") + + state_interface_name + "'}."); } auto [found_act, actutator] = find_component(component_name, actuators_); @@ -806,42 +837,56 @@ class ResourceStorage // Inverted so that compiler is not complaining that we don't return anything... if (!found_async_sys) { - // TODO(Manuel) we should probably throw here something + throw std::runtime_error(std::string( + "While getting LoanedStateInterface from component: Could not find the " + "component with name:{'" + + component_name + "'}.")); } return async_sys->second.create_loaned_state_interface(state_interface_name); } - LoanedCommandInterface get_loaned_command_interface(const std::string command_interface_name) + LoanedCommandInterface get_loaned_command_interface( + const std::string command_interface_name, std::function && release_callback) { auto [found, component_name] = find_component_name_for_interface( command_interface_name, command_interface_to_component_name_); if (!found) { - // TODO(Manuel) we should probably throw + throw std::runtime_error( + std::string("While getting LoanedCommandInterface from component: Could not find a " + "component name for CommandInterface:{'") + + command_interface_name + "'}."); } auto [found_act, actutator] = find_component(component_name, actuators_); if (found_act) { - return actutator->second.create_loaned_command_interface(command_interface_name); + return actutator->second.create_loaned_command_interface( + command_interface_name, std::move(release_callback)); } auto [found_sys, system] = find_component(component_name, systems_); if (found_sys) { - return system->second.create_loaned_command_interface(command_interface_name); + return system->second.create_loaned_command_interface( + command_interface_name, std::move(release_callback)); } auto [found_async_act, async_acturator] = find_component(component_name, async_actuators_); if (found_async_act) { - return async_acturator->second.create_loaned_command_interface(command_interface_name); + return async_acturator->second.create_loaned_command_interface( + command_interface_name, std::move(release_callback)); } auto [found_async_sys, async_sys] = find_component(component_name, async_systems_); // Inverted so that compiler is not complaining that we don't return anything... if (!found_async_sys) { - // TODO(Manuel) we should probably throw here something + throw std::runtime_error(std::string( + "While getting LoanedCommandInterface from component: Could not find the " + "component with name:{'" + + component_name + "'}.")); } - return async_sys->second.create_loaned_command_interface(command_interface_name); + return async_sys->second.create_loaned_command_interface( + command_interface_name, std::move(release_callback)); } // hardware plugins @@ -947,7 +992,7 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac // throw on missing state and command interfaces, not specified keys are being ignored if (validate_interfaces) { - validate_storage(hardware_info); + // validate_storage(hardware_info); } std::lock_guard guard(resources_lock_); @@ -958,7 +1003,7 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } -bool ResourceManager::component_creates_loaned_state(const std::string & key) +bool ResourceManager::component_creates_loaned_state_interface(const std::string & key) { std::lock_guard guard(resource_interfaces_lock_); return resource_storage_->state_interface_descriptions_.find(key) != @@ -976,7 +1021,7 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string & std::lock_guard guard(resource_interfaces_lock_); // TODO(Manuel) just a hack for testing have to be removed later on when all the components create // the loans. - if (component_creates_loaned_state(key)) + if (component_creates_loaned_state_interface(key)) { return resource_storage_->get_loand_state_interface(key); } @@ -1133,6 +1178,13 @@ bool ResourceManager::command_interface_is_claimed(const std::string & key) cons return resource_storage_->claimed_command_interface_map_.at(key); } +bool ResourceManager::component_creates_loaned_command_interface(const std::string & key) +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->command_interface_descriptions_.find(key) != + resource_storage_->command_interface_descriptions_.end(); +} + // CM API: Called in "update"-thread LoanedCommandInterface ResourceManager::claim_command_interface(const std::string & key) { @@ -1148,6 +1200,12 @@ LoanedCommandInterface ResourceManager::claim_command_interface(const std::strin std::string("Command interface with '") + key + "' is already claimed"); } + if (component_creates_loaned_command_interface(key)) + { + return resource_storage_->get_loaned_command_interface( + key, std::bind(&ResourceManager::release_command_interface, this, key)); + } + resource_storage_->claimed_command_interface_map_[key] = true; std::lock_guard guard(resource_interfaces_lock_); return LoanedCommandInterface( @@ -1324,6 +1382,9 @@ return_type ResourceManager::set_component_state( using std::placeholders::_1; using std::placeholders::_2; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("resource_manager"), "Setting component {" << component_name << "}"); + auto found_it = resource_storage_->hardware_info_map_.find(component_name); if (found_it == resource_storage_->hardware_info_map_.end()) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 72ee37d5f1..3f999d84cf 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -189,6 +189,11 @@ std::vector Sensor::export_state_interfaces() return impl_->export_state_interfaces(); } +std::vector Sensor::export_state_interface_descriptions() +{ + return impl_->export_state_interface_descriptions(); +} + LoanedStateInterface Sensor::create_loaned_state_interface(const std::string & interface_name) { return impl_->create_loaned_state_interface(interface_name); diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index f9a27c5bd4..604d79c77b 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -204,9 +204,10 @@ std::vector System::export_command_interfaces() return impl_->export_command_interfaces(); } -LoanedCommandInterface System::create_loaned_command_interface(const std::string & interface_name) +LoanedCommandInterface System::create_loaned_command_interface( + const std::string & interface_name, std::function && release_callback) { - return impl_->create_loaned_command_interface(interface_name); + return impl_->create_loaned_command_interface(interface_name, std::move(release_callback)); } LoanedStateInterface System::create_loaned_state_interface(const std::string & interface_name) From e5ad09f49f7790c457c4035299462688d1612045 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 19 Dec 2023 11:42:35 +0100 Subject: [PATCH 5/5] fix type in CommandInterface --- hardware_interface/include/hardware_interface/handle.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 9831c732fc..eb6ced629a 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -148,7 +148,7 @@ class CommandInterface : public Handle */ CommandInterface(const CommandInterface & other) = delete; - CommandInterface & operator=(const Handle & other) = delete; + CommandInterface & operator=(const CommandInterface & other) = delete; CommandInterface(CommandInterface && other) = default;