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/9] 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/9] 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/9] 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/9] 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)); } From e732160f709f98ef63aec7f68de40acaf6f821b3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 16:49:45 +0100 Subject: [PATCH 5/9] Remove the status arg implementation --- .../include/hardware_interface/handle.hpp | 13 ----------- .../loaned_command_interface.hpp | 22 ------------------- .../loaned_state_interface.hpp | 22 ------------------- hardware_interface/test/test_handle.cpp | 10 --------- 4 files changed, 67 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 45e6581935..9b0a04c753 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -171,19 +171,6 @@ class Handle return value_ptr_ != nullptr ? *value_ptr_ : 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 value_ptr_ != nullptr ? *value_ptr_ : std::get(value_); - } - template [[nodiscard]] bool get_value(T & value) const { diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 9278953119..18c6b06b32 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -150,28 +150,6 @@ class LoanedCommandInterface 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 e3cbad11e8..ce9307824b 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -125,28 +125,6 @@ class LoanedStateInterface 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 8606a8336b..c72d574fca 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -36,9 +36,6 @@ TEST(TestHandle, command_interface) 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); - ASSERT_TRUE(status); ASSERT_TRUE(interface.get_value().has_value()); EXPECT_DOUBLE_EQ(interface.get_value().value(), 0.0); } @@ -47,9 +44,6 @@ TEST(TestHandle, state_interface) { double value = 1.337; StateInterface interface{JOINT_NAME, FOO_INTERFACE, &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 @@ -74,12 +68,8 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) { double value = 1.337; CommandInterface handle{JOINT_NAME, FOO_INTERFACE, &value}; - 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); From 91936b02301291ece7fc27e51270e164a43a58c6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 17:42:43 +0100 Subject: [PATCH 6/9] use get_value() instead of get_value() --- .../force_torque_sensor.hpp | 4 +- .../semantic_components/imu_sensor.hpp | 8 +- .../semantic_components/pose_sensor.hpp | 4 +- .../semantic_components/range_sensor.hpp | 2 +- .../semantic_component_interface.hpp | 2 +- .../test_chainable_controller_interface.cpp | 11 +- .../test_chainable_controller.cpp | 10 +- .../test/test_controller/test_controller.cpp | 2 +- ...llers_chaining_with_controller_manager.cpp | 79 +- .../mock_components/test_generic_system.cpp | 904 +++++++++--------- .../test/test_component_interfaces.cpp | 248 +++-- ...est_component_interfaces_custom_export.cpp | 2 +- hardware_interface/test/test_handle.cpp | 6 +- .../test/test_resource_manager.cpp | 42 +- ...esource_manager_prepare_perform_switch.cpp | 240 ++--- 15 files changed, 832 insertions(+), 732 deletions(-) diff --git a/controller_interface/include/semantic_components/force_torque_sensor.hpp b/controller_interface/include/semantic_components/force_torque_sensor.hpp index 18c3948962..d15a8f85b5 100644 --- a/controller_interface/include/semantic_components/force_torque_sensor.hpp +++ b/controller_interface/include/semantic_components/force_torque_sensor.hpp @@ -96,7 +96,7 @@ class ForceTorqueSensor : public SemanticComponentInterface().value(); ++interface_counter; } } @@ -123,7 +123,7 @@ class ForceTorqueSensor : public SemanticComponentInterface().value(); ++torque_interface_counter; } } diff --git a/controller_interface/include/semantic_components/imu_sensor.hpp b/controller_interface/include/semantic_components/imu_sensor.hpp index 793e5c0fea..675e85c760 100644 --- a/controller_interface/include/semantic_components/imu_sensor.hpp +++ b/controller_interface/include/semantic_components/imu_sensor.hpp @@ -52,7 +52,7 @@ class IMUSensor : public SemanticComponentInterface std::array orientation; for (auto i = 0u; i < orientation.size(); ++i) { - orientation[i] = state_interfaces_[i].get().get_value(); + orientation[i] = state_interfaces_[i].get().get_value().value(); } return orientation; } @@ -69,7 +69,8 @@ class IMUSensor : public SemanticComponentInterface const std::size_t interface_offset{4}; for (auto i = 0u; i < angular_velocity.size(); ++i) { - angular_velocity[i] = state_interfaces_[interface_offset + i].get().get_value(); + angular_velocity[i] = + state_interfaces_[interface_offset + i].get().get_value().value(); } return angular_velocity; } @@ -86,7 +87,8 @@ class IMUSensor : public SemanticComponentInterface const std::size_t interface_offset{7}; for (auto i = 0u; i < linear_acceleration.size(); ++i) { - linear_acceleration[i] = state_interfaces_[interface_offset + i].get().get_value(); + linear_acceleration[i] = + state_interfaces_[interface_offset + i].get().get_value().value(); } return linear_acceleration; } diff --git a/controller_interface/include/semantic_components/pose_sensor.hpp b/controller_interface/include/semantic_components/pose_sensor.hpp index 098fb04278..0192010bbb 100644 --- a/controller_interface/include/semantic_components/pose_sensor.hpp +++ b/controller_interface/include/semantic_components/pose_sensor.hpp @@ -50,7 +50,7 @@ class PoseSensor : public SemanticComponentInterface std::array position; for (auto i = 0u; i < position.size(); ++i) { - position[i] = state_interfaces_[i].get().get_value(); + position[i] = state_interfaces_[i].get().get_value().value(); } return position; } @@ -67,7 +67,7 @@ class PoseSensor : public SemanticComponentInterface const std::size_t interface_offset{3}; for (auto i = 0u; i < orientation.size(); ++i) { - orientation[i] = state_interfaces_[interface_offset + i].get().get_value(); + orientation[i] = state_interfaces_[interface_offset + i].get().get_value().value(); } return orientation; } diff --git a/controller_interface/include/semantic_components/range_sensor.hpp b/controller_interface/include/semantic_components/range_sensor.hpp index df4b0f46c5..464cef58e9 100644 --- a/controller_interface/include/semantic_components/range_sensor.hpp +++ b/controller_interface/include/semantic_components/range_sensor.hpp @@ -35,7 +35,7 @@ class RangeSensor : public SemanticComponentInterface * * \return value of the range in meters */ - float get_range() const { return state_interfaces_[0].get().get_value(); } + float get_range() const { return state_interfaces_[0].get().get_value().value(); } /// Return Range message with range in meters /** diff --git a/controller_interface/include/semantic_components/semantic_component_interface.hpp b/controller_interface/include/semantic_components/semantic_component_interface.hpp index 1f49e8cbff..63ea13418c 100644 --- a/controller_interface/include/semantic_components/semantic_component_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_interface.hpp @@ -93,7 +93,7 @@ class SemanticComponentInterface // insert all the values for (auto i = 0u; i < state_interfaces_.size(); ++i) { - values.emplace_back(state_interfaces_[i].get().get_value()); + values.emplace_back(state_interfaces_[i].get().get_value().value()); } return true; } diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 12d5599d45..2c5b9090ff 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -52,7 +52,8 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) EXPECT_EQ(exported_state_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state"); - EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE); + EXPECT_EQ( + exported_state_interfaces[0]->get_value().value(), EXPORTED_STATE_INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -72,7 +73,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf"); - EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value().value(), INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) @@ -120,7 +121,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode - EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value().value(), INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -133,7 +134,9 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); - EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); + EXPECT_EQ( + exported_state_interfaces[0]->get_value().value(), + EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); controller.configure(); EXPECT_TRUE(controller.set_chained_mode(false)); diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index e68af6345f..e94163b503 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -101,13 +101,15 @@ controller_interface::return_type TestChainableController::update_and_write_comm for (size_t i = 0; i < command_interfaces_.size(); ++i) { - command_interfaces_[i].set_value(reference_interfaces_[i] - state_interfaces_[i].get_value()); + command_interfaces_[i].set_value( + reference_interfaces_[i] - state_interfaces_[i].get_value().value()); } // If there is a command interface then integrate and set it to the exported state interface data for (size_t i = 0; i < exported_state_interface_names_.size() && i < command_interfaces_.size(); ++i) { - state_interfaces_values_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; + state_interfaces_values_[i] = + command_interfaces_[i].get_value().value() * CONTROLLER_DT; } // If there is no command interface and if there is a state interface then just forward the same // value as in the state interface @@ -115,7 +117,7 @@ controller_interface::return_type TestChainableController::update_and_write_comm command_interfaces_.empty(); ++i) { - state_interfaces_values_[i] = state_interfaces_[i].get_value(); + state_interfaces_values_[i] = state_interfaces_[i].get_value().value(); } return controller_interface::return_type::OK; @@ -240,7 +242,7 @@ std::vector TestChainableController::get_state_interface_data() const std::vector state_intr_data; for (const auto & interface : state_interfaces_) { - state_intr_data.push_back(interface.get_value()); + state_intr_data.push_back(interface.get_value().value()); } return state_intr_data; } diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index ee8377f2a3..a6447c0255 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -178,7 +178,7 @@ std::vector TestController::get_state_interface_data() const std::vector state_intr_data; for (const auto & interface : state_interfaces_) { - state_intr_data.push_back(interface.get_value()); + state_intr_data.push_back(interface.get_value().value()); } return state_intr_data; } diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index c143ab4862..059d3eecdf 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -520,8 +520,12 @@ class TestControllerChainingWithControllerManager // Command of DiffDrive controller are references of PID controllers EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); - ASSERT_EQ(diff_drive_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_REF); - ASSERT_EQ(diff_drive_controller->command_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_REF); + ASSERT_EQ( + diff_drive_controller->command_interfaces_[0].get_value().value(), + EXP_LEFT_WHEEL_REF); + ASSERT_EQ( + diff_drive_controller->command_interfaces_[1].get_value().value(), + EXP_RIGHT_WHEEL_REF); ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); @@ -537,21 +541,32 @@ class TestControllerChainingWithControllerManager EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); - ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); - ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + ASSERT_EQ( + pid_left_wheel_controller->command_interfaces_[0].get_value().value(), + EXP_LEFT_WHEEL_CMD); + ASSERT_EQ( + pid_left_wheel_controller->state_interfaces_[0].get_value().value(), + EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state - ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + ASSERT_EQ( + diff_drive_controller->state_interfaces_[0].get_value().value(), + EXP_LEFT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); - ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); ASSERT_EQ( - pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + pid_right_wheel_controller->command_interfaces_[0].get_value().value(), + EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ( + pid_right_wheel_controller->state_interfaces_[0].get_value().value(), + EXP_RIGHT_WHEEL_HW_STATE); // DiffDrive uses the same state - ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ( + diff_drive_controller->state_interfaces_[1].get_value().value(), + EXP_RIGHT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); @@ -811,10 +826,16 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); // 32 / 2 EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); - ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); - ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + ASSERT_EQ( + pid_left_wheel_controller->command_interfaces_[0].get_value().value(), + EXP_LEFT_WHEEL_CMD); + ASSERT_EQ( + pid_left_wheel_controller->state_interfaces_[0].get_value().value(), + EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state - ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + ASSERT_EQ( + diff_drive_controller->state_interfaces_[0].get_value().value(), + EXP_LEFT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); @@ -823,13 +844,19 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); // 128 / 2 EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); - ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); - ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ( + pid_right_wheel_controller->command_interfaces_[0].get_value().value(), + EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ( + pid_right_wheel_controller->state_interfaces_[0].get_value().value(), + EXP_RIGHT_WHEEL_HW_STATE); ASSERT_EQ(odom_publisher_controller->internal_counter, 2u); ASSERT_EQ(sensor_fusion_controller->internal_counter, 6u); ASSERT_EQ(robot_localization_controller->internal_counter, 4u); // DiffDrive uses the same state - ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ( + diff_drive_controller->state_interfaces_[1].get_value().value(), + EXP_RIGHT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); @@ -1986,19 +2013,31 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); // 32 / 2 EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); - ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); - ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + ASSERT_EQ( + pid_left_wheel_controller->command_interfaces_[0].get_value().value(), + EXP_LEFT_WHEEL_CMD); + ASSERT_EQ( + pid_left_wheel_controller->state_interfaces_[0].get_value().value(), + EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state - ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + ASSERT_EQ( + diff_drive_controller->state_interfaces_[0].get_value().value(), + EXP_LEFT_WHEEL_HW_STATE); // 128 - 0 EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); // 128 / 2 EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); - ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); - ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ( + pid_right_wheel_controller->command_interfaces_[0].get_value().value(), + EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ( + pid_right_wheel_controller->state_interfaces_[0].get_value().value(), + EXP_RIGHT_WHEEL_HW_STATE); // DiffDrive uses the same state - ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ( + diff_drive_controller->state_interfaces_[1].get_value().value(), + EXP_RIGHT_WHEEL_HW_STATE); // update all controllers at once and see that all have expected values --> also checks the order // of controller execution diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 446634d12f..e52a2f11fc 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -778,10 +778,10 @@ TEST_F(TestGenericSystem, generic_system_2dof_symetric_interfaces) hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface("joint2/position"); - ASSERT_EQ(1.57, j1p_s.get_value()); - ASSERT_EQ(0.7854, j2p_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); + ASSERT_EQ(1.57, j1p_s.get_value().value()); + ASSERT_EQ(0.7854, j2p_s.get_value().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value().value())); } // Test inspired by hardware_interface/test_resource_manager.cpp @@ -827,10 +827,10 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) hardware_interface::LoanedCommandInterface j2a_c = rm.claim_command_interface("joint2/acceleration"); - ASSERT_EQ(1.57, j1v_s.get_value()); - ASSERT_EQ(0.7854, j2p_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j2a_c.get_value())); + ASSERT_EQ(1.57, j1v_s.get_value().value()); + ASSERT_EQ(0.7854, j2p_s.get_value().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2a_c.get_value().value())); } void generic_system_functional_test( @@ -867,14 +867,14 @@ void generic_system_functional_test( hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); // State interfaces without initial value are set to 0 - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(2.78, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); // set some new values in commands j1p_c.set_value(0.11); @@ -883,36 +883,36 @@ void generic_system_functional_test( j2v_c.set_value(0.44); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(2.78, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.22, j1v_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(0.44, j2v_c.get_value().value()); // write() does not change values ASSERT_TRUE(rm.write(TIME, PERIOD).ok); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(2.78, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.22, j1v_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(0.44, j2v_c.get_value().value()); // read() mirrors commands + offset to states ASSERT_TRUE(rm.read(TIME, PERIOD).ok); - ASSERT_EQ(0.11 + offset, j1p_s.get_value()); - ASSERT_EQ(0.22, j1v_s.get_value()); - ASSERT_EQ(0.33 + offset, j2p_s.get_value()); - ASSERT_EQ(0.44, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(0.11 + offset, j1p_s.get_value().value()); + ASSERT_EQ(0.22, j1v_s.get_value().value()); + ASSERT_EQ(0.33 + offset, j2p_s.get_value().value()); + ASSERT_EQ(0.44, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.22, j1v_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(0.44, j2v_c.get_value().value()); // set some new values in commands j1p_c.set_value(0.55); @@ -921,14 +921,14 @@ void generic_system_functional_test( j2v_c.set_value(0.88); // state values should not be changed - ASSERT_EQ(0.11 + offset, j1p_s.get_value()); - ASSERT_EQ(0.22, j1v_s.get_value()); - ASSERT_EQ(0.33 + offset, j2p_s.get_value()); - ASSERT_EQ(0.44, j2v_s.get_value()); - ASSERT_EQ(0.55, j1p_c.get_value()); - ASSERT_EQ(0.66, j1v_c.get_value()); - ASSERT_EQ(0.77, j2p_c.get_value()); - ASSERT_EQ(0.88, j2v_c.get_value()); + ASSERT_EQ(0.11 + offset, j1p_s.get_value().value()); + ASSERT_EQ(0.22, j1v_s.get_value().value()); + ASSERT_EQ(0.33 + offset, j2p_s.get_value().value()); + ASSERT_EQ(0.44, j2v_s.get_value().value()); + ASSERT_EQ(0.55, j1p_c.get_value().value()); + ASSERT_EQ(0.66, j1v_c.get_value().value()); + ASSERT_EQ(0.77, j2p_c.get_value().value()); + ASSERT_EQ(0.88, j2v_c.get_value().value()); deactivate_components(rm, {component_name}); status_map = rm.get_components_status(); @@ -973,14 +973,14 @@ void generic_system_error_group_test( hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); // State interfaces without initial value are set to 0 - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(2.78, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); // set some new values in commands j1p_c.set_value(0.11); @@ -989,36 +989,36 @@ void generic_system_error_group_test( j2v_c.set_value(0.44); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(2.78, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.22, j1v_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(0.44, j2v_c.get_value().value()); // write() does not change values ASSERT_TRUE(rm.write(TIME, PERIOD).ok); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(2.78, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.22, j1v_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(0.44, j2v_c.get_value().value()); // read() mirrors commands to states ASSERT_TRUE(rm.read(TIME, PERIOD).ok); - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.22, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - ASSERT_EQ(0.44, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value().value()); + ASSERT_EQ(0.22, j1v_s.get_value().value()); + ASSERT_EQ(0.33, j2p_s.get_value().value()); + ASSERT_EQ(0.44, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.22, j1v_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(0.44, j2v_c.get_value().value()); // set some new values in commands j1p_c.set_value(0.55); @@ -1027,14 +1027,14 @@ void generic_system_error_group_test( j2v_c.set_value(0.88); // state values should not be changed - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.22, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - ASSERT_EQ(0.44, j2v_s.get_value()); - ASSERT_EQ(0.55, j1p_c.get_value()); - ASSERT_EQ(0.66, j1v_c.get_value()); - ASSERT_EQ(0.77, j2p_c.get_value()); - ASSERT_EQ(0.88, j2v_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value().value()); + ASSERT_EQ(0.22, j1v_s.get_value().value()); + ASSERT_EQ(0.33, j2p_s.get_value().value()); + ASSERT_EQ(0.44, j2v_s.get_value().value()); + ASSERT_EQ(0.55, j1p_c.get_value().value()); + ASSERT_EQ(0.66, j1v_c.get_value().value()); + ASSERT_EQ(0.77, j2p_c.get_value().value()); + ASSERT_EQ(0.88, j2v_c.get_value().value()); // Error testing j1p_c.set_value(std::numeric_limits::infinity()); @@ -1154,14 +1154,14 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) hardware_interface::LoanedCommandInterface vo_c = rm.claim_command_interface("voltage_output/voltage"); - ASSERT_EQ(1.55, j1p_s.get_value()); - ASSERT_EQ(0.1, j1v_s.get_value()); - ASSERT_EQ(0.65, j2p_s.get_value()); - ASSERT_EQ(0.2, j2v_s.get_value()); - ASSERT_EQ(0.5, vo_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); - ASSERT_TRUE(std::isnan(vo_c.get_value())); + ASSERT_EQ(1.55, j1p_s.get_value().value()); + ASSERT_EQ(0.1, j1v_s.get_value().value()); + ASSERT_EQ(0.65, j2p_s.get_value().value()); + ASSERT_EQ(0.2, j2v_s.get_value().value()); + ASSERT_EQ(0.5, vo_s.get_value().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value().value())); + ASSERT_TRUE(std::isnan(vo_c.get_value().value())); // set some new values in commands j1p_c.set_value(0.11); @@ -1169,36 +1169,36 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) vo_c.set_value(0.99); // State values should not be changed - ASSERT_EQ(1.55, j1p_s.get_value()); - ASSERT_EQ(0.1, j1v_s.get_value()); - ASSERT_EQ(0.65, j2p_s.get_value()); - ASSERT_EQ(0.2, j2v_s.get_value()); - ASSERT_EQ(0.5, vo_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.99, vo_c.get_value()); + ASSERT_EQ(1.55, j1p_s.get_value().value()); + ASSERT_EQ(0.1, j1v_s.get_value().value()); + ASSERT_EQ(0.65, j2p_s.get_value().value()); + ASSERT_EQ(0.2, j2v_s.get_value().value()); + ASSERT_EQ(0.5, vo_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(0.99, vo_c.get_value().value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(1.55, j1p_s.get_value()); - ASSERT_EQ(0.1, j1v_s.get_value()); - ASSERT_EQ(0.65, j2p_s.get_value()); - ASSERT_EQ(0.2, j2v_s.get_value()); - ASSERT_EQ(0.5, vo_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.99, vo_c.get_value()); + ASSERT_EQ(1.55, j1p_s.get_value().value()); + ASSERT_EQ(0.1, j1v_s.get_value().value()); + ASSERT_EQ(0.65, j2p_s.get_value().value()); + ASSERT_EQ(0.2, j2v_s.get_value().value()); + ASSERT_EQ(0.5, vo_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(0.99, vo_c.get_value().value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.1, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - ASSERT_EQ(0.99, vo_s.get_value()); - ASSERT_EQ(0.2, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.99, vo_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value().value()); + ASSERT_EQ(0.1, j1v_s.get_value().value()); + ASSERT_EQ(0.33, j2p_s.get_value().value()); + ASSERT_EQ(0.99, vo_s.get_value().value()); + ASSERT_EQ(0.2, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(0.99, vo_c.get_value().value()); } TEST_F(TestGenericSystem, generic_system_2dof_sensor) @@ -1247,58 +1247,58 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) EXPECT_ANY_THROW(rm.claim_command_interface("tcp_force_sensor/tx")); EXPECT_ANY_THROW(rm.claim_command_interface("tcp_force_sensor/ty")); - ASSERT_EQ(0.0, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); + ASSERT_EQ(0.0, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.0, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value().value())); + EXPECT_TRUE(std::isnan(stx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sty_s.get_value().value())); + ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value().value())); // set some new values in commands j1p_c.set_value(0.11); j2p_c.set_value(0.33); // State values should not be changed - ASSERT_EQ(0.0, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.0, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.0, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value().value())); + EXPECT_TRUE(std::isnan(stx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sty_s.get_value().value())); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(0.0, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.0, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.0, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value().value())); + EXPECT_TRUE(std::isnan(stx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sty_s.get_value().value())); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.33, j2p_s.get_value().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value().value())); + EXPECT_TRUE(std::isnan(stx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sty_s.get_value().value())); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); } void TestGenericSystem::test_generic_system_with_mock_sensor_commands( @@ -1350,20 +1350,20 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands( hardware_interface::LoanedCommandInterface sty_c = rm.claim_command_interface("tcp_force_sensor/ty"); - ASSERT_EQ(0.0, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); - EXPECT_TRUE(std::isnan(sfx_c.get_value())); - EXPECT_TRUE(std::isnan(sfy_c.get_value())); - EXPECT_TRUE(std::isnan(stx_c.get_value())); - EXPECT_TRUE(std::isnan(sty_c.get_value())); + ASSERT_EQ(0.0, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.0, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value().value())); + EXPECT_TRUE(std::isnan(stx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sty_s.get_value().value())); + ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value().value())); + EXPECT_TRUE(std::isnan(sfx_c.get_value().value())); + EXPECT_TRUE(std::isnan(sfy_c.get_value().value())); + EXPECT_TRUE(std::isnan(stx_c.get_value().value())); + EXPECT_TRUE(std::isnan(sty_c.get_value().value())); // set some new values in commands j1p_c.set_value(0.11); @@ -1374,54 +1374,54 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands( sty_c.set_value(4.44); // State values should not be changed - ASSERT_EQ(0.0, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(1.11, sfx_c.get_value()); - ASSERT_EQ(2.22, sfy_c.get_value()); - ASSERT_EQ(3.33, stx_c.get_value()); - ASSERT_EQ(4.44, sty_c.get_value()); + ASSERT_EQ(0.0, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.0, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value().value())); + EXPECT_TRUE(std::isnan(stx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sty_s.get_value().value())); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(1.11, sfx_c.get_value().value()); + ASSERT_EQ(2.22, sfy_c.get_value().value()); + ASSERT_EQ(3.33, stx_c.get_value().value()); + ASSERT_EQ(4.44, sty_c.get_value().value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(0.0, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(1.11, sfx_c.get_value()); - ASSERT_EQ(2.22, sfy_c.get_value()); - ASSERT_EQ(3.33, stx_c.get_value()); - ASSERT_EQ(4.44, sty_c.get_value()); + ASSERT_EQ(0.0, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.0, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value().value())); + EXPECT_TRUE(std::isnan(stx_s.get_value().value())); + EXPECT_TRUE(std::isnan(sty_s.get_value().value())); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(1.11, sfx_c.get_value().value()); + ASSERT_EQ(2.22, sfy_c.get_value().value()); + ASSERT_EQ(3.33, stx_c.get_value().value()); + ASSERT_EQ(4.44, sty_c.get_value().value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(1.11, sfx_s.get_value()); - ASSERT_EQ(2.22, sfy_s.get_value()); - ASSERT_EQ(3.33, stx_s.get_value()); - ASSERT_EQ(4.44, sty_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(1.11, sfx_c.get_value()); - ASSERT_EQ(2.22, sfy_c.get_value()); - ASSERT_EQ(3.33, stx_c.get_value()); - ASSERT_EQ(4.44, sty_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.33, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_EQ(1.11, sfx_s.get_value().value()); + ASSERT_EQ(2.22, sfy_s.get_value().value()); + ASSERT_EQ(3.33, stx_s.get_value().value()); + ASSERT_EQ(4.44, sty_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(1.11, sfx_c.get_value().value()); + ASSERT_EQ(2.22, sfy_c.get_value().value()); + ASSERT_EQ(3.33, stx_c.get_value().value()); + ASSERT_EQ(4.44, sty_c.get_value().value()); } TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command) @@ -1468,42 +1468,42 @@ void TestGenericSystem::test_generic_system_with_mimic_joint( hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); - ASSERT_EQ(1.57, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_EQ(1.57, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.0, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value().value())); // set some new values in commands j1p_c.set_value(0.11); j1v_c.set_value(0.05); // State values should not be changed - ASSERT_EQ(1.57, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.05, j1v_c.get_value()); + ASSERT_EQ(1.57, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.0, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.05, j1v_c.get_value().value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(1.57, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.05, j1v_c.get_value()); + ASSERT_EQ(1.57, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.0, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.05, j1v_c.get_value().value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.05, j1v_s.get_value()); - ASSERT_EQ(-0.22, j2p_s.get_value()); - ASSERT_EQ(-0.1, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.05, j1v_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value().value()); + ASSERT_EQ(0.05, j1v_s.get_value().value()); + ASSERT_EQ(-0.22, j2p_s.get_value().value()); + ASSERT_EQ(-0.1, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.05, j1v_c.get_value().value()); } TEST_F(TestGenericSystem, hardware_system_2dof_with_mimic_joint) @@ -1577,14 +1577,14 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i rm.claim_state_interface("joint2/actual_position"); // State interfaces without initial value are set to 0 - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(2.78, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); // set some new values in commands j1p_c.set_value(0.11); @@ -1593,38 +1593,38 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i j2v_c.set_value(0.44); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(2.78, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.22, j1v_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(0.44, j2v_c.get_value().value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(2.78, j2p_s.get_value().value()); + ASSERT_EQ(0.0, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.22, j1v_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(0.44, j2v_c.get_value().value()); // read() mirrors commands + offset to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.11 + offset, c_j1p_s.get_value()); - ASSERT_EQ(0.22, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - ASSERT_EQ(0.33 + offset, c_j2p_s.get_value()); - ASSERT_EQ(0.44, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value().value()); + ASSERT_EQ(0.11 + offset, c_j1p_s.get_value().value()); + ASSERT_EQ(0.22, j1v_s.get_value().value()); + ASSERT_EQ(0.33, j2p_s.get_value().value()); + ASSERT_EQ(0.33 + offset, c_j2p_s.get_value().value()); + ASSERT_EQ(0.44, j2v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.22, j1v_c.get_value().value()); + ASSERT_EQ(0.33, j2p_c.get_value().value()); + ASSERT_EQ(0.44, j2v_c.get_value().value()); // set some new values in commands j1p_c.set_value(0.55); @@ -1633,16 +1633,16 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i j2v_c.set_value(0.88); // state values should not be changed - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.11 + offset, c_j1p_s.get_value()); - ASSERT_EQ(0.22, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - ASSERT_EQ(0.33 + offset, c_j2p_s.get_value()); - ASSERT_EQ(0.44, j2v_s.get_value()); - ASSERT_EQ(0.55, j1p_c.get_value()); - ASSERT_EQ(0.66, j1v_c.get_value()); - ASSERT_EQ(0.77, j2p_c.get_value()); - ASSERT_EQ(0.88, j2v_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value().value()); + ASSERT_EQ(0.11 + offset, c_j1p_s.get_value().value()); + ASSERT_EQ(0.22, j1v_s.get_value().value()); + ASSERT_EQ(0.33, j2p_s.get_value().value()); + ASSERT_EQ(0.33 + offset, c_j2p_s.get_value().value()); + ASSERT_EQ(0.44, j2v_s.get_value().value()); + ASSERT_EQ(0.55, j1p_c.get_value().value()); + ASSERT_EQ(0.66, j1v_c.get_value().value()); + ASSERT_EQ(0.77, j2p_c.get_value().value()); + ASSERT_EQ(0.88, j2v_c.get_value().value()); deactivate_components(rm, {hardware_name}); status_map = rm.get_components_status(); @@ -1705,44 +1705,44 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) rm.claim_command_interface("flange_vacuum/vacuum"); // State interfaces without initial value are set to 0 - ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); - ASSERT_TRUE(std::isnan(gpio2_vac_s.get_value())); - ASSERT_TRUE(std::isnan(gpio1_a_o1_c.get_value())); - ASSERT_TRUE(std::isnan(gpio2_vac_c.get_value())); + ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value().value())); + ASSERT_TRUE(std::isnan(gpio2_vac_s.get_value().value())); + ASSERT_TRUE(std::isnan(gpio1_a_o1_c.get_value().value())); + ASSERT_TRUE(std::isnan(gpio2_vac_c.get_value().value())); // set some new values in commands gpio1_a_o1_c.set_value(0.111); gpio2_vac_c.set_value(0.222); // State values should not be changed - ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); - ASSERT_TRUE(std::isnan(gpio2_vac_s.get_value())); - ASSERT_EQ(0.111, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.222, gpio2_vac_c.get_value()); + ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value().value())); + ASSERT_TRUE(std::isnan(gpio2_vac_s.get_value().value())); + ASSERT_EQ(0.111, gpio1_a_o1_c.get_value().value()); + ASSERT_EQ(0.222, gpio2_vac_c.get_value().value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); - ASSERT_TRUE(std::isnan(gpio2_vac_s.get_value())); - ASSERT_EQ(0.111, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.222, gpio2_vac_c.get_value()); + ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value().value())); + ASSERT_TRUE(std::isnan(gpio2_vac_s.get_value().value())); + ASSERT_EQ(0.111, gpio1_a_o1_c.get_value().value()); + ASSERT_EQ(0.222, gpio2_vac_c.get_value().value()); // read() mirrors commands + offset to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.111, gpio1_a_o1_s.get_value()); - ASSERT_EQ(0.222, gpio2_vac_s.get_value()); - ASSERT_EQ(0.111, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.222, gpio2_vac_c.get_value()); + ASSERT_EQ(0.111, gpio1_a_o1_s.get_value().value()); + ASSERT_EQ(0.222, gpio2_vac_s.get_value().value()); + ASSERT_EQ(0.111, gpio1_a_o1_c.get_value().value()); + ASSERT_EQ(0.222, gpio2_vac_c.get_value().value()); // set some new values in commands gpio1_a_o1_c.set_value(0.333); gpio2_vac_c.set_value(0.444); // state values should not be changed - ASSERT_EQ(0.111, gpio1_a_o1_s.get_value()); - ASSERT_EQ(0.222, gpio2_vac_s.get_value()); - ASSERT_EQ(0.333, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.444, gpio2_vac_c.get_value()); + ASSERT_EQ(0.111, gpio1_a_o1_s.get_value().value()); + ASSERT_EQ(0.222, gpio2_vac_s.get_value().value()); + ASSERT_EQ(0.333, gpio1_a_o1_c.get_value().value()); + ASSERT_EQ(0.444, gpio2_vac_c.get_value().value()); // check other functionalities are working well generic_system_functional_test(urdf, hardware_name); @@ -1807,14 +1807,14 @@ void TestGenericSystem::test_generic_system_with_mock_gpio_commands( hardware_interface::LoanedCommandInterface gpio2_vac_c = rm.claim_command_interface("flange_vacuum/vacuum"); - EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_value())); - EXPECT_TRUE(std::isnan(gpio2_vac_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_o1_c.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_i1_c.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_i2_c.get_value())); - EXPECT_TRUE(std::isnan(gpio2_vac_c.get_value())); + EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value().value())); + EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_value().value())); + EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_value().value())); + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_value().value())); + EXPECT_TRUE(std::isnan(gpio1_a_o1_c.get_value().value())); + EXPECT_TRUE(std::isnan(gpio1_a_i1_c.get_value().value())); + EXPECT_TRUE(std::isnan(gpio1_a_i2_c.get_value().value())); + EXPECT_TRUE(std::isnan(gpio2_vac_c.get_value().value())); // set some new values in commands gpio1_a_o1_c.set_value(0.11); @@ -1823,36 +1823,36 @@ void TestGenericSystem::test_generic_system_with_mock_gpio_commands( gpio2_vac_c.set_value(2.22); // State values should not be changed - EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_value())); - EXPECT_TRUE(std::isnan(gpio2_vac_s.get_value())); - ASSERT_EQ(0.11, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.33, gpio1_a_i1_c.get_value()); - ASSERT_EQ(1.11, gpio1_a_i2_c.get_value()); - ASSERT_EQ(2.22, gpio2_vac_c.get_value()); + EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value().value())); + EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_value().value())); + EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_value().value())); + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_value().value())); + ASSERT_EQ(0.11, gpio1_a_o1_c.get_value().value()); + ASSERT_EQ(0.33, gpio1_a_i1_c.get_value().value()); + ASSERT_EQ(1.11, gpio1_a_i2_c.get_value().value()); + ASSERT_EQ(2.22, gpio2_vac_c.get_value().value()); // write() does not change values rm.write(TIME, PERIOD); - EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_value())); - EXPECT_TRUE(std::isnan(gpio2_vac_s.get_value())); - ASSERT_EQ(0.11, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.33, gpio1_a_i1_c.get_value()); - ASSERT_EQ(1.11, gpio1_a_i2_c.get_value()); - ASSERT_EQ(2.22, gpio2_vac_c.get_value()); + EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value().value())); + EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_value().value())); + EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_value().value())); + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_value().value())); + ASSERT_EQ(0.11, gpio1_a_o1_c.get_value().value()); + ASSERT_EQ(0.33, gpio1_a_i1_c.get_value().value()); + ASSERT_EQ(1.11, gpio1_a_i2_c.get_value().value()); + ASSERT_EQ(2.22, gpio2_vac_c.get_value().value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.11, gpio1_a_o1_s.get_value()); - ASSERT_EQ(0.33, gpio1_a_i1_s.get_value()); - ASSERT_EQ(1.11, gpio1_a_o2_s.get_value()); - ASSERT_EQ(2.22, gpio2_vac_s.get_value()); - ASSERT_EQ(0.11, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.33, gpio1_a_i1_c.get_value()); - ASSERT_EQ(1.11, gpio1_a_i2_c.get_value()); - ASSERT_EQ(2.22, gpio2_vac_c.get_value()); + ASSERT_EQ(0.11, gpio1_a_o1_s.get_value().value()); + ASSERT_EQ(0.33, gpio1_a_i1_s.get_value().value()); + ASSERT_EQ(1.11, gpio1_a_o2_s.get_value().value()); + ASSERT_EQ(2.22, gpio2_vac_s.get_value().value()); + ASSERT_EQ(0.11, gpio1_a_o1_c.get_value().value()); + ASSERT_EQ(0.33, gpio1_a_i1_c.get_value().value()); + ASSERT_EQ(1.11, gpio1_a_i2_c.get_value().value()); + ASSERT_EQ(2.22, gpio2_vac_c.get_value().value()); } TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command) @@ -1896,9 +1896,9 @@ TEST_F(TestGenericSystem, sensor_with_initial_value) hardware_interface::LoanedStateInterface force_z_s = rm.claim_state_interface("force_sensor/force.z"); - ASSERT_EQ(0.0, force_x_s.get_value()); - ASSERT_EQ(0.0, force_y_s.get_value()); - ASSERT_EQ(0.0, force_z_s.get_value()); + ASSERT_EQ(0.0, force_x_s.get_value().value()); + ASSERT_EQ(0.0, force_y_s.get_value().value()); + ASSERT_EQ(0.0, force_z_s.get_value().value()); } TEST_F(TestGenericSystem, gpio_with_initial_value) @@ -1917,7 +1917,7 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) // Check initial values hardware_interface::LoanedStateInterface state = rm.claim_state_interface("sample_io/output_1"); - ASSERT_EQ(1, state.get_value()); + ASSERT_EQ(1, state.get_value().value()); } TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) @@ -1961,16 +1961,16 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) hardware_interface::LoanedCommandInterface j2a_c = rm.claim_command_interface("joint2/acceleration"); - EXPECT_EQ(3.45, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_EQ(0.0, j2v_s.get_value()); - EXPECT_EQ(0.0, j2a_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_TRUE(std::isnan(j2a_c.get_value())); + EXPECT_EQ(3.45, j1p_s.get_value().value()); + EXPECT_EQ(0.0, j1v_s.get_value().value()); + EXPECT_EQ(0.0, j1a_s.get_value().value()); + EXPECT_EQ(2.78, j2p_s.get_value().value()); + EXPECT_EQ(0.0, j2v_s.get_value().value()); + EXPECT_EQ(0.0, j2a_s.get_value().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2a_c.get_value().value())); // Test error management in prepare mode switch ASSERT_EQ( // joint2 has non 'position', 'velocity', or 'acceleration' interface @@ -1993,68 +1993,68 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) j2a_c.set_value(3.5); // State values should not be changed - EXPECT_EQ(3.45, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_EQ(0.0, j2v_s.get_value()); - EXPECT_EQ(0.0, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(3.45, j1p_s.get_value().value()); + EXPECT_EQ(0.0, j1v_s.get_value().value()); + EXPECT_EQ(0.0, j1a_s.get_value().value()); + EXPECT_EQ(2.78, j2p_s.get_value().value()); + EXPECT_EQ(0.0, j2v_s.get_value().value()); + EXPECT_EQ(0.0, j2a_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); + ASSERT_EQ(3.5, j2a_c.get_value().value()); // write() does not change values rm.write(TIME, PERIOD); - EXPECT_EQ(3.45, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_EQ(0.0, j2v_s.get_value()); - EXPECT_EQ(0.0, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(3.45, j1p_s.get_value().value()); + EXPECT_EQ(0.0, j1v_s.get_value().value()); + EXPECT_EQ(0.0, j1a_s.get_value().value()); + EXPECT_EQ(2.78, j2p_s.get_value().value()); + EXPECT_EQ(0.0, j2v_s.get_value().value()); + EXPECT_EQ(0.0, j2a_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); + ASSERT_EQ(3.5, j2a_c.get_value().value()); // read() mirrors commands to states and calculate dynamics rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(-33.4, j1v_s.get_value()); - EXPECT_NEAR(-334.0, j1a_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_EQ(0.0, j2v_s.get_value()); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value().value()); + EXPECT_EQ(-33.4, j1v_s.get_value().value()); + EXPECT_NEAR(-334.0, j1a_s.get_value().value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value().value()); + EXPECT_EQ(0.0, j2v_s.get_value().value()); + EXPECT_EQ(3.5, j2a_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); + ASSERT_EQ(3.5, j2a_c.get_value().value()); // read() mirrors commands to states and calculate dynamics rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_NEAR(334.0, j1a_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_NEAR(0.35, j2v_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value().value()); + EXPECT_EQ(0.0, j1v_s.get_value().value()); + EXPECT_NEAR(334.0, j1a_s.get_value().value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value().value()); + EXPECT_NEAR(0.35, j2v_s.get_value().value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); + ASSERT_EQ(3.5, j2a_c.get_value().value()); // read() mirrors commands to states and calculate dynamics rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.815, j2p_s.get_value()); - EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value().value()); + EXPECT_EQ(0.0, j1v_s.get_value().value()); + EXPECT_EQ(0.0, j1a_s.get_value().value()); + EXPECT_EQ(2.815, j2p_s.get_value().value()); + EXPECT_NEAR(0.7, j2v_s.get_value().value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); + ASSERT_EQ(3.5, j2a_c.get_value().value()); // switch controller mode as controller manager is doing ASSERT_EQ(rm.prepare_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); @@ -2065,55 +2065,55 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) j2v_c.set_value(2.0); // State values should not be changed - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.815, j2p_s.get_value()); - EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.5, j1v_c.get_value()); - ASSERT_EQ(2.0, j2v_c.get_value()); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value().value()); + EXPECT_EQ(0.0, j1v_s.get_value().value()); + EXPECT_EQ(0.0, j1a_s.get_value().value()); + EXPECT_EQ(2.815, j2p_s.get_value().value()); + EXPECT_NEAR(0.7, j2v_s.get_value().value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.5, j1v_c.get_value().value()); + ASSERT_EQ(2.0, j2v_c.get_value().value()); + ASSERT_EQ(3.5, j2a_c.get_value().value()); // write() does not change values rm.write(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.815, j2p_s.get_value()); - EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.5, j1v_c.get_value()); - ASSERT_EQ(2.0, j2v_c.get_value()); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value().value()); + EXPECT_EQ(0.0, j1v_s.get_value().value()); + EXPECT_EQ(0.0, j1a_s.get_value().value()); + EXPECT_EQ(2.815, j2p_s.get_value().value()); + EXPECT_NEAR(0.7, j2v_s.get_value().value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.5, j1v_c.get_value().value()); + ASSERT_EQ(2.0, j2v_c.get_value().value()); + ASSERT_EQ(3.5, j2a_c.get_value().value()); // read() mirrors commands to states and calculate dynamics (both velocity mode) rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.5, j1v_s.get_value()); - EXPECT_EQ(5.0, j1a_s.get_value()); - EXPECT_EQ(2.885, j2p_s.get_value()); - EXPECT_EQ(2.0, j2v_s.get_value()); - EXPECT_NEAR(13.0, j2a_s.get_value(), COMPARE_DELTA); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.5, j1v_c.get_value()); - ASSERT_EQ(2.0, j2v_c.get_value()); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value().value()); + EXPECT_EQ(0.5, j1v_s.get_value().value()); + EXPECT_EQ(5.0, j1a_s.get_value().value()); + EXPECT_EQ(2.885, j2p_s.get_value().value()); + EXPECT_EQ(2.0, j2v_s.get_value().value()); + EXPECT_NEAR(13.0, j2a_s.get_value().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.5, j1v_c.get_value().value()); + ASSERT_EQ(2.0, j2v_c.get_value().value()); + ASSERT_EQ(3.5, j2a_c.get_value().value()); // read() mirrors commands to states and calculate dynamics (both velocity mode) rm.read(TIME, PERIOD); - EXPECT_EQ(0.16, j1p_s.get_value()); - EXPECT_EQ(0.5, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(3.085, j2p_s.get_value()); - EXPECT_EQ(2.0, j2v_s.get_value()); - EXPECT_EQ(0.0, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.5, j1v_c.get_value()); - ASSERT_EQ(2.0, j2v_c.get_value()); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.16, j1p_s.get_value().value()); + EXPECT_EQ(0.5, j1v_s.get_value().value()); + EXPECT_EQ(0.0, j1a_s.get_value().value()); + EXPECT_EQ(3.085, j2p_s.get_value().value()); + EXPECT_EQ(2.0, j2v_s.get_value().value()); + EXPECT_EQ(0.0, j2a_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); + ASSERT_EQ(0.5, j1v_c.get_value().value()); + ASSERT_EQ(2.0, j2v_c.get_value().value()); + ASSERT_EQ(3.5, j2a_c.get_value().value()); } TEST_F(TestGenericSystem, disabled_commands_flag_is_active) @@ -2138,29 +2138,29 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); // set some new values in commands j1p_c.set_value(0.11); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); // read() also does not change values rm.read(TIME, PERIOD); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value().value()); + ASSERT_EQ(0.0, j1v_s.get_value().value()); + ASSERT_EQ(0.11, j1p_c.get_value().value()); } TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tags) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 28a74aeb66..d0b134c6ba 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -773,8 +773,8 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value().value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value().value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -788,8 +788,10 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[0]->get_value().value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[1]->get_value().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -803,8 +805,10 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[0]->get_value().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -818,8 +822,9 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[0]->get_value().value()); // position value + EXPECT_EQ(0, state_interfaces[1]->get_value().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -892,8 +897,10 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint1_pos]->get_value().value())); // position value + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint1_vel]->get_value().value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -908,8 +915,11 @@ TEST(TestComponentInterfaces, dummy_actuator_default) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + step * velocity_value, + state_interfaces[si_joint1_pos]->get_value().value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, + state_interfaces[si_joint1_vel]->get_value().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -925,8 +935,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity + state_interfaces[si_joint1_pos]->get_value().value()); // position value + EXPECT_EQ( + velocity_value, state_interfaces[si_joint1_vel]->get_value().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -940,8 +951,10 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, + state_interfaces[si_joint1_pos]->get_value().value()); // position value + EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -969,21 +982,21 @@ TEST(TestComponentInterfaces, dummy_sensor) EXPECT_EQ("sens1/voltage", state_interfaces[0]->get_name()); EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); EXPECT_EQ("sens1", state_interfaces[0]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value().value())); // Not updated because is is UNCONFIGURED sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value().value())); // Updated because is is INACTIVE state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[0]->get_value()); + EXPECT_EQ(0.0, state_interfaces[0]->get_value().value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[0]->get_value()); + EXPECT_EQ(0x666, state_interfaces[0]->get_value().value()); } // END @@ -1012,23 +1025,23 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name()); EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value().value())); } // Not updated because is is UNCONFIGURED auto si_sens1_vol = test_components::vector_contains(state_interfaces, "sens1/voltage").second; sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value().value())); // Updated because is is INACTIVE state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value()); + EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value().value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value()); + EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value().value()); } TEST(TestComponentInterfaces, dummy_sensor_default_joint) @@ -1058,7 +1071,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_joint) EXPECT_EQ("sens1/voltage", state_interfaces[si_sens1_vol]->get_name()); EXPECT_EQ("voltage", state_interfaces[si_sens1_vol]->get_interface_name()); EXPECT_EQ("sens1", state_interfaces[si_sens1_vol]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value().value())); auto [contains_joint1_pos, si_joint1_pos] = test_components::vector_contains(state_interfaces, "joint1/position"); @@ -1066,24 +1079,24 @@ TEST(TestComponentInterfaces, dummy_sensor_default_joint) EXPECT_EQ("joint1/position", state_interfaces[si_joint1_pos]->get_name()); EXPECT_EQ("position", state_interfaces[si_joint1_pos]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[si_joint1_pos]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value().value())); // Not updated because is is UNCONFIGURED sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value())); - EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value().value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value().value())); // Updated because is is INACTIVE state = sensor_hw.configure(); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); ASSERT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value()); - EXPECT_EQ(10.0, state_interfaces[si_joint1_pos]->get_value()); + EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value().value()); + EXPECT_EQ(10.0, state_interfaces[si_joint1_pos]->get_value().value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value()); - EXPECT_EQ(0x777, state_interfaces[si_joint1_pos]->get_value()); + EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value().value()); + EXPECT_EQ(0x777, state_interfaces[si_joint1_pos]->get_value().value()); } // BEGIN (Handle export change): for backward compatibility @@ -1142,12 +1155,12 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value().value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value().value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value().value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value().value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value().value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value().value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1161,12 +1174,18 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[0]->get_value().value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[1]->get_value().value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[2]->get_value().value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[3]->get_value().value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[4]->get_value().value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[5]->get_value().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1180,12 +1199,18 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[5]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[0]->get_value().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value().value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[2]->get_value().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[3]->get_value().value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[4]->get_value().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[5]->get_value().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1199,12 +1224,15 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[0]->get_value().value()); // position value + EXPECT_EQ(0.0, state_interfaces[1]->get_value().value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[2]->get_value().value()); // position value + EXPECT_EQ(0.0, state_interfaces[3]->get_value().value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[4]->get_value().value()); // position value + EXPECT_EQ(0.0, state_interfaces[5]->get_value().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1335,12 +1363,18 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_pos]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_vel]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_pos]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_vel]->get_value())); // velocity + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint1_pos]->get_value().value())); // position value + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint1_vel]->get_value().value())); // velocity + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint2_pos]->get_value().value())); // position value + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint2_vel]->get_value().value())); // velocity + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint3_pos]->get_value().value())); // position value + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint3_vel]->get_value().value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1355,14 +1389,23 @@ TEST(TestComponentInterfaces, dummy_system_default) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + step * velocity_value, + state_interfaces[si_joint1_pos]->get_value().value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, + state_interfaces[si_joint1_vel]->get_value().value()); // velocity EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint2_vel]->get_value()); // velocity + step * velocity_value, + state_interfaces[si_joint2_pos]->get_value().value()); // position value EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint3_vel]->get_value()); // velocity + step ? velocity_value : 0, + state_interfaces[si_joint2_vel]->get_value().value()); // velocity + EXPECT_EQ( + step * velocity_value, + state_interfaces[si_joint3_pos]->get_value().value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, + state_interfaces[si_joint3_vel]->get_value().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1378,16 +1421,19 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity + state_interfaces[si_joint1_pos]->get_value().value()); // position value + EXPECT_EQ( + velocity_value, state_interfaces[si_joint1_vel]->get_value().value()); // velocity EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint2_pos]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint2_vel]->get_value()); // velocity + state_interfaces[si_joint2_pos]->get_value().value()); // position value + EXPECT_EQ( + velocity_value, state_interfaces[si_joint2_vel]->get_value().value()); // velocity EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint3_pos]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint3_vel]->get_value()); // velocity + state_interfaces[si_joint3_pos]->get_value().value()); // position value + EXPECT_EQ( + velocity_value, state_interfaces[si_joint3_vel]->get_value().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1401,12 +1447,18 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, + state_interfaces[si_joint1_pos]->get_value().value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value().value()); // velocity + EXPECT_EQ( + 20 * velocity_value, + state_interfaces[si_joint2_pos]->get_value().value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value().value()); // velocity + EXPECT_EQ( + 20 * velocity_value, + state_interfaces[si_joint3_pos]->get_value().value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1483,8 +1535,8 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value().value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value().value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1556,8 +1608,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) auto ci_joint1_vel = test_components::vector_contains(command_interfaces, "joint1/velocity").second; state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value().value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value().value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1618,8 +1670,8 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value().value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value().value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1690,8 +1742,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) auto ci_joint1_vel = test_components::vector_contains(command_interfaces, "joint1/velocity").second; state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value().value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value().value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1755,7 +1807,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) // activate again and expect reset values state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value().value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1823,7 +1875,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) // activate again and expect reset values auto si_joint1_vol = test_components::vector_contains(state_interfaces, "sens1/voltage").second; state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[si_joint1_vol]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_vol]->get_value().value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1883,11 +1935,11 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value().value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value().value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1956,11 +2008,11 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value().value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value().value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -2023,11 +2075,11 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value().value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value().value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -2096,11 +2148,11 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value().value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value().value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index 62e2b703cf..e9fd5519ec 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -250,7 +250,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name()); EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value().value())); } { auto [contains, position] = diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index c72d574fca..b60cc470d5 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -31,10 +31,10 @@ TEST(TestHandle, command_interface) { double value = 1.337; CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(interface.get_value(), value); + EXPECT_DOUBLE_EQ(interface.get_value().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_DOUBLE_EQ(interface.get_value().value(), value); EXPECT_NO_THROW(bool status = interface.set_value(0.0)); ASSERT_TRUE(interface.get_value().has_value()); EXPECT_DOUBLE_EQ(interface.get_value().value(), 0.0); @@ -60,7 +60,7 @@ 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.get_value().value()); EXPECT_ANY_THROW(bool status = handle.set_value(0.0)); } diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index be9e672b3b..0141d20ba4 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1286,13 +1286,13 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) EXPECT_TRUE(rm.command_interface_is_claimed(FULL_REFERENCE_INTERFACE_NAMES[2])); // access interface value - EXPECT_EQ(claimed_itf1.get_value(), 1.0); - EXPECT_EQ(claimed_itf3.get_value(), 3.0); + EXPECT_EQ(claimed_itf1.get_value().value(), 1.0); + EXPECT_EQ(claimed_itf3.get_value().value(), 3.0); claimed_itf1.set_value(11.1); claimed_itf3.set_value(33.3); - EXPECT_EQ(claimed_itf1.get_value(), 11.1); - EXPECT_EQ(claimed_itf3.get_value(), 33.3); + EXPECT_EQ(claimed_itf1.get_value().value(), 11.1); + EXPECT_EQ(claimed_itf3.get_value().value(), 33.3); EXPECT_EQ(reference_interface_values[0], 11.1); EXPECT_EQ(reference_interface_values[1], 2.0); @@ -1821,8 +1821,8 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage void check_read_and_write_cycles(bool test_for_changing_values) { - double prev_act_state_value = state_itfs[0].get_value(); - double prev_system_state_value = state_itfs[1].get_value(); + double prev_act_state_value = state_itfs[0].get_value().value(); + double prev_system_state_value = state_itfs[1].get_value().value(); for (size_t i = 1; i < 100; i++) { @@ -1832,19 +1832,19 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage if (i % (cm_update_rate_ / system_rw_rate_) == 0 && test_for_changing_values) { // The values are computations exactly within the test_components - prev_system_state_value = claimed_itfs[1].get_value() / 2.0; - claimed_itfs[1].set_value(claimed_itfs[1].get_value() + 20.0); + prev_system_state_value = claimed_itfs[1].get_value().value() / 2.0; + claimed_itfs[1].set_value(claimed_itfs[1].get_value().value() + 20.0); } if (i % (cm_update_rate_ / actuator_rw_rate_) == 0 && test_for_changing_values) { // The values are computations exactly within the test_components - prev_act_state_value = claimed_itfs[0].get_value() / 2.0; - claimed_itfs[0].set_value(claimed_itfs[0].get_value() + 10.0); + prev_act_state_value = claimed_itfs[0].get_value().value() / 2.0; + claimed_itfs[0].set_value(claimed_itfs[0].get_value().value() + 10.0); } // Even though we skip some read and write iterations, the state interfaces should be the same // as previous updated one until the next cycle - ASSERT_EQ(state_itfs[0].get_value(), prev_act_state_value); - ASSERT_EQ(state_itfs[1].get_value(), prev_system_state_value); + ASSERT_EQ(state_itfs[0].get_value().value(), prev_act_state_value); + ASSERT_EQ(state_itfs[1].get_value().value(), prev_system_state_value); auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); EXPECT_TRUE(ok_write); EXPECT_TRUE(failed_hardware_names_write.empty()); @@ -2034,8 +2034,8 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest void check_read_and_write_cycles(bool check_for_updated_values) { - double prev_act_state_value = state_itfs[0].get_value(); - double prev_system_state_value = state_itfs[1].get_value(); + double prev_act_state_value = state_itfs[0].get_value().value(); + double prev_system_state_value = state_itfs[1].get_value().value(); const double actuator_increment = 10.0; const double system_increment = 20.0; for (size_t i = 1; i < 100; i++) @@ -2047,15 +2047,17 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest // The values are computations exactly within the test_components if (check_for_updated_values) { - prev_system_state_value = claimed_itfs[1].get_value() / 2.0; - prev_act_state_value = claimed_itfs[0].get_value() / 2.0; + prev_system_state_value = claimed_itfs[1].get_value().value() / 2.0; + prev_act_state_value = claimed_itfs[0].get_value().value() / 2.0; } - claimed_itfs[0].set_value(claimed_itfs[0].get_value() + actuator_increment); - claimed_itfs[1].set_value(claimed_itfs[1].get_value() + system_increment); + claimed_itfs[0].set_value(claimed_itfs[0].get_value().value() + actuator_increment); + claimed_itfs[1].set_value(claimed_itfs[1].get_value().value() + system_increment); // This is needed to account for any missing hits to the read and write cycles as the tests // are going to be run on a non-RT operating system - ASSERT_NEAR(state_itfs[0].get_value(), prev_act_state_value, actuator_increment / 2.0); - ASSERT_NEAR(state_itfs[1].get_value(), prev_system_state_value, system_increment / 2.0); + ASSERT_NEAR( + state_itfs[0].get_value().value(), prev_act_state_value, actuator_increment / 2.0); + ASSERT_NEAR( + state_itfs[1].get_value().value(), prev_system_state_value, system_increment / 2.0); auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); std::this_thread::sleep_for(std::chrono::milliseconds(1)); EXPECT_TRUE(ok_write); diff --git a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp index d730029d90..db664af74d 100644 --- a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp +++ b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp @@ -128,47 +128,47 @@ TEST_F( // When TestSystemCommandModes is ACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); // When TestActuatorHardware is UNCONFIGURED expect OK EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 403.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 403.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 403.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 403.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 503.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 503.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 503.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 503.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 603.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 603.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); }; // System : ACTIVE @@ -182,47 +182,47 @@ TEST_F( // When TestSystemCommandModes is ACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 1.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 1.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 101.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 101.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 102.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 102.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 202.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 202.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 203.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 203.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 303.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 303.0); // When TestActuatorHardware is INACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 304.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 304.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 304.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 304.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 404.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 404.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 404.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 404.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 405.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 405.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 405.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 405.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 505.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 505.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 505.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 505.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 506.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 506.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 506.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 506.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 606.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 606.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 606.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 606.0); }; // System : INACTIVE @@ -236,47 +236,47 @@ TEST_F( // When TestSystemCommandModes is INACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 1.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 1.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 101.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 101.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 102.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 102.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 202.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 202.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 203.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 203.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 303.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 303.0); // When TestActuatorHardware is ACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 304.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 304.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 304.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 304.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 404.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 404.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 404.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 404.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 405.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 405.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 405.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 405.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 505.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 505.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 505.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 505.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 506.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 506.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 506.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 506.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 606.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 606.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 606.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 606.0); }; // System : UNCONFIGURED @@ -291,47 +291,47 @@ TEST_F( // When TestSystemCommandModes is UNCONFIGURED expect error EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 100.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 100.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 100.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 100.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 200.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 200.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 200.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 200.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 300.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 300.0); // When TestActuatorHardware is INACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 301.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 301.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 401.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 401.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 402.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 402.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 502.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 502.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 503.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 503.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 603.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 603.0); }; // System : UNCONFIGURED @@ -346,47 +346,47 @@ TEST_F( // When TestSystemCommandModes is UNCONFIGURED expect error EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); // When TestActuatorHardware is INACTIVE expect OK EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); }; int main(int argc, char ** argv) From 7cd44c263d14e6b75c7dde6ab26b5e7f351e4001 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 18:28:33 +0100 Subject: [PATCH 7/9] use (void) to discard set_value return statement --- .../test_chainable_controller_interface.cpp | 2 +- .../test_chainable_controller.cpp | 2 +- .../test/test_controller/test_controller.cpp | 4 +- .../mock_components/test_generic_system.cpp | 108 +++++++++--------- .../test/test_component_interfaces.cpp | 16 +-- 5 files changed, 66 insertions(+), 66 deletions(-) diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 2c5b9090ff..9919ec3d60 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -130,7 +130,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Success setting chained mode - reference_interfaces[0]->set_value(0.0); + (void)reference_interfaces[0]->set_value(0.0); EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index e94163b503..644bda10cd 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -101,7 +101,7 @@ controller_interface::return_type TestChainableController::update_and_write_comm for (size_t i = 0; i < command_interfaces_.size(); ++i) { - command_interfaces_[i].set_value( + (void)command_interfaces_[i].set_value( reference_interfaces_[i] - state_interfaces_[i].get_value().value()); } // If there is a command interface then integrate and set it to the exported state interface data diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index e26d3ea328..cadaf115ae 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -71,7 +71,7 @@ controller_interface::return_type TestController::update( // set value to hardware to produce and test different behaviors there if (!std::isnan(set_first_command_interface_value_to)) { - command_interfaces_[0].set_value(set_first_command_interface_value_to); + (void)command_interfaces_[0].set_value(set_first_command_interface_value_to); // reset to be easier to test set_first_command_interface_value_to = std::numeric_limits::quiet_NaN(); } @@ -90,7 +90,7 @@ controller_interface::return_type TestController::update( RCLCPP_DEBUG( get_node()->get_logger(), "Setting value of command interface '%s' to %f", command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]); - command_interfaces_[i].set_value(external_commands_for_testing_[i]); + (void)command_interfaces_[i].set_value(external_commands_for_testing_[i]); } } diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index e52a2f11fc..ca2683d2c8 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -877,10 +877,10 @@ void generic_system_functional_test( ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); // set some new values in commands - j1p_c.set_value(0.11); - j1v_c.set_value(0.22); - j2p_c.set_value(0.33); - j2v_c.set_value(0.44); + (void)j1p_c.set_value(0.11); + (void)j1v_c.set_value(0.22); + (void)j2p_c.set_value(0.33); + (void)j2v_c.set_value(0.44); // State values should not be changed ASSERT_EQ(3.45, j1p_s.get_value().value()); @@ -915,10 +915,10 @@ void generic_system_functional_test( ASSERT_EQ(0.44, j2v_c.get_value().value()); // set some new values in commands - j1p_c.set_value(0.55); - j1v_c.set_value(0.66); - j2p_c.set_value(0.77); - j2v_c.set_value(0.88); + (void)j1p_c.set_value(0.55); + (void)j1v_c.set_value(0.66); + (void)j2p_c.set_value(0.77); + (void)j2v_c.set_value(0.88); // state values should not be changed ASSERT_EQ(0.11 + offset, j1p_s.get_value().value()); @@ -983,10 +983,10 @@ void generic_system_error_group_test( ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); // set some new values in commands - j1p_c.set_value(0.11); - j1v_c.set_value(0.22); - j2p_c.set_value(0.33); - j2v_c.set_value(0.44); + (void)j1p_c.set_value(0.11); + (void)j1v_c.set_value(0.22); + (void)j2p_c.set_value(0.33); + (void)j2v_c.set_value(0.44); // State values should not be changed ASSERT_EQ(3.45, j1p_s.get_value().value()); @@ -1021,10 +1021,10 @@ void generic_system_error_group_test( ASSERT_EQ(0.44, j2v_c.get_value().value()); // set some new values in commands - j1p_c.set_value(0.55); - j1v_c.set_value(0.66); - j2p_c.set_value(0.77); - j2v_c.set_value(0.88); + (void)j1p_c.set_value(0.55); + (void)j1v_c.set_value(0.66); + (void)j2p_c.set_value(0.77); + (void)j2v_c.set_value(0.88); // state values should not be changed ASSERT_EQ(0.11, j1p_s.get_value().value()); @@ -1037,8 +1037,8 @@ void generic_system_error_group_test( ASSERT_EQ(0.88, j2v_c.get_value().value()); // Error testing - j1p_c.set_value(std::numeric_limits::infinity()); - j1v_c.set_value(std::numeric_limits::infinity()); + (void)j1p_c.set_value(std::numeric_limits::infinity()); + (void)j1v_c.set_value(std::numeric_limits::infinity()); // read() should now bring error in the first component auto read_result = rm.read(TIME, PERIOD); ASSERT_FALSE(read_result.ok); @@ -1075,8 +1075,8 @@ void generic_system_error_group_test( } // Error should be recoverable only after reactivating the hardware component - j1p_c.set_value(0.0); - j1v_c.set_value(0.0); + (void)j1p_c.set_value(0.0); + (void)j1v_c.set_value(0.0); ASSERT_FALSE(rm.read(TIME, PERIOD).ok); // Now it should be recoverable @@ -1164,9 +1164,9 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) ASSERT_TRUE(std::isnan(vo_c.get_value().value())); // set some new values in commands - j1p_c.set_value(0.11); - j2p_c.set_value(0.33); - vo_c.set_value(0.99); + (void)j1p_c.set_value(0.11); + (void)j2p_c.set_value(0.33); + (void)vo_c.set_value(0.99); // State values should not be changed ASSERT_EQ(1.55, j1p_s.get_value().value()); @@ -1259,8 +1259,8 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) ASSERT_TRUE(std::isnan(j2p_c.get_value().value())); // set some new values in commands - j1p_c.set_value(0.11); - j2p_c.set_value(0.33); + (void)j1p_c.set_value(0.11); + (void)j2p_c.set_value(0.33); // State values should not be changed ASSERT_EQ(0.0, j1p_s.get_value().value()); @@ -1366,12 +1366,12 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands( EXPECT_TRUE(std::isnan(sty_c.get_value().value())); // set some new values in commands - j1p_c.set_value(0.11); - j2p_c.set_value(0.33); - sfx_c.set_value(1.11); - sfy_c.set_value(2.22); - stx_c.set_value(3.33); - sty_c.set_value(4.44); + (void)j1p_c.set_value(0.11); + (void)j2p_c.set_value(0.33); + (void)sfx_c.set_value(1.11); + (void)sfy_c.set_value(2.22); + (void)stx_c.set_value(3.33); + (void)sty_c.set_value(4.44); // State values should not be changed ASSERT_EQ(0.0, j1p_s.get_value().value()); @@ -1476,8 +1476,8 @@ void TestGenericSystem::test_generic_system_with_mimic_joint( ASSERT_TRUE(std::isnan(j1v_c.get_value().value())); // set some new values in commands - j1p_c.set_value(0.11); - j1v_c.set_value(0.05); + (void)j1p_c.set_value(0.11); + (void)j1v_c.set_value(0.05); // State values should not be changed ASSERT_EQ(1.57, j1p_s.get_value().value()); @@ -1587,10 +1587,10 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); // set some new values in commands - j1p_c.set_value(0.11); - j1v_c.set_value(0.22); - j2p_c.set_value(0.33); - j2v_c.set_value(0.44); + (void)j1p_c.set_value(0.11); + (void)j1v_c.set_value(0.22); + (void)j2p_c.set_value(0.33); + (void)j2v_c.set_value(0.44); // State values should not be changed ASSERT_EQ(3.45, j1p_s.get_value().value()); @@ -1627,10 +1627,10 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ASSERT_EQ(0.44, j2v_c.get_value().value()); // set some new values in commands - j1p_c.set_value(0.55); - j1v_c.set_value(0.66); - j2p_c.set_value(0.77); - j2v_c.set_value(0.88); + (void)j1p_c.set_value(0.55); + (void)j1v_c.set_value(0.66); + (void)j2p_c.set_value(0.77); + (void)j2v_c.set_value(0.88); // state values should not be changed ASSERT_EQ(0.11, j1p_s.get_value().value()); @@ -1711,8 +1711,8 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) ASSERT_TRUE(std::isnan(gpio2_vac_c.get_value().value())); // set some new values in commands - gpio1_a_o1_c.set_value(0.111); - gpio2_vac_c.set_value(0.222); + (void)gpio1_a_o1_c.set_value(0.111); + (void)gpio2_vac_c.set_value(0.222); // State values should not be changed ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value().value())); @@ -1735,8 +1735,8 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) ASSERT_EQ(0.222, gpio2_vac_c.get_value().value()); // set some new values in commands - gpio1_a_o1_c.set_value(0.333); - gpio2_vac_c.set_value(0.444); + (void)gpio1_a_o1_c.set_value(0.333); + (void)gpio2_vac_c.set_value(0.444); // state values should not be changed ASSERT_EQ(0.111, gpio1_a_o1_s.get_value().value()); @@ -1817,10 +1817,10 @@ void TestGenericSystem::test_generic_system_with_mock_gpio_commands( EXPECT_TRUE(std::isnan(gpio2_vac_c.get_value().value())); // set some new values in commands - gpio1_a_o1_c.set_value(0.11); - gpio1_a_i1_c.set_value(0.33); - gpio1_a_i2_c.set_value(1.11); - gpio2_vac_c.set_value(2.22); + (void)gpio1_a_o1_c.set_value(0.11); + (void)gpio1_a_i1_c.set_value(0.33); + (void)gpio1_a_i2_c.set_value(1.11); + (void)gpio2_vac_c.set_value(2.22); // State values should not be changed EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value().value())); @@ -1989,8 +1989,8 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) true); // set some new values in commands - j1p_c.set_value(0.11); - j2a_c.set_value(3.5); + (void)j1p_c.set_value(0.11); + (void)j2a_c.set_value(3.5); // State values should not be changed EXPECT_EQ(3.45, j1p_s.get_value().value()); @@ -2061,8 +2061,8 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) ASSERT_EQ(rm.perform_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); // set some new values in commands - j1v_c.set_value(0.5); - j2v_c.set_value(2.0); + (void)j1v_c.set_value(0.5); + (void)j2v_c.set_value(2.0); // State values should not be changed EXPECT_EQ(0.11, j1p_s.get_value().value()); @@ -2143,7 +2143,7 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); // set some new values in commands - j1p_c.set_value(0.11); + (void)j1p_c.set_value(0.11); // State values should not be changed ASSERT_EQ(3.45, j1p_s.get_value().value()); diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index d0b134c6ba..f2b6684f21 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -765,7 +765,7 @@ TEST(TestComponentInterfaces, dummy_actuator) EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); double velocity_value = 1.0; - command_interfaces[0]->set_value(velocity_value); // velocity + (void)command_interfaces[0]->set_value(velocity_value); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED @@ -887,7 +887,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default) double velocity_value = 1.0; auto ci_joint1_vel = test_components::vector_contains(command_interfaces, "joint1/velocity").second; - command_interfaces[ci_joint1_vel]->set_value(velocity_value); // velocity + (void)command_interfaces[ci_joint1_vel]->set_value(velocity_value); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED @@ -1145,9 +1145,9 @@ TEST(TestComponentInterfaces, dummy_system) EXPECT_EQ("joint3", command_interfaces[2]->get_prefix_name()); double velocity_value = 1.0; - command_interfaces[0]->set_value(velocity_value); // velocity - command_interfaces[1]->set_value(velocity_value); // velocity - command_interfaces[2]->set_value(velocity_value); // velocity + (void)command_interfaces[0]->set_value(velocity_value); // velocity + (void)command_interfaces[1]->set_value(velocity_value); // velocity + (void)command_interfaces[2]->set_value(velocity_value); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED @@ -1347,9 +1347,9 @@ TEST(TestComponentInterfaces, dummy_system_default) auto ci_joint3_vel = test_components::vector_contains(command_interfaces, "joint3/velocity").second; double velocity_value = 1.0; - command_interfaces[ci_joint1_vel]->set_value(velocity_value); // velocity - command_interfaces[ci_joint2_vel]->set_value(velocity_value); // velocity - command_interfaces[ci_joint3_vel]->set_value(velocity_value); // velocity + (void)command_interfaces[ci_joint1_vel]->set_value(velocity_value); // velocity + (void)command_interfaces[ci_joint2_vel]->set_value(velocity_value); // velocity + (void)command_interfaces[ci_joint3_vel]->set_value(velocity_value); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED From 6dc3af7c112d716ad8aa238614ebc7a8e9924120 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 19:11:09 +0100 Subject: [PATCH 8/9] Add assertion instead of discarding the output --- .../include/hardware_interface/handle.hpp | 6 +- .../mock_components/test_generic_system.cpp | 108 +++++++++--------- .../test/test_component_interfaces.cpp | 16 +-- 3 files changed, 67 insertions(+), 63 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 9b0a04c753..ae29d2c6b5 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -168,7 +168,11 @@ class Handle { return std::nullopt; } - return value_ptr_ != nullptr ? *value_ptr_ : std::get(value_); + THROW_ON_NULLPTR(this->value_ptr_); + // TODO(saikishor): uncomment the following line when HANDLE_DATATYPE is used with multiple + // datatypes and remove the line below it. + // return value_ptr_ != nullptr ? *value_ptr_ : std::get(value_); + return *value_ptr_; } template diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index ca2683d2c8..815647b3f5 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -877,10 +877,10 @@ void generic_system_functional_test( ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); // set some new values in commands - (void)j1p_c.set_value(0.11); - (void)j1v_c.set_value(0.22); - (void)j2p_c.set_value(0.33); - (void)j2v_c.set_value(0.44); + ASSERT_TRUE(j1p_c.set_value(0.11)); + ASSERT_TRUE(j1v_c.set_value(0.22)); + ASSERT_TRUE(j2p_c.set_value(0.33)); + ASSERT_TRUE(j2v_c.set_value(0.44)); // State values should not be changed ASSERT_EQ(3.45, j1p_s.get_value().value()); @@ -915,10 +915,10 @@ void generic_system_functional_test( ASSERT_EQ(0.44, j2v_c.get_value().value()); // set some new values in commands - (void)j1p_c.set_value(0.55); - (void)j1v_c.set_value(0.66); - (void)j2p_c.set_value(0.77); - (void)j2v_c.set_value(0.88); + ASSERT_TRUE(j1p_c.set_value(0.55)); + ASSERT_TRUE(j1v_c.set_value(0.66)); + ASSERT_TRUE(j2p_c.set_value(0.77)); + ASSERT_TRUE(j2v_c.set_value(0.88)); // state values should not be changed ASSERT_EQ(0.11 + offset, j1p_s.get_value().value()); @@ -983,10 +983,10 @@ void generic_system_error_group_test( ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); // set some new values in commands - (void)j1p_c.set_value(0.11); - (void)j1v_c.set_value(0.22); - (void)j2p_c.set_value(0.33); - (void)j2v_c.set_value(0.44); + ASSERT_TRUE(j1p_c.set_value(0.11)); + ASSERT_TRUE(j1v_c.set_value(0.22)); + ASSERT_TRUE(j2p_c.set_value(0.33)); + ASSERT_TRUE(j2v_c.set_value(0.44)); // State values should not be changed ASSERT_EQ(3.45, j1p_s.get_value().value()); @@ -1021,10 +1021,10 @@ void generic_system_error_group_test( ASSERT_EQ(0.44, j2v_c.get_value().value()); // set some new values in commands - (void)j1p_c.set_value(0.55); - (void)j1v_c.set_value(0.66); - (void)j2p_c.set_value(0.77); - (void)j2v_c.set_value(0.88); + ASSERT_TRUE(j1p_c.set_value(0.55)); + ASSERT_TRUE(j1v_c.set_value(0.66)); + ASSERT_TRUE(j2p_c.set_value(0.77)); + ASSERT_TRUE(j2v_c.set_value(0.88)); // state values should not be changed ASSERT_EQ(0.11, j1p_s.get_value().value()); @@ -1037,8 +1037,8 @@ void generic_system_error_group_test( ASSERT_EQ(0.88, j2v_c.get_value().value()); // Error testing - (void)j1p_c.set_value(std::numeric_limits::infinity()); - (void)j1v_c.set_value(std::numeric_limits::infinity()); + ASSERT_TRUE(j1p_c.set_value(std::numeric_limits::infinity())); + ASSERT_TRUE(j1v_c.set_value(std::numeric_limits::infinity())); // read() should now bring error in the first component auto read_result = rm.read(TIME, PERIOD); ASSERT_FALSE(read_result.ok); @@ -1075,8 +1075,8 @@ void generic_system_error_group_test( } // Error should be recoverable only after reactivating the hardware component - (void)j1p_c.set_value(0.0); - (void)j1v_c.set_value(0.0); + ASSERT_TRUE(j1p_c.set_value(0.0)); + ASSERT_TRUE(j1v_c.set_value(0.0)); ASSERT_FALSE(rm.read(TIME, PERIOD).ok); // Now it should be recoverable @@ -1164,9 +1164,9 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) ASSERT_TRUE(std::isnan(vo_c.get_value().value())); // set some new values in commands - (void)j1p_c.set_value(0.11); - (void)j2p_c.set_value(0.33); - (void)vo_c.set_value(0.99); + ASSERT_TRUE(j1p_c.set_value(0.11)); + ASSERT_TRUE(j2p_c.set_value(0.33)); + ASSERT_TRUE(vo_c.set_value(0.99)); // State values should not be changed ASSERT_EQ(1.55, j1p_s.get_value().value()); @@ -1259,8 +1259,8 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) ASSERT_TRUE(std::isnan(j2p_c.get_value().value())); // set some new values in commands - (void)j1p_c.set_value(0.11); - (void)j2p_c.set_value(0.33); + ASSERT_TRUE(j1p_c.set_value(0.11)); + ASSERT_TRUE(j2p_c.set_value(0.33)); // State values should not be changed ASSERT_EQ(0.0, j1p_s.get_value().value()); @@ -1366,12 +1366,12 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands( EXPECT_TRUE(std::isnan(sty_c.get_value().value())); // set some new values in commands - (void)j1p_c.set_value(0.11); - (void)j2p_c.set_value(0.33); - (void)sfx_c.set_value(1.11); - (void)sfy_c.set_value(2.22); - (void)stx_c.set_value(3.33); - (void)sty_c.set_value(4.44); + ASSERT_TRUE(j1p_c.set_value(0.11)); + ASSERT_TRUE(j2p_c.set_value(0.33)); + ASSERT_TRUE(sfx_c.set_value(1.11)); + ASSERT_TRUE(sfy_c.set_value(2.22)); + ASSERT_TRUE(stx_c.set_value(3.33)); + ASSERT_TRUE(sty_c.set_value(4.44)); // State values should not be changed ASSERT_EQ(0.0, j1p_s.get_value().value()); @@ -1476,8 +1476,8 @@ void TestGenericSystem::test_generic_system_with_mimic_joint( ASSERT_TRUE(std::isnan(j1v_c.get_value().value())); // set some new values in commands - (void)j1p_c.set_value(0.11); - (void)j1v_c.set_value(0.05); + ASSERT_TRUE(j1p_c.set_value(0.11)); + ASSERT_TRUE(j1v_c.set_value(0.05)); // State values should not be changed ASSERT_EQ(1.57, j1p_s.get_value().value()); @@ -1587,10 +1587,10 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ASSERT_TRUE(std::isnan(j2v_c.get_value().value())); // set some new values in commands - (void)j1p_c.set_value(0.11); - (void)j1v_c.set_value(0.22); - (void)j2p_c.set_value(0.33); - (void)j2v_c.set_value(0.44); + ASSERT_TRUE(j1p_c.set_value(0.11)); + ASSERT_TRUE(j1v_c.set_value(0.22)); + ASSERT_TRUE(j2p_c.set_value(0.33)); + ASSERT_TRUE(j2v_c.set_value(0.44)); // State values should not be changed ASSERT_EQ(3.45, j1p_s.get_value().value()); @@ -1627,10 +1627,10 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ASSERT_EQ(0.44, j2v_c.get_value().value()); // set some new values in commands - (void)j1p_c.set_value(0.55); - (void)j1v_c.set_value(0.66); - (void)j2p_c.set_value(0.77); - (void)j2v_c.set_value(0.88); + ASSERT_TRUE(j1p_c.set_value(0.55)); + ASSERT_TRUE(j1v_c.set_value(0.66)); + ASSERT_TRUE(j2p_c.set_value(0.77)); + ASSERT_TRUE(j2v_c.set_value(0.88)); // state values should not be changed ASSERT_EQ(0.11, j1p_s.get_value().value()); @@ -1711,8 +1711,8 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) ASSERT_TRUE(std::isnan(gpio2_vac_c.get_value().value())); // set some new values in commands - (void)gpio1_a_o1_c.set_value(0.111); - (void)gpio2_vac_c.set_value(0.222); + ASSERT_TRUE(gpio1_a_o1_c.set_value(0.111)); + ASSERT_TRUE(gpio2_vac_c.set_value(0.222)); // State values should not be changed ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value().value())); @@ -1735,8 +1735,8 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) ASSERT_EQ(0.222, gpio2_vac_c.get_value().value()); // set some new values in commands - (void)gpio1_a_o1_c.set_value(0.333); - (void)gpio2_vac_c.set_value(0.444); + ASSERT_TRUE(gpio1_a_o1_c.set_value(0.333)); + ASSERT_TRUE(gpio2_vac_c.set_value(0.444)); // state values should not be changed ASSERT_EQ(0.111, gpio1_a_o1_s.get_value().value()); @@ -1817,10 +1817,10 @@ void TestGenericSystem::test_generic_system_with_mock_gpio_commands( EXPECT_TRUE(std::isnan(gpio2_vac_c.get_value().value())); // set some new values in commands - (void)gpio1_a_o1_c.set_value(0.11); - (void)gpio1_a_i1_c.set_value(0.33); - (void)gpio1_a_i2_c.set_value(1.11); - (void)gpio2_vac_c.set_value(2.22); + ASSERT_TRUE(gpio1_a_o1_c.set_value(0.11)); + ASSERT_TRUE(gpio1_a_i1_c.set_value(0.33)); + ASSERT_TRUE(gpio1_a_i2_c.set_value(1.11)); + ASSERT_TRUE(gpio2_vac_c.set_value(2.22)); // State values should not be changed EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value().value())); @@ -1989,8 +1989,8 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) true); // set some new values in commands - (void)j1p_c.set_value(0.11); - (void)j2a_c.set_value(3.5); + ASSERT_TRUE(j1p_c.set_value(0.11)); + ASSERT_TRUE(j2a_c.set_value(3.5)); // State values should not be changed EXPECT_EQ(3.45, j1p_s.get_value().value()); @@ -2061,8 +2061,8 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) ASSERT_EQ(rm.perform_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); // set some new values in commands - (void)j1v_c.set_value(0.5); - (void)j2v_c.set_value(2.0); + ASSERT_TRUE(j1v_c.set_value(0.5)); + ASSERT_TRUE(j2v_c.set_value(2.0)); // State values should not be changed EXPECT_EQ(0.11, j1p_s.get_value().value()); @@ -2143,7 +2143,7 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); // set some new values in commands - (void)j1p_c.set_value(0.11); + ASSERT_TRUE(j1p_c.set_value(0.11)); // State values should not be changed ASSERT_EQ(3.45, j1p_s.get_value().value()); diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index f2b6684f21..793d2a2960 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -765,7 +765,7 @@ TEST(TestComponentInterfaces, dummy_actuator) EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); double velocity_value = 1.0; - (void)command_interfaces[0]->set_value(velocity_value); // velocity + ASSERT_TRUE(command_interfaces[0]->set_value(velocity_value)); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED @@ -887,7 +887,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default) double velocity_value = 1.0; auto ci_joint1_vel = test_components::vector_contains(command_interfaces, "joint1/velocity").second; - (void)command_interfaces[ci_joint1_vel]->set_value(velocity_value); // velocity + ASSERT_TRUE(command_interfaces[ci_joint1_vel]->set_value(velocity_value)); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED @@ -1145,9 +1145,9 @@ TEST(TestComponentInterfaces, dummy_system) EXPECT_EQ("joint3", command_interfaces[2]->get_prefix_name()); double velocity_value = 1.0; - (void)command_interfaces[0]->set_value(velocity_value); // velocity - (void)command_interfaces[1]->set_value(velocity_value); // velocity - (void)command_interfaces[2]->set_value(velocity_value); // velocity + ASSERT_TRUE(command_interfaces[0]->set_value(velocity_value)); // velocity + ASSERT_TRUE(command_interfaces[1]->set_value(velocity_value)); // velocity + ASSERT_TRUE(command_interfaces[2]->set_value(velocity_value)); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED @@ -1347,9 +1347,9 @@ TEST(TestComponentInterfaces, dummy_system_default) auto ci_joint3_vel = test_components::vector_contains(command_interfaces, "joint3/velocity").second; double velocity_value = 1.0; - (void)command_interfaces[ci_joint1_vel]->set_value(velocity_value); // velocity - (void)command_interfaces[ci_joint2_vel]->set_value(velocity_value); // velocity - (void)command_interfaces[ci_joint3_vel]->set_value(velocity_value); // velocity + ASSERT_TRUE(command_interfaces[ci_joint1_vel]->set_value(velocity_value)); // velocity + ASSERT_TRUE(command_interfaces[ci_joint2_vel]->set_value(velocity_value)); // velocity + ASSERT_TRUE(command_interfaces[ci_joint3_vel]->set_value(velocity_value)); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED From 45d8e71c5bb1355305960ccf272cf2a6a7299a87 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 19 Jan 2025 18:33:30 +0100 Subject: [PATCH 9/9] Apply suggestions from code review Co-authored-by: Dr. Denis --- hardware_interface/include/hardware_interface/handle.hpp | 2 +- .../include/hardware_interface/loaned_command_interface.hpp | 2 +- .../include/hardware_interface/loaned_state_interface.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index ae29d2c6b5..7cc3b0ac8f 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -144,7 +144,7 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } [[deprecated( - "Use std::optional get_value() or T get_value(bool &status) or bool get_value(double & " + "Use std::optional get_value() or bool get_value(double & " "value) instead to retrieve the value.")]] double get_value() const { diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 18c6b06b32..b10147c9f2 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -114,7 +114,7 @@ class LoanedCommandInterface } [[deprecated( - "Use std::optional get_value() or T get_value(bool &status) or bool get_value(double & " + "Use std::optional get_value() or bool get_value(double & " "value) instead to retrieve the value.")]] double get_value() const { diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index ce9307824b..997f8e7a1e 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -89,7 +89,7 @@ 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 & " + "Use std::optional get_value() or bool get_value(double & " "value) instead to retrieve the value.")]] double get_value() const {