From cd371a11323564321044f563c5e588465eef4a1e Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 9 Aug 2021 17:37:27 -0700 Subject: [PATCH 1/2] expose complex data types through interfaces Signed-off-by: Karsten Knese --- .../include/hardware_interface/handle.hpp | 46 +++++++++++++------ .../loaned_command_interface.hpp | 10 ++-- .../loaned_state_interface.hpp | 5 +- .../test/test_component_interfaces.cpp | 19 +++++++- hardware_interface/test/test_handle.cpp | 21 +++++++++ .../include/transmission_interface/handle.hpp | 13 ++++-- 6 files changed, 87 insertions(+), 27 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 4789659212..f11360ca8e 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -30,20 +30,17 @@ class ReadOnlyHandle ReadOnlyHandle( const std::string & name, const std::string & interface_name, - double * value_ptr = nullptr) + void * value_ptr = nullptr) : name_(name), interface_name_(interface_name), value_ptr_(value_ptr) - { - } + {} explicit ReadOnlyHandle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) - { - } + {} explicit ReadOnlyHandle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) - { - } + {} ReadOnlyHandle(const ReadOnlyHandle & other) = default; @@ -73,16 +70,17 @@ class ReadOnlyHandle return name_ + "/" + interface_name_; } - double get_value() const + template + DataT get_value() const { THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; + return *static_cast(value_ptr_); } protected: std::string name_; std::string interface_name_; - double * value_ptr_; + void * value_ptr_; }; class ReadWriteHandle : public ReadOnlyHandle @@ -91,7 +89,7 @@ class ReadWriteHandle : public ReadOnlyHandle ReadWriteHandle( const std::string & name, const std::string & interface_name, - double * value_ptr = nullptr) + void * value_ptr = nullptr) : ReadOnlyHandle(name, interface_name, value_ptr) {} @@ -113,26 +111,35 @@ 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; + *static_cast(this->value_ptr_) = value; } }; class StateInterface : public ReadOnlyHandle { public: + using ReadOnlyHandle::ReadOnlyHandle; + StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; - using ReadOnlyHandle::ReadOnlyHandle; + template + DataT get_value() const + { + return ReadOnlyHandle::get_value(); + } }; class CommandInterface : public ReadWriteHandle { public: + using ReadWriteHandle::ReadWriteHandle; + /// CommandInterface copy constructor is actively deleted. /** * Command interfaces are having a unique ownership and thus @@ -143,7 +150,16 @@ class CommandInterface : public ReadWriteHandle CommandInterface(CommandInterface && other) = default; - using ReadWriteHandle::ReadWriteHandle; + template + DataT get_value() const + { + return ReadWriteHandle::get_value(); + } + + template + void set_value(DataT value) { + return ReadWriteHandle::set_value(value); + } }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 99047d0380..33cc79c5d4 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -66,14 +66,16 @@ class LoanedCommandInterface return command_interface_.get_full_name(); } - void set_value(double val) + template + void set_value(DataT val) { - command_interface_.set_value(val); + command_interface_.set_value(val); } - double get_value() const + template + DataT get_value() const { - return command_interface_.get_value(); + return command_interface_.get_value(); } protected: diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index e979f6c628..3b6f0e6a55 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -66,9 +66,10 @@ class LoanedStateInterface return state_interface_.get_full_name(); } - double get_value() const + template + DataT get_value() const { - return state_interface_.get_value(); + return state_interface_.get_value(); } protected: diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index af3f2f49aa..c87662c30a 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -36,6 +36,13 @@ using namespace ::testing; // NOLINT namespace test_components { +struct POD +{ + std::string str; + int i; + float f; +}; + class DummyActuator : public hardware_interface::ActuatorInterface { hardware_interface::return_type configure( @@ -125,6 +132,10 @@ class DummySensor : public hardware_interface::SensorInterface std::vector state_interfaces; state_interfaces.emplace_back( hardware_interface::StateInterface("joint1", "voltage", &voltage_level_)); + // Any non-double data type can be exposed + state_interfaces.emplace_back( + hardware_interface::StateInterface( + "joint1", "custom_complex_pod", &complex_data_)); return state_interfaces; } @@ -157,6 +168,7 @@ class DummySensor : public hardware_interface::SensorInterface private: double voltage_level_ = 0x666; + POD complex_data_ = {"abc", 123, 456.f}; }; class DummySystem : public hardware_interface::SystemInterface @@ -383,10 +395,15 @@ TEST(TestComponentInterfaces, dummy_sensor) EXPECT_EQ(hardware_interface::return_type::OK, sensor_hw.configure(mock_hw_info)); auto state_interfaces = sensor_hw.export_state_interfaces(); - ASSERT_EQ(1u, state_interfaces.size()); + ASSERT_EQ(2u, state_interfaces.size()); EXPECT_EQ("joint1", state_interfaces[0].get_name()); EXPECT_EQ("voltage", state_interfaces[0].get_interface_name()); EXPECT_EQ(0x666, state_interfaces[0].get_value()); + EXPECT_EQ("joint1", state_interfaces[1].get_name()); + EXPECT_EQ("custom_complex_pod", state_interfaces[1].get_interface_name()); + EXPECT_EQ("abc", state_interfaces[1].get_value().str); + EXPECT_EQ(123, state_interfaces[1].get_value().i); + EXPECT_FLOAT_EQ(456.f, state_interfaces[1].get_value().f); } TEST(TestComponentInterfaces, dummy_system) diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 647b247e28..64b6f2afaa 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -33,6 +33,27 @@ TEST(TestHandle, command_interface) EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); } +TEST(TestHandle, complex_command_interface) +{ + struct Complex + { + std::string str; + }; + + std::string value = "Hello Complex Interface"; + Complex c{value}; + + CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &c}; + EXPECT_EQ(interface.get_value(), value); + // TODO(karsten1987): Make get_value (implicit as double) type safe on caller side. + // interface.get_value() is now undefined behavior. + // EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); + + value = "Hello Modified Complex Interface"; + EXPECT_NO_THROW(interface.set_value(value)); + EXPECT_EQ(interface.get_value(), value); +} + TEST(TestHandle, state_interface) { double value = 1.337; diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index bc1c0a78d8..8fdd47246c 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -25,15 +25,18 @@ namespace transmission_interface class ActuatorHandle : public hardware_interface::ReadWriteHandle { public: + void set_value(double value) { + return hardware_interface::ReadWriteHandle::set_value(value); + } + double get_value() { + return hardware_interface::ReadWriteHandle::get_value(); + } + using hardware_interface::ReadWriteHandle::ReadWriteHandle; }; /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public hardware_interface::ReadWriteHandle -{ -public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; -}; +using JointHandle = ActuatorHandle; } // namespace transmission_interface From d33ece90137f60d27f1b4c8f90fcb78573322c83 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 9 Aug 2021 17:46:36 -0700 Subject: [PATCH 2/2] linters Signed-off-by: Karsten Knese --- hardware_interface/include/hardware_interface/handle.hpp | 3 ++- hardware_interface/test/test_handle.cpp | 2 ++ .../include/transmission_interface/handle.hpp | 6 ++++-- 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index f11360ca8e..3d9a71e2b6 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -157,7 +157,8 @@ class CommandInterface : public ReadWriteHandle } template - void set_value(DataT value) { + void set_value(DataT value) + { return ReadWriteHandle::set_value(value); } }; diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 64b6f2afaa..3e0f69a060 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include +#include + #include "hardware_interface/handle.hpp" using hardware_interface::CommandInterface; diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index 8fdd47246c..9298382281 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -25,10 +25,12 @@ namespace transmission_interface class ActuatorHandle : public hardware_interface::ReadWriteHandle { public: - void set_value(double value) { + void set_value(double value) + { return hardware_interface::ReadWriteHandle::set_value(value); } - double get_value() { + double get_value() + { return hardware_interface::ReadWriteHandle::get_value(); }