diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 30908cb40b..cc3fbb00f9 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -16,19 +16,126 @@ #define HARDWARE_INTERFACE__HANDLE_HPP_ #include +#include #include #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" +#include + namespace hardware_interface { +struct InterfaceConfiguration +{ + std::string joint_name; + InterfaceInfo interface_info; +}; + +/// A variant class used to store value on a given interface +class Variant +{ +enum value_type +{ + boolean = 0, + integer, + floating, + vector_boolean, + vector_integer, + vector_floating, +}; + +public: + Variant(bool value) + { + value_type_ = value_type::boolean; + bool_value_ = value; + } + + Variant(int value) + { + value_type_ = value_type::integer; + int_value_ = value; + } + + Variant(double value) + { + value_type_ = value_type::floating; + double_value_ = value; + } + + Variant(const std::vector & value) + { + value_type_ = value_type::vector_boolean; + vector_bool_value_ = value; + } + + Variant(const std::vector & value) + { + value_type_ = value_type::vector_integer; + vector_int_value_ = value; + } + + Variant(const std::vector & value) + { + value_type_ = value_type::vector_floating; + vector_double_value_ = value; + } + + void set_value(bool value) + { + bool_value_ = value; + } + + void set_value(int value) + { + int_value_ = value; + } + + void set_value(double value) + { + double_value_ = value; + } + + template , bool> = true> + DataT get_value() + { + return bool_value_; + } + + template , int> = true> + DataT get_value() + { + return int_value_; + } + + template , double> = true> + DataT get_value() + { + return double_value_; + } + + bool is_valid() + { + // Check if nan or empty to return "false" + } + +private: + value_type value_type_; + bool bool_value_; + int int_value_; + double double_value_; + std::vector vector_bool_value_; + std::vector vector_int_value_; + std::vector vector_double_value_; +}; + /// A handle used to get and set a value on a given interface. class ReadOnlyHandle { public: ReadOnlyHandle( - const std::string & name, const std::string & interface_name, double * value_ptr = nullptr) + const std::string & name, const std::string & interface_name, Variant * value_ptr = nullptr) : name_(name), interface_name_(interface_name), value_ptr_(value_ptr) { } @@ -62,23 +169,24 @@ class ReadOnlyHandle const std::string get_full_name() const { return name_ + "/" + interface_name_; } - double get_value() const + template + DataT get_value() const { THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; + return value_ptr_->get_value(); } protected: std::string name_; std::string interface_name_; - double * value_ptr_; + Variant * value_ptr_; }; class ReadWriteHandle : public ReadOnlyHandle { public: ReadWriteHandle( - const std::string & name, const std::string & interface_name, double * value_ptr = nullptr) + const std::string & name, const std::string & interface_name, Variant * value_ptr = nullptr) : ReadOnlyHandle(name, interface_name, value_ptr) { } @@ -97,10 +205,11 @@ class ReadWriteHandle : public ReadOnlyHandle virtual ~ReadWriteHandle() = default; - void set_value(double value) + template + void set_value(DataT value) { THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; + this->value_ptr_->set_value(value); } }; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index b82113914b..a750c0f2fc 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -91,6 +91,9 @@ class System final private: std::unique_ptr impl_; + + std::vector commands_; + std::vector states_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index f43b60a52b..684dd13ca4 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -97,6 +97,10 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return CallbackReturn::SUCCESS; }; + virtual std::vector export_state_interfaces_info() = 0; + + virtual std::vector export_command_interfaces_info() = 0; + /// Exports all state interfaces for this hardware interface. /** * The state interfaces have to be created and transferred according @@ -166,7 +170,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return return_type::OK if the read was successful, return_type::ERROR otherwise. */ - virtual return_type read() = 0; + virtual return_type read(std::vector & states) = 0; /// Write the current command values to the actuator. /** @@ -175,7 +179,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return return_type::OK if the read was successful, return_type::ERROR otherwise. */ - virtual return_type write() = 0; + virtual return_type write(std::vector & commands) = 0; /// Get name of the actuator hardware. /** diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 1b30d7d304..903b6cc087 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -186,11 +186,74 @@ const rclcpp_lifecycle::State & System::error() std::vector System::export_state_interfaces() { - return impl_->export_state_interfaces(); + std::vector state_interfaces; + + auto state_interfaces_info = impl_->export_state_interfaces_info(); + states_.reserve(state_interfaces_info.size()); + state_interfaces.reserve(state_interfaces_info.size()); + + for (auto const & itf : state_interfaces_info) + { + if (itf.interface_info.data_type.empty()) // default is double (check in parser if not already set) + { + if (itf.interface_info.initial_value.empty()) + { + states_.emplace_back(hardware_interface::Variant(std::numeric_limits::quiet_NaN())); + } + else + { + states_.emplace_back(hardware_interface::Variant(std::stod(itf.interface_info.initial_value))); + } + } + else if (itf.interface_info.data_type == "bool") + { + if (itf.interface_info.initial_value == "true" || itf.interface_info.initial_value == "True") + { + states_.emplace_back(hardware_interface::Variant(true)); + } + else + { + states_.emplace_back(hardware_interface::Variant(false)); + } + } + else if (itf.interface_info.data_type == "int") + { + if (itf.interface_info.initial_value.empty()) + { + states_.emplace_back(hardware_interface::Variant(std::numeric_limits::quiet_NaN())); + } + else + { + states_.emplace_back(hardware_interface::Variant(std::stoi(itf.interface_info.initial_value))); + } + } + else if (itf.interface_info.data_type == "double") + { + if (itf.interface_info.initial_value.empty()) + { + states_.emplace_back(hardware_interface::Variant(std::numeric_limits::quiet_NaN())); + } + else + { + states_.emplace_back(hardware_interface::Variant(std::stod(itf.interface_info.initial_value))); + } + } + else if (itf.interface_info.data_type == "vec") // somewhere we should have "size" + { + states_.emplace_back(hardware_interface::Variant(std::vector().reserve(std::stoi(itf.interface_info.size)))); + } + + // .... and so on for other types + + state_interfaces.emplace_back(StateInterface( + itf.joint_name, itf.interface_info.name, &states_.back())); + } } std::vector System::export_command_interfaces() { + // The same change as for state interfaces + return impl_->export_command_interfaces(); } @@ -219,7 +282,7 @@ return_type System::read() impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(); + result = impl_->read(state_); if (result == return_type::ERROR) { error(); @@ -235,7 +298,7 @@ return_type System::write() impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->write(); + result = impl_->write(commands_); if (result == return_type::ERROR) { error(); diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index eee55c428a..37cfebdd5f 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -19,6 +19,7 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +using hardware_interface::InterfaceConfiguration; using hardware_interface::CommandInterface; using hardware_interface::return_type; using hardware_interface::StateInterface; @@ -26,6 +27,33 @@ using hardware_interface::SystemInterface; class TestSystem : public SystemInterface { + std::vector export_state_interfaces_info() override + { + std::vector state_interfaces; + + for (auto i = 0u; i < info_.joints.size(); ++i) + { + if (info_.joints[i].name != "configuration") + { + state_interfaces.emplace_back(hardware_interface::InterfaceConfiguration({ + info_.joints[i].name, info_.joints[i].state_interfaces[0]})); + state_interfaces.emplace_back(hardware_interface::InterfaceConfiguration({ + info_.joints[i].name, info_.joints[i].state_interfaces[1]})); + state_interfaces.emplace_back(hardware_interface::InterfaceConfiguration({ + info_.joints[i].name, info_.joints[i].state_interfaces[2]})); + } + } + + if (info_.joints.size() > 2) + { + // Add configuration/max_tcp_jerk interface + state_interfaces.emplace_back(hardware_interface::InterfaceConfiguration({ + info_.joints[2].name, info_.joints[2].state_interfaces[0]})); + } + + return state_interfaces; + } + std::vector export_state_interfaces() override { std::vector state_interfaces; @@ -52,6 +80,31 @@ class TestSystem : public SystemInterface return state_interfaces; } + std::vector export_command_interfaces_info() override + { + std::vector command_interfaces; + for (auto i = 0u; i < info_.joints.size(); ++i) + { + if (info_.joints[i].name != "configuration") + { + command_interfaces.emplace_back(hardware_interface::InterfaceConfiguration({ + info_.joints[i].name, info_.joints[i].command_interfaces[0]})); + } + } + // Add max_acceleration command interface + command_interfaces.emplace_back(hardware_interface::InterfaceConfiguration({ + info_.joints[0].name, info_.joints[0].command_interfaces[1]})); + + if (info_.joints.size() > 2) + { + // Add configuration/max_tcp_jerk interface + command_interfaces.emplace_back(hardware_interface::InterfaceConfiguration({ + info_.joints[2].name, info_.joints[2].command_interfaces[0]})); + } + + return command_interfaces; + } + std::vector export_command_interfaces() override { std::vector command_interfaces; @@ -78,9 +131,22 @@ class TestSystem : public SystemInterface return command_interfaces; } - return_type read() override { return return_type::OK; } + return_type read(std::vector & states) override + { + // get some states from robots + states[0].set_value(false); // there is still some implicit knowledge about order of interfaces (the same order as they info is exported) + states[1].set_value(1.2); + + return return_type::OK; + } + + return_type write(std::vector & commands) override + { + // write some state to robot + fprintf("%.5f", commands[0].get_value()); - return_type write() override { return return_type::OK; } + return return_type::OK; + } private: std::array velocity_command_ = {0.0, 0.0};