From f2be0f941719190f06a1b2645cf6dc28d9c14d91 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 2 Jan 2025 16:12:17 +0100 Subject: [PATCH 1/4] Add new get_value API for handles and interfaces --- .../include/hardware_interface/handle.hpp | 29 ++++++++++- .../loaned_command_interface.hpp | 48 ++++++++++++++++++- .../loaned_state_interface.hpp | 48 ++++++++++++++++++- hardware_interface/test/test_handle.cpp | 31 ++++++++---- 4 files changed, 145 insertions(+), 11 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1dfd499c2c..764ddb1d2c 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -142,7 +143,9 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } - [[deprecated("Use bool get_value(double & value) instead to retrieve the value.")]] + [[deprecated( + "Use std::optional get_value() or T get_value(bool &status) or bool get_value(double & " + "value) instead to retrieve the value.")]] double get_value() const { std::shared_lock lock(handle_mutex_, std::try_to_lock); @@ -157,6 +160,30 @@ class Handle // END } + template + [[nodiscard]] std::optional get_value() const + { + std::shared_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return std::nullopt; + } + return std::get(value_); + } + + template + [[nodiscard]] T get_value(bool & status) const + { + std::shared_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + status = false; + return T(); + } + status = true; + return std::get(value_); + } + [[nodiscard]] bool get_value(double & value) const { std::shared_lock lock(handle_mutex_, std::try_to_lock); diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 7bb4d3a0ef..9278953119 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -113,9 +113,12 @@ class LoanedCommandInterface return true; } + [[deprecated( + "Use std::optional get_value() or T get_value(bool &status) or bool get_value(double & " + "value) instead to retrieve the value.")]] double get_value() const { - double value; + double value = std::numeric_limits::quiet_NaN(); if (get_value(value)) { return value; @@ -126,6 +129,49 @@ class LoanedCommandInterface } } + template + [[nodiscard]] std::optional get_value(unsigned int max_tries = 10) const + { + unsigned int nr_tries = 0; + do + { + ++get_value_statistics_.total_counter; + const std::optional data = command_interface_.get_value(); + if (data.has_value()) + { + return data; + } + ++get_value_statistics_.failed_counter; + ++nr_tries; + std::this_thread::yield(); + } while (nr_tries < max_tries); + + ++get_value_statistics_.timeout_counter; + return std::nullopt; + } + + template + [[nodiscard]] T get_value(bool & status, unsigned int max_tries = 10) const + { + unsigned int nr_tries = 0; + do + { + status = false; + ++get_value_statistics_.total_counter; + const T data = command_interface_.get_value(status); + if (status) + { + return data; + } + ++get_value_statistics_.failed_counter; + ++nr_tries; + std::this_thread::yield(); + } while (nr_tries < max_tries); + + ++get_value_statistics_.timeout_counter; + return T(); + } + template [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const { diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 27b3da813e..e3cbad11e8 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -88,9 +88,12 @@ class LoanedStateInterface const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } + [[deprecated( + "Use std::optional get_value() or T get_value(bool &status) or bool get_value(double & " + "value) instead to retrieve the value.")]] double get_value() const { - double value; + double value = std::numeric_limits::quiet_NaN(); if (get_value(value)) { return value; @@ -101,6 +104,49 @@ class LoanedStateInterface } } + template + [[nodiscard]] std::optional get_value(unsigned int max_tries = 10) const + { + unsigned int nr_tries = 0; + do + { + ++get_value_statistics_.total_counter; + const std::optional data = state_interface_.get_value(); + if (data.has_value()) + { + return data; + } + ++get_value_statistics_.failed_counter; + ++nr_tries; + std::this_thread::yield(); + } while (nr_tries < max_tries); + + ++get_value_statistics_.timeout_counter; + return std::nullopt; + } + + template + [[nodiscard]] T get_value(bool & status, unsigned int max_tries = 10) const + { + unsigned int nr_tries = 0; + do + { + status = false; + ++get_value_statistics_.total_counter; + const T data = state_interface_.get_value(status); + if (status) + { + return data; + } + ++get_value_statistics_.failed_counter; + ++nr_tries; + std::this_thread::yield(); + } while (nr_tries < max_tries); + + ++get_value_statistics_.timeout_counter; + return T(); + } + template [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const { diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 7d79c032f0..dc41584ec0 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -32,15 +32,24 @@ TEST(TestHandle, command_interface) double value = 1.337; CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; EXPECT_DOUBLE_EQ(interface.get_value(), value); - EXPECT_NO_THROW(interface.set_value(0.0)); - EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); + EXPECT_DOUBLE_EQ(interface.get_value().value(), value); + EXPECT_NO_THROW(bool status = interface.set_value(0.0)); + bool status; + EXPECT_DOUBLE_EQ(interface.get_value(status), 0.0); + ASSERT_TRUE(status); + ASSERT_TRUE(interface.get_value().has_value()); + EXPECT_DOUBLE_EQ(interface.get_value().value(), 0.0); } TEST(TestHandle, state_interface) { double value = 1.337; StateInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(interface.get_value(), value); + bool status = false; + EXPECT_DOUBLE_EQ(interface.get_value(status), value); + ASSERT_TRUE(status); + ASSERT_TRUE(interface.get_value().has_value()); + EXPECT_DOUBLE_EQ(interface.get_value().value(), value); // interface.set_value(5); compiler error, no set_value function } @@ -55,17 +64,23 @@ TEST(TestHandle, name_getters_work) TEST(TestHandle, value_methods_throw_for_nullptr) { CommandInterface handle{JOINT_NAME, FOO_INTERFACE}; - EXPECT_ANY_THROW(handle.get_value()); - EXPECT_ANY_THROW(handle.set_value(0.0)); + EXPECT_ANY_THROW(handle.get_value()); + EXPECT_ANY_THROW(bool status = handle.set_value(0.0)); } TEST(TestHandle, value_methods_work_on_non_nullptr) { double value = 1.337; CommandInterface handle{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(handle.get_value(), value); - EXPECT_NO_THROW(handle.set_value(0.0)); - EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); + bool status; + EXPECT_DOUBLE_EQ(handle.get_value(status), value); + ASSERT_TRUE(handle.get_value().has_value()); + EXPECT_DOUBLE_EQ(handle.get_value().value(), value); + EXPECT_DOUBLE_EQ(handle.get_value(status), value); + ASSERT_TRUE(status); + EXPECT_NO_THROW(bool status_set = handle.set_value(0.0)); + ASSERT_TRUE(handle.get_value().has_value()); + EXPECT_DOUBLE_EQ(handle.get_value().value(), 0.0); } TEST(TestHandle, interface_description_state_interface_name_getters_work) From 169ba839f194e1d52b5d8e6b9ba04627ad92a86d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 2 Jan 2025 16:31:38 +0100 Subject: [PATCH 2/4] add minor fixes --- hardware_interface/include/hardware_interface/handle.hpp | 4 ++-- hardware_interface/test/test_handle.cpp | 5 ++++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 764ddb1d2c..57e27fb3e4 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -168,7 +168,7 @@ class Handle { return std::nullopt; } - return std::get(value_); + return value_ptr_ != nullptr ? *value_ptr_ : std::get(value_); } template @@ -181,7 +181,7 @@ class Handle return T(); } status = true; - return std::get(value_); + return value_ptr_ != nullptr ? *value_ptr_ : std::get(value_); } [[nodiscard]] bool get_value(double & value) const diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index dc41584ec0..f20a1124ae 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -32,7 +32,9 @@ TEST(TestHandle, command_interface) double value = 1.337; CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; EXPECT_DOUBLE_EQ(interface.get_value(), value); + ASSERT_TRUE(interface.get_value().has_value()); EXPECT_DOUBLE_EQ(interface.get_value().value(), value); + EXPECT_DOUBLE_EQ(interface.get_value(), value); EXPECT_NO_THROW(bool status = interface.set_value(0.0)); bool status; EXPECT_DOUBLE_EQ(interface.get_value(status), 0.0); @@ -64,7 +66,8 @@ TEST(TestHandle, name_getters_work) TEST(TestHandle, value_methods_throw_for_nullptr) { CommandInterface handle{JOINT_NAME, FOO_INTERFACE}; - EXPECT_ANY_THROW(handle.get_value()); + double value; + EXPECT_ANY_THROW(handle.get_value(value)); EXPECT_ANY_THROW(bool status = handle.set_value(0.0)); } From a01be647153c1328a7399d6242d42a797697e3f2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 2 Jan 2025 19:23:32 +0100 Subject: [PATCH 3/4] update the other get_value method --- hardware_interface/include/hardware_interface/handle.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 57e27fb3e4..45e6581935 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -184,7 +184,8 @@ class Handle return value_ptr_ != nullptr ? *value_ptr_ : std::get(value_); } - [[nodiscard]] bool get_value(double & value) const + template + [[nodiscard]] bool get_value(T & value) const { std::shared_lock lock(handle_mutex_, std::try_to_lock); if (!lock.owns_lock()) @@ -193,8 +194,7 @@ class Handle } // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) set value directly if old functionality is removed - THROW_ON_NULLPTR(value_ptr_); - value = *value_ptr_; + value = value_ptr_ != nullptr ? *value_ptr_ : std::get(value_); return true; // END } From 4403195570644c799638ee72416c03e706516133 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 2 Jan 2025 19:38:01 +0100 Subject: [PATCH 4/4] Fix tests --- hardware_interface/test/test_handle.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index f20a1124ae..8606a8336b 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -66,8 +66,7 @@ TEST(TestHandle, name_getters_work) TEST(TestHandle, value_methods_throw_for_nullptr) { CommandInterface handle{JOINT_NAME, FOO_INTERFACE}; - double value; - EXPECT_ANY_THROW(handle.get_value(value)); + EXPECT_ANY_THROW(handle.get_value()); EXPECT_ANY_THROW(bool status = handle.set_value(0.0)); }