From 3526deb579759a566505082d13b1e38f22719e2c Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 9 Dec 2022 14:09:39 +0100 Subject: [PATCH] first poc for export of inferface description --- .../include/hardware_interface/handle.hpp | 141 ++++++++------ .../hardware_interface/hardware_info.hpp | 115 +++++++++++ .../hardware_interface/hardware_interface.hpp | 76 ++++++++ .../loaned_command_interface.hpp | 9 +- .../loaned_hw_command_interface.hpp | 82 ++++++++ .../loaned_hw_state_interface.hpp | 81 ++++++++ .../loaned_state_interface.hpp | 5 + .../include/hardware_interface/system.hpp | 11 ++ .../hardware_interface/system_interface.hpp | 107 ++++++++++ .../mock_components/generic_system.hpp | 2 + .../src/mock_components/generic_system.cpp | 182 +++++++++--------- hardware_interface/src/resource_manager.cpp | 45 +++++ hardware_interface/src/system.cpp | 34 +++- transmission_interface/COLCON_IGNORE | 0 14 files changed, 742 insertions(+), 148 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/hardware_interface.hpp create mode 100644 hardware_interface/include/hardware_interface/loaned_hw_command_interface.hpp create mode 100644 hardware_interface/include/hardware_interface/loaned_hw_state_interface.hpp create mode 100644 transmission_interface/COLCON_IGNORE diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1aea017754..913c29e517 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,47 +15,55 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include #include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" namespace hardware_interface { /// A handle used to get and set a value on a given interface. -class ReadOnlyHandle +class Handle { public: - ReadOnlyHandle( - 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) + Handle(const std::string & prefix_name, const std::string & interface_name) + : prefix_name_(prefix_name), + interface_name_(interface_name), + has_new_value_(false), + is_valid_(false), + value_(std::numeric_limits::quiet_NaN()) { } - explicit ReadOnlyHandle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + explicit Handle(const std::string & interface_name) + : interface_name_(interface_name), + has_new_value_(false), + is_valid_(false), + value_(std::numeric_limits::quiet_NaN()) { } - explicit ReadOnlyHandle(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + explicit Handle(const char * interface_name) + : interface_name_(interface_name), + has_new_value_(false), + is_valid_(false), + value_(std::numeric_limits::quiet_NaN()) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; + // Handle should be unique + Handle(const Handle & other) = delete; - 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; - - /// Returns true if handle references a value. - inline operator bool() const { return value_ptr_ != nullptr; } + virtual ~Handle() = default; const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } @@ -70,62 +78,87 @@ class ReadOnlyHandle const std::string & get_prefix_name() const { return prefix_name_; } - double get_value() const + /** + * @brief Set the new value of the handle and mark the Handle as "has_new_value_ = true". + * This indicates that new data has been set since last read access. + * + * @param value current stored value in the handle. + */ + virtual void set_value(const double & value) { - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; + value_ = value; + has_new_value_ = true; } -protected: - std::string prefix_name_; - std::string interface_name_; - 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) + /** + * @brief Get the value of the handle an mark the handle as "has_new_value_ = false" + * since the value has been read and not be changed since last read access. + * + * @return HandleValue is the current stored value of the handle. + */ + virtual double get_value() { + has_new_value_ = false; + return value_; } - 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; + /** + * @brief Indicates if new value has been stored in the handle since the last + * read access. + * + * @return true => new value has been stored since last read access to the handle. + * @return false => no new value has been stored since last read access to the handle. + */ + virtual bool has_new_value() const { return has_new_value_; } - void set_value(double value) + /** + * @brief Indicates if the value stored inside the handle is valid + * + * @return true => stored value is valid and can be used. + * @return false => false stored value is not valid and should not be used. + */ + virtual bool value_is_valid() const { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; + if (value_ == std::numeric_limits::quiet_NaN()) + { + return false; + } + return true; } + +protected: + std::string prefix_name_; + std::string interface_name_; + // marks if value is new or has already been read + bool has_new_value_; + // stores if the stored value is valid + bool is_valid_; + // the current stored value of the handle + double value_; }; -class StateInterface : public ReadOnlyHandle +class StateInterface : public Handle { public: - StateInterface(const StateInterface & other) = default; + explicit StateInterface(const InterfaceDescription & interface_description) + : Handle(interface_description.prefix_name, interface_description.interface_info.name) + { + } + + StateInterface(const StateInterface & other) = delete; 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) + : Handle(interface_description.prefix_name, interface_description.interface_info.name) + { + } /// CommandInterface copy constructor is actively deleted. /** * Command interfaces are having a unique ownership and thus @@ -136,7 +169,7 @@ class CommandInterface : public ReadWriteHandle 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 eb6b63cfc3..05743e5c7b 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -15,12 +15,38 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ +#include #include #include #include namespace hardware_interface { + +template +std::ostream & operator<<(std::ostream & os, const std::vector & input) +{ + os << "["; + for (auto const & i : input) + { + os << i << ", "; + } + os << "]"; + return os; +} + +template +std::ostream & operator<<(std::ostream & os, const std::unordered_map & input) +{ + os << "["; + for (auto const & [key, value] : input) + { + os << "{" << key << ", " << value << "}, "; + } + os << "]"; + return os; +} + /** * This structure stores information about components defined for a specific hardware * in robot's URDF. @@ -42,6 +68,42 @@ struct InterfaceInfo std::string data_type; /// (Optional) If the handle is an array, the size of the array. Used by GPIOs. int size; + + friend std::ostream & operator<<(std::ostream & os, const InterfaceInfo & interface_info) + { + os << ""; + return os; + } +}; + +/** + * This structure stores information about an interface for a specific hardware which should be + * instantiated internally. + */ +struct InterfaceDescription +{ + InterfaceDescription(const std::string & prefix_name, const InterfaceInfo & interface_info) + : prefix_name(prefix_name), interface_info(interface_info) + { + } + + /** + * Name of the interface defined by the user. + */ + std::string prefix_name; + + InterfaceInfo interface_info; + + friend std::ostream & operator<<(std::ostream & os, const InterfaceDescription & interface_descr) + { + os << ""; + return os; + } + + // InterfaceInfo interface_info; }; /** @@ -67,6 +129,15 @@ struct ComponentInfo std::vector state_interfaces; /// (Optional) Key-value pairs of component parameters, e.g. min/max values or serial number. std::unordered_map parameters; + + friend std::ostream & operator<<(std::ostream & os, const ComponentInfo & component_info) + { + os << ""; + return os; + } }; /// Contains semantic info about a given joint loaded from URDF for a transmission @@ -78,6 +149,17 @@ struct JointInfo std::string role; double mechanical_reduction = 1.0; double offset = 0.0; + + friend std::ostream & operator<<(std::ostream & os, const JointInfo & joint_info) + { + os << ""; + return os; + } }; /// Contains semantic info about a given actuator loaded from URDF for a transmission @@ -89,6 +171,17 @@ struct ActuatorInfo std::string role; double mechanical_reduction = 1.0; double offset = 0.0; + + friend std::ostream & operator<<(std::ostream & os, const ActuatorInfo & actuator_info) + { + os << ""; + return os; + } }; /// Contains semantic info about a given transmission loaded from URDF @@ -100,6 +193,15 @@ struct TransmissionInfo std::vector actuators; /// (Optional) Key-value pairs of custom parameters std::unordered_map parameters; + + friend std::ostream & operator<<(std::ostream & os, const TransmissionInfo & transmission_info) + { + os << ""; + return os; + } }; /// This structure stores information about hardware defined in a robot's URDF. @@ -139,6 +241,19 @@ struct HardwareInfo * The XML contents prior to parsing */ std::string original_xml; + + friend std::ostream & operator<<(std::ostream & os, const HardwareInfo & hardware_info) + { + os << " + +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/loaned_hw_command_interface.hpp" +#include "hardware_interface/loaned_hw_state_interface.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class HardwareInterface +{ +public: + virtual ~HardwareInterface() {} + // Could be possible to provide default implementation and parse the hardware_info here. Only if + // user wants to export subset or special cases he needs to override. + + /** + * @brief Only export information describing the interfaces. Handle construction + * and management internally. No need for the user to initialize and manage shared memory. + * + * @return std::vector A vector containing all the information + * needed to create the interfaces exported by the hardware. + */ + HARDWARE_INTERFACE_PUBLIC + virtual std::vector export_state_interfaces_descriptions() = 0; + + /** + * @brief Only export information describing the interfaces. Handle construction + * and management internally. No need for the user to initialize and manage shared memory. + * + * @return std::vector A vector containing all the information + * needed to create the interfaces exported by the hardware. + */ + HARDWARE_INTERFACE_PUBLIC + virtual std::vector export_command_interfaces_descriptions() = 0; + + // Could be possible to provide default implementation and store the loans in the interface + // itself. User could then set/get states via function calls. Ordering could be made explicit. + + /** + * @brief Import the LoanedHwStateInterface to the before exported StateInterface InterfaceDescription. + * + */ + HARDWARE_INTERFACE_PUBLIC + virtual void import_loaned_hw_state_interfaces( + std::vector loaned_hw_state_interfaces) = 0; + + /** + * @brief Import the LoanedHwCommandInterface to the before exported CommandInterface InterfaceDescription. + * + */ + HARDWARE_INTERFACE_PUBLIC + virtual void import_loaned_hw_command_interfaces( + std::vector loaned_hw_command_interfaces) = 0; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__HARDWARE_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index bb5807c398..0e5b7c7851 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -20,6 +20,7 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" namespace hardware_interface { @@ -63,10 +64,14 @@ class LoanedCommandInterface const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } - void set_value(double val) { command_interface_.set_value(val); } - double get_value() const { return command_interface_.get_value(); } + bool has_new_value() const { return command_interface_.has_new_value(); } + + void set_value(const double & value) { command_interface_.set_value(value); } + + bool value_is_valid() const { return command_interface_.value_is_valid(); } + protected: CommandInterface & command_interface_; Deleter deleter_; diff --git a/hardware_interface/include/hardware_interface/loaned_hw_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_hw_command_interface.hpp new file mode 100644 index 0000000000..d524940e7a --- /dev/null +++ b/hardware_interface/include/hardware_interface/loaned_hw_command_interface.hpp @@ -0,0 +1,82 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// 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__LOANED_HW_COMMAND_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__LOANED_HW_COMMAND_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace hardware_interface +{ +class LoanedHwCommandInterface +{ +public: + using Deleter = std::function; + + explicit LoanedHwCommandInterface(CommandInterface & command_interface) + : LoanedHwCommandInterface(command_interface, nullptr) + { + } + + LoanedHwCommandInterface(CommandInterface & command_interface, Deleter && deleter) + : command_interface_(command_interface), deleter_(std::forward(deleter)) + { + } + + LoanedHwCommandInterface(const LoanedHwCommandInterface & other) = delete; + + LoanedHwCommandInterface(LoanedHwCommandInterface && other) = default; + + virtual ~LoanedHwCommandInterface() + { + if (deleter_) + { + deleter_(); + } + } + + const std::string get_name() const { return command_interface_.get_name(); } + + const std::string & get_interface_name() const { return command_interface_.get_interface_name(); } + + [[deprecated( + "Replaced by get_name method, which is semantically more correct")]] const std::string + get_full_name() const + { + return command_interface_.get_name(); + } + + const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } + + double get_value() const { return command_interface_.get_value(); } + + bool has_new_value() const { return command_interface_.has_new_value(); } + + void reset_command() { command_interface_.set_value(std::numeric_limits::quiet_NaN()); } + + bool value_is_valid() const { return command_interface_.value_is_valid(); } + +protected: + CommandInterface & command_interface_; + Deleter deleter_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__LOANED_HW_COMMAND_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_hw_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_hw_state_interface.hpp new file mode 100644 index 0000000000..42dab01155 --- /dev/null +++ b/hardware_interface/include/hardware_interface/loaned_hw_state_interface.hpp @@ -0,0 +1,81 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// 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__LOANED_HW_STATE_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__LOANED_HW_STATE_INTERFACE_HPP_ + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace hardware_interface +{ +class LoanedHwStateInterface +{ +public: + using Deleter = std::function; + + explicit LoanedHwStateInterface(StateInterface & state_interface) + : LoanedHwStateInterface(state_interface, nullptr) + { + } + + LoanedHwStateInterface(StateInterface & state_interface, Deleter && deleter) + : state_interface_(state_interface), deleter_(std::forward(deleter)) + { + } + + LoanedHwStateInterface(const LoanedHwStateInterface & other) = delete; + + LoanedHwStateInterface(LoanedHwStateInterface && other) = default; + + virtual ~LoanedHwStateInterface() + { + if (deleter_) + { + deleter_(); + } + } + + const std::string get_name() const { return state_interface_.get_name(); } + + const std::string & get_interface_name() const { return state_interface_.get_interface_name(); } + + [[deprecated( + "Replaced by get_name method, which is semantically more correct")]] const std::string + get_full_name() const + { + return state_interface_.get_name(); + } + + const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } + + double get_value() const { return state_interface_.get_value(); } + + bool has_new_value() const { return state_interface_.has_new_value(); } + + void set_value(const double & value) const { return state_interface_.set_value(value); } + + bool value_is_valid() const { return state_interface_.value_is_valid(); } + +protected: + StateInterface & state_interface_; + Deleter deleter_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__LOANED_HW_STATE_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 4fe67d1290..2d9702ed1a 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -20,6 +20,7 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" namespace hardware_interface { @@ -65,6 +66,10 @@ class LoanedStateInterface double get_value() const { return state_interface_.get_value(); } + bool has_new_value() const { return state_interface_.has_new_value(); } + + bool value_is_valid() const { return state_interface_.value_is_valid(); } + protected: StateInterface & state_interface_; Deleter deleter_; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index ece14f814d..0c4ca6285d 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_HPP_ #define HARDWARE_INTERFACE__SYSTEM_HPP_ +#include #include #include #include @@ -22,6 +23,8 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/loaned_hw_command_interface.hpp" +#include "hardware_interface/loaned_hw_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" #include "rclcpp/duration.hpp" @@ -68,9 +71,17 @@ class System final HARDWARE_INTERFACE_PUBLIC std::vector export_state_interfaces(); + HARDWARE_INTERFACE_PUBLIC + void assign_state_interface_loans_to_hw( + std::map && loaned_hw_state_interfaces); + HARDWARE_INTERFACE_PUBLIC std::vector export_command_interfaces(); + HARDWARE_INTERFACE_PUBLIC + void assign_command_interface_loans_to_hw( + std::map && loaned_hw_command_interfaces); + 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 e5c6f2f542..cf76334e76 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,16 +15,24 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ +#include #include #include +#include #include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/hardware_interface.hpp" +#include "hardware_interface/loaned_hw_command_interface.hpp" +#include "hardware_interface/loaned_hw_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" + #include "rclcpp/duration.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -123,6 +131,80 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual std::vector export_command_interfaces() = 0; + /** + * @brief Only export information describing the interfaces. Handle construction + * and management internally. No need for the user to initialize and manage shared memory. + * + * @return std::vector A vector containing all the information + * needed to create the interfaces exported by the hardware. + */ + virtual std::vector export_state_interfaces_descriptions() + { + RCLCPP_INFO_STREAM( + rclcpp::get_logger(info_.name), "Exporting following StateInterface description:"); + std::vector state_interface_descriptions; + for (const auto & joint : info_.joints) + { + for (const auto & state_interface : joint.state_interfaces) + { + InterfaceDescription description(joint.name, state_interface); + RCLCPP_INFO_STREAM(rclcpp::get_logger(info_.name), description); + state_interface_descriptions.push_back(description); + } + } + + return state_interface_descriptions; + } + + /** + * @brief Import the loans for the internally stored and managed StateInterfaces. + * + * @param loaned_hw_state_interfaces The StateInterfaces are stored in a map where the + * key is the loaned interfaces's name and the value is the loaned interface. + */ + virtual void import_loaned_hw_state_interfaces( + std::map && loaned_hw_state_interfaces) + { + hw_states_ = std::move(loaned_hw_state_interfaces); + } + + /** + * @brief Only export information describing the interfaces. Handle construction + * and management internally. No need for the user to initialize and manage shared memory. + * + * @return std::vector A vector containing all the information + * needed to create the interfaces exported by the hardware. + */ + virtual std::vector export_command_interfaces_descriptions() + { + RCLCPP_INFO_STREAM( + rclcpp::get_logger(info_.name), "Exporting following CommandInterface description:"); + std::vector command_interface_descriptions; + for (const auto & joint : info_.joints) + { + for (const auto & command_interface : joint.command_interfaces) + { + InterfaceDescription description(joint.name, command_interface); + RCLCPP_INFO_STREAM(rclcpp::get_logger(info_.name), description); + command_interface_descriptions.push_back(description); + } + } + + return command_interface_descriptions; + } + + /** + * @brief Import the loans for the internally stored and managed CommandInterfaces. + * + * @param loaned_hw_command_interfaces The CommandInterfaces are stored in a map where the + * key is the loaned interfaces's name and the value is the loaned interface. + */ + virtual void import_loaned_hw_command_interfaces( + std::map && loaned_hw_command_interfaces) + { + hw_commands_ = std::move(loaned_hw_command_interfaces); + } + /// Prepare for a new command interface switch. /** * Prepare for any mode-switching required by the new command interface combination. @@ -204,8 +286,33 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } protected: + virtual double state_interface_get_value(const std::string & state_interface_name) + { + return hw_states_.at(state_interface_name).get_value(); + } + + virtual void state_interface_set_value( + const std::string & state_interface_name, const double & value) + { + hw_states_.at(state_interface_name).set_value(value); + } + + virtual double command_interface_get_command(const std::string & command_interface_name) + { + return hw_commands_.at(command_interface_name).get_value(); + } + + virtual void command_interface_reset_command(const std::string & command_interface_name) + { + hw_commands_.at(command_interface_name).reset_command(); + } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; + +private: + std::map hw_states_; + std::map hw_commands_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index e9b38de65d..170ffc6dec 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -22,6 +22,8 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/loaned_hw_command_interface.hpp" +#include "hardware_interface/loaned_hw_state_interface.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 7b62af105b..6f987225ad 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -254,40 +254,42 @@ std::vector GenericSystem::export_state_inte { std::vector state_interfaces; - // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) - { - const auto & joint = info_.joints[i]; - for (const auto & interface : joint.state_interfaces) - { - // Add interface: if not in the standard list then use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_states_, state_interfaces)) - { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_states_, state_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } - } - - // Sensor state interfaces - if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. This should never happen!"); - }; - - // GPIO state interfaces - if (!populate_interfaces(info_.gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) - { - throw std::runtime_error("Interface is not found in the gpio list. This should never happen!"); - } + // // Joints' state interfaces + // for (auto i = 0u; i < info_.joints.size(); i++) + // { + // const auto & joint = info_.joints[i]; + // for (const auto & interface : joint.state_interfaces) + // { + // // Add interface: if not in the standard list then use "other" interface list + // if (!get_interface( + // joint.name, standard_interfaces_, interface.name, i, joint_states_, + // state_interfaces)) + // { + // if (!get_interface( + // joint.name, other_interfaces_, interface.name, i, other_states_, state_interfaces)) + // { + // throw std::runtime_error( + // "Interface is not found in the standard nor other list. " + // "This should never happen!"); + // } + // } + // } + // } + + // // Sensor state interfaces + // if (!populate_interfaces( + // info_.sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) + // { + // throw std::runtime_error( + // "Interface is not found in the standard nor other list. This should never happen!"); + // }; + + // // GPIO state interfaces + // if (!populate_interfaces(info_.gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) + // { + // throw std::runtime_error("Interface is not found in the gpio list. This should never + // happen!"); + // } return state_interfaces; } @@ -296,62 +298,62 @@ std::vector GenericSystem::export_command_ { std::vector command_interfaces; - // Joints' state interfaces - for (size_t i = 0; i < info_.joints.size(); ++i) - { - const auto & joint = info_.joints[i]; - for (const auto & interface : joint.command_interfaces) - { - // Add interface: if not in the standard list than use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_commands_, - command_interfaces)) - { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_commands_, - command_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } - } - // Set position control mode per default - joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); - - // Mock sensor command interfaces - if (use_mock_sensor_command_interfaces_) - { - if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_mock_commands_, command_interfaces, true)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. This should never happen!"); - } - } - - // Mock gpio command interfaces (consider all state interfaces for command interfaces) - if (use_mock_gpio_command_interfaces_) - { - if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, true)) - { - throw std::runtime_error( - "Interface is not found in the gpio list. This should never happen!"); - } - } - // GPIO command interfaces (real command interfaces) - else - { - if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) - { - throw std::runtime_error( - "Interface is not found in the gpio list. This should never happen!"); - } - } + // // Joints' state interfaces + // for (size_t i = 0; i < info_.joints.size(); ++i) + // { + // const auto & joint = info_.joints[i]; + // for (const auto & interface : joint.command_interfaces) + // { + // // Add interface: if not in the standard list than use "other" interface list + // if (!get_interface( + // joint.name, standard_interfaces_, interface.name, i, joint_commands_, + // command_interfaces)) + // { + // if (!get_interface( + // joint.name, other_interfaces_, interface.name, i, other_commands_, + // command_interfaces)) + // { + // throw std::runtime_error( + // "Interface is not found in the standard nor other list. " + // "This should never happen!"); + // } + // } + // } + // } + // // Set position control mode per default + // joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); + + // // Mock sensor command interfaces + // if (use_mock_sensor_command_interfaces_) + // { + // if (!populate_interfaces( + // info_.sensors, sensor_interfaces_, sensor_mock_commands_, command_interfaces, true)) + // { + // throw std::runtime_error( + // "Interface is not found in the standard nor other list. This should never happen!"); + // } + // } + + // // Mock gpio command interfaces (consider all state interfaces for command interfaces) + // if (use_mock_gpio_command_interfaces_) + // { + // if (!populate_interfaces( + // info_.gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, true)) + // { + // throw std::runtime_error( + // "Interface is not found in the gpio list. This should never happen!"); + // } + // } + // // GPIO command interfaces (real command interfaces) + // else + // { + // if (!populate_interfaces( + // info_.gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) + // { + // throw std::runtime_error( + // "Interface is not found in the gpio list. This should never happen!"); + // } + // } return command_interfaces; } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5c5d5af3e6..84ee93d8cb 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -29,11 +29,14 @@ #include "hardware_interface/async_components.hpp" #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_component_info.hpp" +#include "hardware_interface/loaned_hw_command_interface.hpp" +#include "hardware_interface/loaned_hw_state_interface.hpp" #include "hardware_interface/sensor.hpp" #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" + #include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" #include "rcutils/logging_macros.h" @@ -429,6 +432,12 @@ class ResourceStorage return result; } + template + std::vector get_state_interface_names(const HardwareT & hardware) + { + return hardware_info_map_[hardware.get_name()].state_interfaces; + } + template void import_state_interfaces(HardwareT & hardware) { @@ -446,6 +455,21 @@ class ResourceStorage available_state_interfaces_.capacity() + interface_names.size()); } + template + void assign_state_interface_loans_to_hw(HardwareT & hardware) + { + std::map hw_state_interface_loans; + for (const auto & state_interface_name : get_state_interface_names(hardware)) + { + // TODO(Manuel): should we mark as hw claimed??? + LoanedHwStateInterface loaned_hw_state_interface( + state_interface_map_.at(state_interface_name)); + hw_state_interface_loans.emplace( + loaned_hw_state_interface.get_name(), std::move(loaned_hw_state_interface)); + } + hardware.assign_state_interface_loans_to_hw(std::move(hw_state_interface_loans)); + } + template void import_command_interfaces(HardwareT & hardware) { @@ -481,6 +505,27 @@ class ResourceStorage return interface_names; } + template + std::vector get_command_interface_names(const HardwareT & hardware) + { + return hardware_info_map_[hardware.get_name()].command_interfaces; + } + + template + void assign_command_interface_loans_to_hw(HardwareT & hardware) + { + std::map hw_command_interface_loans; + for (const auto & command_interface_name : get_command_interface_names(hardware)) + { + // TODO(Manuel): should we mark as hw claimed??? + LoanedHwCommandInterface loaned_hw_command_interface( + command_interface_map_.at(command_interface_name)); + hw_command_interface_loans.emplace( + loaned_hw_command_interface.get_name(), std::move(loaned_hw_command_interface)); + } + hardware.assign_command_interface_loans_to_hw(std::move(hw_command_interface_loans)); + } + /// Removes command interfaces from internal storage. /** * Command interface are removed from the maps with theirs storage and their claimed status. diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index f8703a47bc..f0564e2574 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -14,6 +14,7 @@ #include "hardware_interface/system.hpp" +#include #include #include #include @@ -24,7 +25,10 @@ #include "hardware_interface/system_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" + +#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -186,12 +190,38 @@ const rclcpp_lifecycle::State & System::error() std::vector System::export_state_interfaces() { - return impl_->export_state_interfaces(); + std::vector state_interfaces; + + for (const auto & state_interface_descr : impl_->export_state_interfaces_descriptions()) + { + state_interfaces.emplace_back(StateInterface(state_interface_descr)); + } + + return state_interfaces; +} + +void System::assign_state_interface_loans_to_hw( + std::map && state_intefaces_for_hw) +{ + impl_->import_loaned_hw_state_interfaces(std::move(state_intefaces_for_hw)); } std::vector System::export_command_interfaces() { - return impl_->export_command_interfaces(); + std::vector command_interfaces; + + for (const auto & command_interface_descr : impl_->export_command_interfaces_descriptions()) + { + command_interfaces.emplace_back(CommandInterface(command_interface_descr)); + } + + return command_interfaces; +} + +void System::assign_command_interface_loans_to_hw( + std::map && command_intefaces_for_hw) +{ + impl_->import_loaned_hw_command_interfaces(std::move(command_intefaces_for_hw)); } return_type System::prepare_command_mode_switch( diff --git a/transmission_interface/COLCON_IGNORE b/transmission_interface/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2