diff --git a/controller_interface/include/semantic_components/force_torque_sensor.hpp b/controller_interface/include/semantic_components/force_torque_sensor.hpp index d15a8f85b5..79d182397e 100644 --- a/controller_interface/include/semantic_components/force_torque_sensor.hpp +++ b/controller_interface/include/semantic_components/force_torque_sensor.hpp @@ -15,6 +15,7 @@ #ifndef SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_ #define SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_ +#include #include #include #include @@ -25,7 +26,6 @@ namespace semantic_components { -constexpr std::size_t FORCES_SIZE = 3; class ForceTorqueSensor : public SemanticComponentInterface { public: @@ -42,6 +42,7 @@ class ForceTorqueSensor : public SemanticComponentInterface::quiet_NaN()); } /// Constructor for 6D FTS with custom interface names. @@ -59,6 +60,7 @@ class ForceTorqueSensor : public SemanticComponentInterface::quiet_NaN()); auto check_and_add_interface = [this](const std::string & interface_name, const std::size_t index) { @@ -89,17 +91,9 @@ class ForceTorqueSensor : public SemanticComponentInterface get_forces() const { + update_data_from_interfaces(); std::array forces; - forces.fill(std::numeric_limits::quiet_NaN()); - std::size_t interface_counter{0}; - for (auto i = 0u; i < forces.size(); ++i) - { - if (existing_axes_[i]) - { - forces[i] = state_interfaces_[interface_counter].get().get_value().value(); - ++interface_counter; - } - } + std::copy(data_.begin(), data_.begin() + 3, forces.begin()); return forces; } @@ -111,22 +105,9 @@ class ForceTorqueSensor : public SemanticComponentInterface get_torques() const { + update_data_from_interfaces(); std::array torques; - torques.fill(std::numeric_limits::quiet_NaN()); - - // find out how many force interfaces are being used - // torque interfaces will be found from the next index onward - auto torque_interface_counter = static_cast( - std::count(existing_axes_.begin(), existing_axes_.begin() + FORCES_SIZE, true)); - - for (auto i = 0u; i < torques.size(); ++i) - { - if (existing_axes_[i + FORCES_SIZE]) - { - torques[i] = state_interfaces_[torque_interface_counter].get().get_value().value(); - ++torque_interface_counter; - } - } + std::copy(data_.begin() + 3, data_.end(), torques.begin()); return torques; } @@ -140,20 +121,43 @@ class ForceTorqueSensor : public SemanticComponentInterface data_; /// Vector with existing axes for sensors with less then 6D axes. std::array existing_axes_; }; diff --git a/controller_interface/include/semantic_components/gps_sensor.hpp b/controller_interface/include/semantic_components/gps_sensor.hpp index 052acf27a4..6d70e8be53 100644 --- a/controller_interface/include/semantic_components/gps_sensor.hpp +++ b/controller_interface/include/semantic_components/gps_sensor.hpp @@ -16,6 +16,7 @@ #define SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_ #include +#include #include #include "semantic_components/semantic_component_interface.hpp" @@ -61,7 +62,12 @@ class GPSSensor : public SemanticComponentInterface */ int8_t get_status() const { - return static_cast(state_interfaces_[0].get().template get_value().value()); + const auto data = state_interfaces_[0].get().get_optional(); + if (data.has_value()) + { + return static_cast(data.value()); + } + return std::numeric_limits::max(); } /** @@ -71,7 +77,12 @@ class GPSSensor : public SemanticComponentInterface */ uint16_t get_service() const { - return static_cast(state_interfaces_[1].get().template get_value().value()); + const auto data = state_interfaces_[1].get().get_optional(); + if (data.has_value()) + { + return static_cast(data.value()); + } + return std::numeric_limits::max(); } /** @@ -81,7 +92,12 @@ class GPSSensor : public SemanticComponentInterface */ double get_latitude() const { - return state_interfaces_[2].get().template get_value().value(); + const auto data = state_interfaces_[2].get().get_optional(); + if (data.has_value()) + { + return data.value(); + } + return std::numeric_limits::quiet_NaN(); } /** @@ -91,7 +107,12 @@ class GPSSensor : public SemanticComponentInterface */ double get_longitude() const { - return state_interfaces_[3].get().template get_value().value(); + const auto data = state_interfaces_[3].get().get_optional(); + if (data.has_value()) + { + return data.value(); + } + return std::numeric_limits::quiet_NaN(); } /** @@ -101,7 +122,12 @@ class GPSSensor : public SemanticComponentInterface */ double get_altitude() const { - return state_interfaces_[4].get().template get_value().value(); + const auto data = state_interfaces_[4].get().get_optional(); + if (data.has_value()) + { + return data.value(); + } + return std::numeric_limits::quiet_NaN(); } /** @@ -114,9 +140,21 @@ class GPSSensor : public SemanticComponentInterface typename = std::enable_if_t> const std::array & get_covariance() { - covariance_[0] = state_interfaces_[5].get().template get_value().value(); - covariance_[4] = state_interfaces_[6].get().template get_value().value(); - covariance_[8] = state_interfaces_[7].get().template get_value().value(); + const auto data_1 = state_interfaces_[5].get().get_optional(); + const auto data_2 = state_interfaces_[6].get().get_optional(); + const auto data_3 = state_interfaces_[7].get().get_optional(); + if (data_1.has_value()) + { + covariance_[0] = data_1.value(); + } + if (data_2.has_value()) + { + covariance_[4] = data_2.value(); + } + if (data_3.has_value()) + { + covariance_[8] = data_3.value(); + } return covariance_; } diff --git a/controller_interface/include/semantic_components/imu_sensor.hpp b/controller_interface/include/semantic_components/imu_sensor.hpp index 675e85c760..298c5995f0 100644 --- a/controller_interface/include/semantic_components/imu_sensor.hpp +++ b/controller_interface/include/semantic_components/imu_sensor.hpp @@ -15,6 +15,7 @@ #ifndef SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_ #define SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_ +#include #include #include #include @@ -49,11 +50,9 @@ class IMUSensor : public SemanticComponentInterface */ std::array get_orientation() const { + update_data_from_interfaces(); std::array orientation; - for (auto i = 0u; i < orientation.size(); ++i) - { - orientation[i] = state_interfaces_[i].get().get_value().value(); - } + std::copy(data_.begin(), data_.begin() + 4, orientation.begin()); return orientation; } @@ -65,13 +64,9 @@ class IMUSensor : public SemanticComponentInterface */ std::array get_angular_velocity() const { + update_data_from_interfaces(); std::array angular_velocity; - 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().value(); - } + std::copy(data_.begin() + 4, data_.begin() + 7, angular_velocity.begin()); return angular_velocity; } @@ -83,13 +78,9 @@ class IMUSensor : public SemanticComponentInterface */ std::array get_linear_acceleration() const { + update_data_from_interfaces(); std::array linear_acceleration; - 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().value(); - } + std::copy(data_.begin() + 7, data_.end(), linear_acceleration.begin()); return linear_acceleration; } @@ -100,27 +91,44 @@ class IMUSensor : public SemanticComponentInterface */ bool get_values_as_message(sensor_msgs::msg::Imu & message) const { - const auto [orientation_x, orientation_y, orientation_z, orientation_w] = get_orientation(); - const auto [angular_velocity_x, angular_velocity_y, angular_velocity_z] = - get_angular_velocity(); - const auto [linear_acceleration_x, linear_acceleration_y, linear_acceleration_z] = - get_linear_acceleration(); + update_data_from_interfaces(); + message.orientation.x = data_[0]; + message.orientation.y = data_[1]; + message.orientation.z = data_[2]; + message.orientation.w = data_[3]; - message.orientation.x = orientation_x; - message.orientation.y = orientation_y; - message.orientation.z = orientation_z; - message.orientation.w = orientation_w; + message.angular_velocity.x = data_[4]; + message.angular_velocity.y = data_[5]; + message.angular_velocity.z = data_[6]; - message.angular_velocity.x = angular_velocity_x; - message.angular_velocity.y = angular_velocity_y; - message.angular_velocity.z = angular_velocity_z; - - message.linear_acceleration.x = linear_acceleration_x; - message.linear_acceleration.y = linear_acceleration_y; - message.linear_acceleration.z = linear_acceleration_z; + message.linear_acceleration.x = data_[7]; + message.linear_acceleration.y = data_[8]; + message.linear_acceleration.z = data_[9]; return true; } + +private: + /** + * @brief Update the data array from the state interfaces. + * @note This method is thread-safe and non-blocking. + * @note This method might return stale data if the data is not updated. This is to ensure that + * the data from the sensor is not discontinuous. + */ + void update_data_from_interfaces() const + { + for (auto i = 0u; i < data_.size(); ++i) + { + const auto data = state_interfaces_[i].get().get_optional(); + if (data.has_value()) + { + data_[i] = data.value(); + } + } + } + + // Array to store the data of the IMU sensor + mutable std::array data_{{0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; }; } // namespace semantic_components diff --git a/controller_interface/include/semantic_components/pose_sensor.hpp b/controller_interface/include/semantic_components/pose_sensor.hpp index 0192010bbb..d9e9125dbc 100644 --- a/controller_interface/include/semantic_components/pose_sensor.hpp +++ b/controller_interface/include/semantic_components/pose_sensor.hpp @@ -14,6 +14,7 @@ #ifndef SEMANTIC_COMPONENTS__POSE_SENSOR_HPP_ #define SEMANTIC_COMPONENTS__POSE_SENSOR_HPP_ +#include #include #include #include @@ -47,11 +48,9 @@ class PoseSensor : public SemanticComponentInterface */ std::array get_position() const { + update_data_from_interfaces(); std::array position; - for (auto i = 0u; i < position.size(); ++i) - { - position[i] = state_interfaces_[i].get().get_value().value(); - } + std::copy(data_.begin(), data_.begin() + 3, position.begin()); return position; } @@ -63,12 +62,9 @@ class PoseSensor : public SemanticComponentInterface */ std::array get_orientation() const { + update_data_from_interfaces(); std::array orientation; - 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().value(); - } + std::copy(data_.begin() + 3, data_.end(), orientation.begin()); return orientation; } @@ -78,19 +74,40 @@ class PoseSensor : public SemanticComponentInterface */ bool get_values_as_message(geometry_msgs::msg::Pose & message) const { - const auto [position_x, position_y, position_z] = get_position(); - const auto [orientation_x, orientation_y, orientation_z, orientation_w] = get_orientation(); + update_data_from_interfaces(); - message.position.x = position_x; - message.position.y = position_y; - message.position.z = position_z; - message.orientation.x = orientation_x; - message.orientation.y = orientation_y; - message.orientation.z = orientation_z; - message.orientation.w = orientation_w; + message.position.x = data_[0]; + message.position.y = data_[1]; + message.position.z = data_[2]; + message.orientation.x = data_[3]; + message.orientation.y = data_[4]; + message.orientation.z = data_[5]; + message.orientation.w = data_[6]; return true; } + +private: + /** + * @brief Update the data array from the state interfaces. + * @note This method is thread-safe and non-blocking. + * @note This method might return stale data if the data is not updated. This is to ensure that + * the data from the sensor is not discontinuous. + */ + void update_data_from_interfaces() const + { + for (auto i = 0u; i < data_.size(); ++i) + { + const auto data = state_interfaces_[i].get().get_optional(); + if (data.has_value()) + { + data_[i] = data.value(); + } + } + } + + /// Array to store the data of the pose sensor + mutable std::array data_{{0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}}; }; } // namespace semantic_components diff --git a/controller_interface/include/semantic_components/range_sensor.hpp b/controller_interface/include/semantic_components/range_sensor.hpp index 464cef58e9..2a9d2bb1e2 100644 --- a/controller_interface/include/semantic_components/range_sensor.hpp +++ b/controller_interface/include/semantic_components/range_sensor.hpp @@ -15,6 +15,7 @@ #ifndef SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ #define SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ +#include #include #include @@ -35,7 +36,15 @@ class RangeSensor : public SemanticComponentInterface * * \return value of the range in meters */ - float get_range() const { return state_interfaces_[0].get().get_value().value(); } + float get_range() const + { + const auto data = state_interfaces_[0].get().get_optional(); + if (data.has_value()) + { + return data.value(); + } + return std::numeric_limits::quiet_NaN(); + } /// 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 63ea13418c..76a34aaddd 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().value()); + values.emplace_back(state_interfaces_[i].get().get_optional().value()); } return true; } diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 9919ec3d60..bd664f86dd 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -52,8 +52,7 @@ 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().value(), EXPORTED_STATE_INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0]->get_optional().value(), EXPORTED_STATE_INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -73,7 +72,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().value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_optional().value(), INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) @@ -121,7 +120,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().value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_optional().value(), INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -135,7 +134,7 @@ 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().value(), + exported_state_interfaces[0]->get_optional().value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); controller.configure(); 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 644bda10cd..df3ea0c813 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -102,14 +102,13 @@ controller_interface::return_type TestChainableController::update_and_write_comm for (size_t i = 0; i < command_interfaces_.size(); ++i) { (void)command_interfaces_[i].set_value( - reference_interfaces_[i] - state_interfaces_[i].get_value().value()); + reference_interfaces_[i] - state_interfaces_[i].get_optional().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().value() * CONTROLLER_DT; + state_interfaces_values_[i] = command_interfaces_[i].get_optional().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 @@ -117,7 +116,7 @@ controller_interface::return_type TestChainableController::update_and_write_comm command_interfaces_.empty(); ++i) { - state_interfaces_values_[i] = state_interfaces_[i].get_value().value(); + state_interfaces_values_[i] = state_interfaces_[i].get_optional().value(); } return controller_interface::return_type::OK; @@ -242,7 +241,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().value()); + state_intr_data.push_back(interface.get_optional().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 cadaf115ae..8ab795d168 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -187,7 +187,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().value()); + state_intr_data.push_back(interface.get_optional().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 059d3eecdf..7d8e2b5cfe 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -521,11 +521,9 @@ class TestControllerChainingWithControllerManager 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().value(), - EXP_LEFT_WHEEL_REF); + diff_drive_controller->command_interfaces_[0].get_optional().value(), EXP_LEFT_WHEEL_REF); ASSERT_EQ( - diff_drive_controller->command_interfaces_[1].get_value().value(), - EXP_RIGHT_WHEEL_REF); + diff_drive_controller->command_interfaces_[1].get_optional().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); @@ -542,15 +540,13 @@ 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().value(), - EXP_LEFT_WHEEL_CMD); + pid_left_wheel_controller->command_interfaces_[0].get_optional().value(), EXP_LEFT_WHEEL_CMD); ASSERT_EQ( - pid_left_wheel_controller->state_interfaces_[0].get_value().value(), + pid_left_wheel_controller->state_interfaces_[0].get_optional().value(), EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state ASSERT_EQ( - diff_drive_controller->state_interfaces_[0].get_value().value(), - EXP_LEFT_WHEEL_HW_STATE); + diff_drive_controller->state_interfaces_[0].get_optional().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); @@ -558,15 +554,14 @@ class TestControllerChainingWithControllerManager 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().value(), + pid_right_wheel_controller->command_interfaces_[0].get_optional().value(), EXP_RIGHT_WHEEL_CMD); ASSERT_EQ( - pid_right_wheel_controller->state_interfaces_[0].get_value().value(), + pid_right_wheel_controller->state_interfaces_[0].get_optional().value(), EXP_RIGHT_WHEEL_HW_STATE); // DiffDrive uses the same state ASSERT_EQ( - diff_drive_controller->state_interfaces_[1].get_value().value(), - EXP_RIGHT_WHEEL_HW_STATE); + diff_drive_controller->state_interfaces_[1].get_optional().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); @@ -827,15 +822,13 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) // 32 / 2 EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); ASSERT_EQ( - pid_left_wheel_controller->command_interfaces_[0].get_value().value(), - EXP_LEFT_WHEEL_CMD); + pid_left_wheel_controller->command_interfaces_[0].get_optional().value(), EXP_LEFT_WHEEL_CMD); ASSERT_EQ( - pid_left_wheel_controller->state_interfaces_[0].get_value().value(), + pid_left_wheel_controller->state_interfaces_[0].get_optional().value(), EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state ASSERT_EQ( - diff_drive_controller->state_interfaces_[0].get_value().value(), - EXP_LEFT_WHEEL_HW_STATE); + diff_drive_controller->state_interfaces_[0].get_optional().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); @@ -845,18 +838,16 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) // 128 / 2 EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); ASSERT_EQ( - pid_right_wheel_controller->command_interfaces_[0].get_value().value(), - EXP_RIGHT_WHEEL_CMD); + pid_right_wheel_controller->command_interfaces_[0].get_optional().value(), EXP_RIGHT_WHEEL_CMD); ASSERT_EQ( - pid_right_wheel_controller->state_interfaces_[0].get_value().value(), + pid_right_wheel_controller->state_interfaces_[0].get_optional().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().value(), - EXP_RIGHT_WHEEL_HW_STATE); + diff_drive_controller->state_interfaces_[1].get_optional().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); @@ -2014,30 +2005,26 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add // 32 / 2 EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); ASSERT_EQ( - pid_left_wheel_controller->command_interfaces_[0].get_value().value(), - EXP_LEFT_WHEEL_CMD); + pid_left_wheel_controller->command_interfaces_[0].get_optional().value(), EXP_LEFT_WHEEL_CMD); ASSERT_EQ( - pid_left_wheel_controller->state_interfaces_[0].get_value().value(), + pid_left_wheel_controller->state_interfaces_[0].get_optional().value(), EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state ASSERT_EQ( - diff_drive_controller->state_interfaces_[0].get_value().value(), - EXP_LEFT_WHEEL_HW_STATE); + diff_drive_controller->state_interfaces_[0].get_optional().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().value(), - EXP_RIGHT_WHEEL_CMD); + pid_right_wheel_controller->command_interfaces_[0].get_optional().value(), EXP_RIGHT_WHEEL_CMD); ASSERT_EQ( - pid_right_wheel_controller->state_interfaces_[0].get_value().value(), + pid_right_wheel_controller->state_interfaces_[0].get_optional().value(), EXP_RIGHT_WHEEL_HW_STATE); // DiffDrive uses the same state ASSERT_EQ( - diff_drive_controller->state_interfaces_[1].get_value().value(), - EXP_RIGHT_WHEEL_HW_STATE); + diff_drive_controller->state_interfaces_[1].get_optional().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/doc/migration.rst b/doc/migration.rst index 777f724cee..da2ca75ffc 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -75,7 +75,7 @@ hardware_interface ****************** * ``test_components`` was moved to its own package. Update the dependencies if you are using them. (`#1325 `_) * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. -* A new ``get_value`` that returns a ``std::optional`` was added to the ``CommandInterface`` and ``StateInterface``. This can be used to check if the value is available or not. (`#1976 `_) +* A new ``get_optional`` that returns a ``std::optional`` was added to the ``CommandInterface`` and ``StateInterface``. This can be used to check if the value is available or not. (`#1976 `_ and `#2061 `_) Adaption of Command-/StateInterfaces *************************************** @@ -90,12 +90,12 @@ Earlier code will issue compile-time warnings like: .. code:: - warning: ‘double hardware_interface::Handle::get_value() const’ is deprecated: Use std::optional get_value() or bool get_value(double & value) instead to retrieve the value. [-Wdeprecated-declarations] + warning: ‘double hardware_interface::Handle::get_value() const’ is deprecated: Use std::optional get_optional() or bool get_value(double & value) instead to retrieve the value. [-Wdeprecated-declarations] warning: ignoring return value of ‘bool hardware_interface::Handle::set_value(const T&) [with T = double]’ [-Wunused-result] The old methods are deprecated and will be removed in the future. The new methods are: - * ``std::optional get_value()`` or ``bool get_value(T & value)`` for getting the value. + * ``std::optional get_optional()`` or ``bool get_value(T & value)`` for getting the value. * ``bool set_value(const T & value)`` for setting the value. The return value ``bool`` or ``std::optional`` with ``get_value`` can be used to check if the value is available or not. Similarly, the ``set_value`` method returns a ``bool`` to check if the value was set or not. diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 9143dc6ac1..a5b87cd7f5 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -164,7 +164,7 @@ hardware_interface * With (`#1763 `_) parsing for SDF published to ``robot_description`` topic is now also supported. * With (`#1567 `_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components. * The hardware components can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 `_) -* Added new ``get_value`` method that returns ``std::optional`` of the templated type, and this can be used to check if the value is available or not (`#1976 `_) +* Added new ``get_optional`` method that returns ``std::optional`` of the templated type, and this can be used to check if the value is available or not (`#1976 `_ and `#2061 `_) joint_limits ************ diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 4327191810..9f3340e41b 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -113,7 +113,7 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } [[deprecated( - "Use std::optional get_value() or bool get_value(double & " + "Use std::optional get_optional() or bool get_value(double & " "value) instead to retrieve the value.")]] double get_value() const { @@ -140,7 +140,7 @@ class Handle * successful, the value is returned. */ template - [[nodiscard]] std::optional get_value() const + [[nodiscard]] std::optional get_optional() const { std::shared_lock lock(handle_mutex_, std::try_to_lock); if (!lock.owns_lock()) diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 3e4c0ca441..7b7a3ad8b2 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -129,7 +129,7 @@ class LoanedCommandInterface } [[deprecated( - "Use std::optional get_value() or bool get_value(double & " + "Use std::optional get_optional() or bool get_value(double & " "value) instead to retrieve the value.")]] double get_value() const { @@ -159,13 +159,13 @@ class LoanedCommandInterface * returns the value immediately. */ template - [[nodiscard]] std::optional get_value(unsigned int max_tries = 10) const + [[nodiscard]] std::optional get_optional(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(); + const std::optional data = command_interface_.get_optional(); if (data.has_value()) { return data; diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index cb7b99066a..23b1d801be 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 bool get_value(double & " + "Use std::optional get_optional() or bool get_value(double & " "value) instead to retrieve the value.")]] double get_value() const { @@ -119,13 +119,13 @@ class LoanedStateInterface * returns the value immediately. */ template - [[nodiscard]] std::optional get_value(unsigned int max_tries = 10) const + [[nodiscard]] std::optional get_optional(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(); + const std::optional data = state_interface_.get_optional(); if (data.has_value()) { return data; diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 815647b3f5..6551dcc9e9 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().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())); + ASSERT_EQ(1.57, j1p_s.get_optional().value()); + ASSERT_EQ(0.7854, j2p_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_optional().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().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())); + ASSERT_EQ(1.57, j1v_s.get_optional().value()); + ASSERT_EQ(0.7854, j2p_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2a_c.get_optional().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().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())); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.11)); @@ -883,36 +883,36 @@ void generic_system_functional_test( ASSERT_TRUE(j2v_c.set_value(0.44)); // State values should not be changed - 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()); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.22, j1v_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(0.44, j2v_c.get_optional().value()); // write() does not change values ASSERT_TRUE(rm.write(TIME, PERIOD).ok); - 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()); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.22, j1v_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(0.44, j2v_c.get_optional().value()); // read() mirrors commands + offset to states ASSERT_TRUE(rm.read(TIME, PERIOD).ok); - 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()); + ASSERT_EQ(0.11 + offset, j1p_s.get_optional().value()); + ASSERT_EQ(0.22, j1v_s.get_optional().value()); + ASSERT_EQ(0.33 + offset, j2p_s.get_optional().value()); + ASSERT_EQ(0.44, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.22, j1v_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(0.44, j2v_c.get_optional().value()); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.55)); @@ -921,14 +921,14 @@ void generic_system_functional_test( ASSERT_TRUE(j2v_c.set_value(0.88)); // state values should not be changed - 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()); + ASSERT_EQ(0.11 + offset, j1p_s.get_optional().value()); + ASSERT_EQ(0.22, j1v_s.get_optional().value()); + ASSERT_EQ(0.33 + offset, j2p_s.get_optional().value()); + ASSERT_EQ(0.44, j2v_s.get_optional().value()); + ASSERT_EQ(0.55, j1p_c.get_optional().value()); + ASSERT_EQ(0.66, j1v_c.get_optional().value()); + ASSERT_EQ(0.77, j2p_c.get_optional().value()); + ASSERT_EQ(0.88, j2v_c.get_optional().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().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())); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.11)); @@ -989,36 +989,36 @@ void generic_system_error_group_test( ASSERT_TRUE(j2v_c.set_value(0.44)); // State values should not be changed - 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()); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.22, j1v_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(0.44, j2v_c.get_optional().value()); // write() does not change values ASSERT_TRUE(rm.write(TIME, PERIOD).ok); - 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()); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.22, j1v_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(0.44, j2v_c.get_optional().value()); // read() mirrors commands to states ASSERT_TRUE(rm.read(TIME, PERIOD).ok); - 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()); + ASSERT_EQ(0.11, j1p_s.get_optional().value()); + ASSERT_EQ(0.22, j1v_s.get_optional().value()); + ASSERT_EQ(0.33, j2p_s.get_optional().value()); + ASSERT_EQ(0.44, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.22, j1v_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(0.44, j2v_c.get_optional().value()); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.55)); @@ -1027,14 +1027,14 @@ void generic_system_error_group_test( ASSERT_TRUE(j2v_c.set_value(0.88)); // state values should not be changed - 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()); + ASSERT_EQ(0.11, j1p_s.get_optional().value()); + ASSERT_EQ(0.22, j1v_s.get_optional().value()); + ASSERT_EQ(0.33, j2p_s.get_optional().value()); + ASSERT_EQ(0.44, j2v_s.get_optional().value()); + ASSERT_EQ(0.55, j1p_c.get_optional().value()); + ASSERT_EQ(0.66, j1v_c.get_optional().value()); + ASSERT_EQ(0.77, j2p_c.get_optional().value()); + ASSERT_EQ(0.88, j2v_c.get_optional().value()); // Error testing ASSERT_TRUE(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().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())); + ASSERT_EQ(1.55, j1p_s.get_optional().value()); + ASSERT_EQ(0.1, j1v_s.get_optional().value()); + ASSERT_EQ(0.65, j2p_s.get_optional().value()); + ASSERT_EQ(0.2, j2v_s.get_optional().value()); + ASSERT_EQ(0.5, vo_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(vo_c.get_optional().value())); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.11)); @@ -1169,36 +1169,36 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) ASSERT_TRUE(vo_c.set_value(0.99)); // State values should not be changed - 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()); + ASSERT_EQ(1.55, j1p_s.get_optional().value()); + ASSERT_EQ(0.1, j1v_s.get_optional().value()); + ASSERT_EQ(0.65, j2p_s.get_optional().value()); + ASSERT_EQ(0.2, j2v_s.get_optional().value()); + ASSERT_EQ(0.5, vo_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(0.99, vo_c.get_optional().value()); // write() does not change values rm.write(TIME, PERIOD); - 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()); + ASSERT_EQ(1.55, j1p_s.get_optional().value()); + ASSERT_EQ(0.1, j1v_s.get_optional().value()); + ASSERT_EQ(0.65, j2p_s.get_optional().value()); + ASSERT_EQ(0.2, j2v_s.get_optional().value()); + ASSERT_EQ(0.5, vo_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(0.99, vo_c.get_optional().value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - 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()); + ASSERT_EQ(0.11, j1p_s.get_optional().value()); + ASSERT_EQ(0.1, j1v_s.get_optional().value()); + ASSERT_EQ(0.33, j2p_s.get_optional().value()); + ASSERT_EQ(0.99, vo_s.get_optional().value()); + ASSERT_EQ(0.2, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(0.99, vo_c.get_optional().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().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())); + ASSERT_EQ(0.0, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.0, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); + EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sty_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_optional().value())); // set some new values in commands 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()); - 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(0.0, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.0, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); + EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sty_s.get_optional().value())); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); // write() does not change values rm.write(TIME, PERIOD); - 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(0.0, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.0, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); + EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sty_s.get_optional().value())); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - 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()); + ASSERT_EQ(0.11, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.33, j2p_s.get_optional().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); + EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sty_s.get_optional().value())); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().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().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())); + ASSERT_EQ(0.0, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.0, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); + EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sty_s.get_optional().value())); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_optional().value())); + EXPECT_TRUE(std::isnan(sfx_c.get_optional().value())); + EXPECT_TRUE(std::isnan(sfy_c.get_optional().value())); + EXPECT_TRUE(std::isnan(stx_c.get_optional().value())); + EXPECT_TRUE(std::isnan(sty_c.get_optional().value())); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.11)); @@ -1374,54 +1374,54 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands( ASSERT_TRUE(sty_c.set_value(4.44)); // State values should not be changed - 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()); + ASSERT_EQ(0.0, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.0, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); + EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sty_s.get_optional().value())); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(1.11, sfx_c.get_optional().value()); + ASSERT_EQ(2.22, sfy_c.get_optional().value()); + ASSERT_EQ(3.33, stx_c.get_optional().value()); + ASSERT_EQ(4.44, sty_c.get_optional().value()); // write() does not change values rm.write(TIME, PERIOD); - 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()); + ASSERT_EQ(0.0, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.0, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_TRUE(std::isnan(sfx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sfy_s.get_optional().value())); + EXPECT_TRUE(std::isnan(stx_s.get_optional().value())); + EXPECT_TRUE(std::isnan(sty_s.get_optional().value())); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(1.11, sfx_c.get_optional().value()); + ASSERT_EQ(2.22, sfy_c.get_optional().value()); + ASSERT_EQ(3.33, stx_c.get_optional().value()); + ASSERT_EQ(4.44, sty_c.get_optional().value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - 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()); + ASSERT_EQ(0.11, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.33, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(1.11, sfx_s.get_optional().value()); + ASSERT_EQ(2.22, sfy_s.get_optional().value()); + ASSERT_EQ(3.33, stx_s.get_optional().value()); + ASSERT_EQ(4.44, sty_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(1.11, sfx_c.get_optional().value()); + ASSERT_EQ(2.22, sfy_c.get_optional().value()); + ASSERT_EQ(3.33, stx_c.get_optional().value()); + ASSERT_EQ(4.44, sty_c.get_optional().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().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())); + ASSERT_EQ(1.57, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.0, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); // set some new values in commands 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()); - 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()); + ASSERT_EQ(1.57, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.0, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.05, j1v_c.get_optional().value()); // write() does not change values rm.write(TIME, PERIOD); - 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()); + ASSERT_EQ(1.57, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.0, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.05, j1v_c.get_optional().value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - 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()); + ASSERT_EQ(0.11, j1p_s.get_optional().value()); + ASSERT_EQ(0.05, j1v_s.get_optional().value()); + ASSERT_EQ(-0.22, j2p_s.get_optional().value()); + ASSERT_EQ(-0.1, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.05, j1v_c.get_optional().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().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())); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.11)); @@ -1593,38 +1593,38 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ASSERT_TRUE(j2v_c.set_value(0.44)); // State values should not be changed - 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()); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.22, j1v_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(0.44, j2v_c.get_optional().value()); // write() does not change values rm.write(TIME, PERIOD); - 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()); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(2.78, j2p_s.get_optional().value()); + ASSERT_EQ(0.0, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.22, j1v_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(0.44, j2v_c.get_optional().value()); // read() mirrors commands + offset to states rm.read(TIME, PERIOD); - 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()); + ASSERT_EQ(0.11, j1p_s.get_optional().value()); + ASSERT_EQ(0.11 + offset, c_j1p_s.get_optional().value()); + ASSERT_EQ(0.22, j1v_s.get_optional().value()); + ASSERT_EQ(0.33, j2p_s.get_optional().value()); + ASSERT_EQ(0.33 + offset, c_j2p_s.get_optional().value()); + ASSERT_EQ(0.44, j2v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.22, j1v_c.get_optional().value()); + ASSERT_EQ(0.33, j2p_c.get_optional().value()); + ASSERT_EQ(0.44, j2v_c.get_optional().value()); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.55)); @@ -1633,16 +1633,16 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ASSERT_TRUE(j2v_c.set_value(0.88)); // state values should not be changed - 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()); + ASSERT_EQ(0.11, j1p_s.get_optional().value()); + ASSERT_EQ(0.11 + offset, c_j1p_s.get_optional().value()); + ASSERT_EQ(0.22, j1v_s.get_optional().value()); + ASSERT_EQ(0.33, j2p_s.get_optional().value()); + ASSERT_EQ(0.33 + offset, c_j2p_s.get_optional().value()); + ASSERT_EQ(0.44, j2v_s.get_optional().value()); + ASSERT_EQ(0.55, j1p_c.get_optional().value()); + ASSERT_EQ(0.66, j1v_c.get_optional().value()); + ASSERT_EQ(0.77, j2p_c.get_optional().value()); + ASSERT_EQ(0.88, j2v_c.get_optional().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().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())); + ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); + ASSERT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + ASSERT_TRUE(std::isnan(gpio1_a_o1_c.get_optional().value())); + ASSERT_TRUE(std::isnan(gpio2_vac_c.get_optional().value())); // set some new values in commands 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())); - 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()); + ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); + ASSERT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + ASSERT_EQ(0.111, gpio1_a_o1_c.get_optional().value()); + ASSERT_EQ(0.222, gpio2_vac_c.get_optional().value()); // write() does not change values rm.write(TIME, PERIOD); - 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()); + ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); + ASSERT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + ASSERT_EQ(0.111, gpio1_a_o1_c.get_optional().value()); + ASSERT_EQ(0.222, gpio2_vac_c.get_optional().value()); // read() mirrors commands + offset to states rm.read(TIME, PERIOD); - 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()); + ASSERT_EQ(0.111, gpio1_a_o1_s.get_optional().value()); + ASSERT_EQ(0.222, gpio2_vac_s.get_optional().value()); + ASSERT_EQ(0.111, gpio1_a_o1_c.get_optional().value()); + ASSERT_EQ(0.222, gpio2_vac_c.get_optional().value()); // set some new values in commands 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()); - 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()); + ASSERT_EQ(0.111, gpio1_a_o1_s.get_optional().value()); + ASSERT_EQ(0.222, gpio2_vac_s.get_optional().value()); + ASSERT_EQ(0.333, gpio1_a_o1_c.get_optional().value()); + ASSERT_EQ(0.444, gpio2_vac_c.get_optional().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().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())); + EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio1_a_o1_c.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio1_a_i1_c.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio1_a_i2_c.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio2_vac_c.get_optional().value())); // set some new values in commands ASSERT_TRUE(gpio1_a_o1_c.set_value(0.11)); @@ -1823,36 +1823,36 @@ void TestGenericSystem::test_generic_system_with_mock_gpio_commands( 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())); - 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()); + EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + ASSERT_EQ(0.11, gpio1_a_o1_c.get_optional().value()); + ASSERT_EQ(0.33, gpio1_a_i1_c.get_optional().value()); + ASSERT_EQ(1.11, gpio1_a_i2_c.get_optional().value()); + ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); // write() does not change values rm.write(TIME, PERIOD); - 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()); + EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + ASSERT_EQ(0.11, gpio1_a_o1_c.get_optional().value()); + ASSERT_EQ(0.33, gpio1_a_i1_c.get_optional().value()); + ASSERT_EQ(1.11, gpio1_a_i2_c.get_optional().value()); + ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - 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()); + ASSERT_EQ(0.11, gpio1_a_o1_s.get_optional().value()); + ASSERT_EQ(0.33, gpio1_a_i1_s.get_optional().value()); + ASSERT_EQ(1.11, gpio1_a_o2_s.get_optional().value()); + ASSERT_EQ(2.22, gpio2_vac_s.get_optional().value()); + ASSERT_EQ(0.11, gpio1_a_o1_c.get_optional().value()); + ASSERT_EQ(0.33, gpio1_a_i1_c.get_optional().value()); + ASSERT_EQ(1.11, gpio1_a_i2_c.get_optional().value()); + ASSERT_EQ(2.22, gpio2_vac_c.get_optional().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().value()); - ASSERT_EQ(0.0, force_y_s.get_value().value()); - ASSERT_EQ(0.0, force_z_s.get_value().value()); + ASSERT_EQ(0.0, force_x_s.get_optional().value()); + ASSERT_EQ(0.0, force_y_s.get_optional().value()); + ASSERT_EQ(0.0, force_z_s.get_optional().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().value()); + ASSERT_EQ(1, state.get_optional().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().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())); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(0.0, j1a_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_EQ(0.0, j2a_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2a_c.get_optional().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) ASSERT_TRUE(j2a_c.set_value(3.5)); // State values should not be changed - 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()); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(0.0, j1a_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_EQ(0.0, j2a_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); + ASSERT_EQ(3.5, j2a_c.get_optional().value()); // write() does not change values rm.write(TIME, PERIOD); - 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()); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(0.0, j1a_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_EQ(0.0, j2a_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); + ASSERT_EQ(3.5, j2a_c.get_optional().value()); // read() mirrors commands to states and calculate dynamics rm.read(TIME, PERIOD); - 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()); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(-33.4, j1v_s.get_optional().value()); + EXPECT_NEAR(-334.0, j1a_s.get_optional().value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_EQ(3.5, j2a_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); + ASSERT_EQ(3.5, j2a_c.get_optional().value()); // read() mirrors commands to states and calculate dynamics rm.read(TIME, PERIOD); - 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()); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_NEAR(334.0, j1a_s.get_optional().value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_NEAR(0.35, j2v_s.get_optional().value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); + ASSERT_EQ(3.5, j2a_c.get_optional().value()); // read() mirrors commands to states and calculate dynamics rm.read(TIME, PERIOD); - 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()); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(0.0, j1a_s.get_optional().value()); + EXPECT_EQ(2.815, j2p_s.get_optional().value()); + EXPECT_NEAR(0.7, j2v_s.get_optional().value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); + ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); + ASSERT_EQ(3.5, j2a_c.get_optional().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) ASSERT_TRUE(j2v_c.set_value(2.0)); // State values should not be changed - 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()); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(0.0, j1a_s.get_optional().value()); + EXPECT_EQ(2.815, j2p_s.get_optional().value()); + EXPECT_NEAR(0.7, j2v_s.get_optional().value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.5, j1v_c.get_optional().value()); + ASSERT_EQ(2.0, j2v_c.get_optional().value()); + ASSERT_EQ(3.5, j2a_c.get_optional().value()); // write() does not change values rm.write(TIME, PERIOD); - 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()); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(0.0, j1a_s.get_optional().value()); + EXPECT_EQ(2.815, j2p_s.get_optional().value()); + EXPECT_NEAR(0.7, j2v_s.get_optional().value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.5, j1v_c.get_optional().value()); + ASSERT_EQ(2.0, j2v_c.get_optional().value()); + ASSERT_EQ(3.5, j2a_c.get_optional().value()); // read() mirrors commands to states and calculate dynamics (both velocity mode) rm.read(TIME, PERIOD); - 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()); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(0.5, j1v_s.get_optional().value()); + EXPECT_EQ(5.0, j1a_s.get_optional().value()); + EXPECT_EQ(2.885, j2p_s.get_optional().value()); + EXPECT_EQ(2.0, j2v_s.get_optional().value()); + EXPECT_NEAR(13.0, j2a_s.get_optional().value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.5, j1v_c.get_optional().value()); + ASSERT_EQ(2.0, j2v_c.get_optional().value()); + ASSERT_EQ(3.5, j2a_c.get_optional().value()); // read() mirrors commands to states and calculate dynamics (both velocity mode) rm.read(TIME, PERIOD); - 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()); + EXPECT_EQ(0.16, j1p_s.get_optional().value()); + EXPECT_EQ(0.5, j1v_s.get_optional().value()); + EXPECT_EQ(0.0, j1a_s.get_optional().value()); + EXPECT_EQ(3.085, j2p_s.get_optional().value()); + EXPECT_EQ(2.0, j2v_s.get_optional().value()); + EXPECT_EQ(0.0, j2a_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); + ASSERT_EQ(0.5, j1v_c.get_optional().value()); + ASSERT_EQ(2.0, j2v_c.get_optional().value()); + ASSERT_EQ(3.5, j2a_c.get_optional().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().value()); - ASSERT_EQ(0.0, j1v_s.get_value().value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value().value())); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.11)); // State values should not be changed - 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()); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); // write() does not change values rm.write(TIME, PERIOD); - 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()); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().value()); // read() also does not change values rm.read(TIME, PERIOD); - 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()); + ASSERT_EQ(3.45, j1p_s.get_optional().value()); + ASSERT_EQ(0.0, j1v_s.get_optional().value()); + ASSERT_EQ(0.11, j1p_c.get_optional().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 36dbfffe02..31d0cc7f64 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -757,8 +757,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().value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value().value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_optional().value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_optional().value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -773,9 +773,8 @@ 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().value()); // position value - EXPECT_EQ( - step ? velocity_value : 0, state_interfaces[1]->get_value().value()); // velocity + step * velocity_value, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -791,8 +790,8 @@ TEST(TestComponentInterfaces, dummy_actuator) 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 + state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -806,9 +805,8 @@ 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().value()); // position value - EXPECT_EQ(0, state_interfaces[1]->get_value().value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(0, state_interfaces[1]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -882,9 +880,8 @@ 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().value())); // position value - ASSERT_TRUE( - std::isnan(state_interfaces[si_joint1_vel]->get_value().value())); // velocity + std::isnan(state_interfaces[si_joint1_pos]->get_optional().value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_optional().value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -900,10 +897,10 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ( step * velocity_value, - state_interfaces[si_joint1_pos]->get_value().value()); // position value + state_interfaces[si_joint1_pos]->get_optional().value()); // position value EXPECT_EQ( step ? velocity_value : 0, - state_interfaces[si_joint1_vel]->get_value().value()); // velocity + state_interfaces[si_joint1_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -919,9 +916,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint1_pos]->get_value().value()); // position value - EXPECT_EQ( - velocity_value, state_interfaces[si_joint1_vel]->get_value().value()); // velocity + state_interfaces[si_joint1_pos]->get_optional().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -937,8 +933,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) 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 + state_interfaces[si_joint1_pos]->get_optional().value()); // position value + EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -966,21 +962,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().value())); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_optional().value())); // Not updated because is is UNCONFIGURED sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value().value())); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_optional().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().value()); + EXPECT_EQ(0.0, state_interfaces[0]->get_optional().value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[0]->get_value().value()); + EXPECT_EQ(0x666, state_interfaces[0]->get_optional().value()); } // END @@ -1009,23 +1005,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().value())); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_optional().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().value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_optional().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().value()); + EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_optional().value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value().value()); + EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_optional().value()); } TEST(TestComponentInterfaces, dummy_sensor_default_joint) @@ -1055,7 +1051,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().value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_optional().value())); auto [contains_joint1_pos, si_joint1_pos] = test_components::vector_contains(state_interfaces, "joint1/position"); @@ -1063,24 +1059,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().value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_optional().value())); // Not updated because is is UNCONFIGURED sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value().value())); - EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value().value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_optional().value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_optional().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().value()); - EXPECT_EQ(10.0, state_interfaces[si_joint1_pos]->get_value().value()); + EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_optional().value()); + EXPECT_EQ(10.0, state_interfaces[si_joint1_pos]->get_optional().value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value().value()); - EXPECT_EQ(0x777, state_interfaces[si_joint1_pos]->get_value().value()); + EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_optional().value()); + EXPECT_EQ(0x777, state_interfaces[si_joint1_pos]->get_optional().value()); } // BEGIN (Handle export change): for backward compatibility @@ -1139,12 +1135,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().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_TRUE(std::isnan(state_interfaces[0]->get_optional().value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_optional().value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[2]->get_optional().value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[3]->get_optional().value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[4]->get_optional().value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[5]->get_optional().value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1159,17 +1155,14 @@ 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().value()); // position value + step * velocity_value, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_optional().value()); // velocity EXPECT_EQ( - step ? velocity_value : 0, state_interfaces[1]->get_value().value()); // velocity + step * velocity_value, state_interfaces[2]->get_optional().value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_optional().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 + step * velocity_value, state_interfaces[4]->get_optional().value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1185,16 +1178,16 @@ TEST(TestComponentInterfaces, dummy_system) 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 + state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_optional().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 + state_interfaces[2]->get_optional().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[3]->get_optional().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 + state_interfaces[4]->get_optional().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[5]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1208,15 +1201,12 @@ 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().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 + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(0.0, state_interfaces[1]->get_optional().value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_optional().value()); // position value + EXPECT_EQ(0.0, state_interfaces[3]->get_optional().value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_optional().value()); // position value + EXPECT_EQ(0.0, state_interfaces[5]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1348,17 +1338,14 @@ 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().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 + std::isnan(state_interfaces[si_joint1_pos]->get_optional().value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_optional().value())); // velocity ASSERT_TRUE( - std::isnan(state_interfaces[si_joint2_vel]->get_value().value())); // velocity + std::isnan(state_interfaces[si_joint2_pos]->get_optional().value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_vel]->get_optional().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 + std::isnan(state_interfaces[si_joint3_pos]->get_optional().value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_vel]->get_optional().value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1374,22 +1361,22 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ( step * velocity_value, - state_interfaces[si_joint1_pos]->get_value().value()); // position value + state_interfaces[si_joint1_pos]->get_optional().value()); // position value EXPECT_EQ( step ? velocity_value : 0, - state_interfaces[si_joint1_vel]->get_value().value()); // velocity + state_interfaces[si_joint1_vel]->get_optional().value()); // velocity EXPECT_EQ( step * velocity_value, - state_interfaces[si_joint2_pos]->get_value().value()); // position value + state_interfaces[si_joint2_pos]->get_optional().value()); // position value EXPECT_EQ( step ? velocity_value : 0, - state_interfaces[si_joint2_vel]->get_value().value()); // velocity + state_interfaces[si_joint2_vel]->get_optional().value()); // velocity EXPECT_EQ( step * velocity_value, - state_interfaces[si_joint3_pos]->get_value().value()); // position value + state_interfaces[si_joint3_pos]->get_optional().value()); // position value EXPECT_EQ( step ? velocity_value : 0, - state_interfaces[si_joint3_vel]->get_value().value()); // velocity + state_interfaces[si_joint3_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1405,19 +1392,16 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint1_pos]->get_value().value()); // position value - EXPECT_EQ( - velocity_value, state_interfaces[si_joint1_vel]->get_value().value()); // velocity + state_interfaces[si_joint1_pos]->get_optional().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint2_pos]->get_value().value()); // position value - EXPECT_EQ( - velocity_value, state_interfaces[si_joint2_vel]->get_value().value()); // velocity + state_interfaces[si_joint2_pos]->get_optional().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint2_vel]->get_optional().value()); // velocity EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint3_pos]->get_value().value()); // position value - EXPECT_EQ( - velocity_value, state_interfaces[si_joint3_vel]->get_value().value()); // velocity + state_interfaces[si_joint3_pos]->get_optional().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint3_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1433,16 +1417,16 @@ TEST(TestComponentInterfaces, dummy_system_default) 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 + state_interfaces[si_joint1_pos]->get_optional().value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_optional().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 + state_interfaces[si_joint2_pos]->get_optional().value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_optional().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 + state_interfaces[si_joint3_pos]->get_optional().value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1519,8 +1503,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().value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value().value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_optional().value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_optional().value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1592,8 +1576,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().value(), 0.0); - EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value().value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_optional().value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_optional().value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1654,8 +1638,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().value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value().value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_optional().value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_optional().value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1726,8 +1710,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().value(), 0.0); - EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value().value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_optional().value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_optional().value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1791,7 +1775,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().value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_optional().value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1859,7 +1843,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().value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_vol]->get_optional().value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1919,11 +1903,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().value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_optional().value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value().value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_optional().value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1992,11 +1976,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().value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_optional().value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value().value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_optional().value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -2059,11 +2043,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().value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_optional().value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value().value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_optional().value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -2132,11 +2116,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().value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_optional().value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value().value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_optional().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 99428a4496..fc1b88916b 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -244,7 +244,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().value())); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_optional().value())); } { auto [contains, position] = diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 1a35fd836a..8f7214a50a 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -31,21 +31,21 @@ TEST(TestHandle, command_interface) { double value = 1.337; CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &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(), value); + EXPECT_DOUBLE_EQ(interface.get_optional().value(), value); + ASSERT_TRUE(interface.get_optional().has_value()); + EXPECT_DOUBLE_EQ(interface.get_optional().value(), value); + EXPECT_DOUBLE_EQ(interface.get_optional().value(), value); EXPECT_NO_THROW({ interface.set_value(0.0); }); - ASSERT_TRUE(interface.get_value().has_value()); - EXPECT_DOUBLE_EQ(interface.get_value().value(), 0.0); + ASSERT_TRUE(interface.get_optional().has_value()); + EXPECT_DOUBLE_EQ(interface.get_optional().value(), 0.0); } TEST(TestHandle, state_interface) { double value = 1.337; StateInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - ASSERT_TRUE(interface.get_value().has_value()); - EXPECT_DOUBLE_EQ(interface.get_value().value(), value); + ASSERT_TRUE(interface.get_optional().has_value()); + EXPECT_DOUBLE_EQ(interface.get_optional().value(), value); // interface.set_value(5); compiler error, no set_value function } @@ -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().value()); + EXPECT_ANY_THROW(handle.get_optional().value()); EXPECT_ANY_THROW(bool status = handle.set_value(0.0)); } @@ -68,11 +68,11 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) { double value = 1.337; CommandInterface handle{JOINT_NAME, FOO_INTERFACE, &value}; - ASSERT_TRUE(handle.get_value().has_value()); - EXPECT_DOUBLE_EQ(handle.get_value().value(), value); + ASSERT_TRUE(handle.get_optional().has_value()); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); EXPECT_NO_THROW({ handle.set_value(0.0); }); - ASSERT_TRUE(handle.get_value().has_value()); - EXPECT_DOUBLE_EQ(handle.get_value().value(), 0.0); + ASSERT_TRUE(handle.get_optional().has_value()); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), 0.0); } TEST(TestHandle, interface_description_state_interface_name_getters_work) @@ -109,11 +109,11 @@ TEST(TestHandle, copy_constructor) double value = 1.337; hardware_interface::Handle handle{JOINT_NAME, FOO_INTERFACE, &value}; hardware_interface::Handle copy(handle); - EXPECT_DOUBLE_EQ(copy.get_value().value(), value); - EXPECT_DOUBLE_EQ(handle.get_value().value(), value); + EXPECT_DOUBLE_EQ(copy.get_optional().value(), value); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); EXPECT_NO_THROW({ copy.set_value(0.0); }); - EXPECT_DOUBLE_EQ(copy.get_value().value(), 0.0); - EXPECT_DOUBLE_EQ(handle.get_value().value(), 0.0); + EXPECT_DOUBLE_EQ(copy.get_optional().value(), 0.0); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), 0.0); } { double value = 1.337; @@ -122,20 +122,20 @@ TEST(TestHandle, copy_constructor) info.data_type = "double"; InterfaceDescription itf_descr{JOINT_NAME, info}; hardware_interface::Handle handle{itf_descr}; - EXPECT_TRUE(std::isnan(handle.get_value().value())); + EXPECT_TRUE(std::isnan(handle.get_optional().value())); ASSERT_TRUE(handle.set_value(value)); hardware_interface::Handle copy(handle); EXPECT_EQ(copy.get_name(), handle.get_name()); EXPECT_EQ(copy.get_interface_name(), handle.get_interface_name()); EXPECT_EQ(copy.get_prefix_name(), handle.get_prefix_name()); - EXPECT_DOUBLE_EQ(copy.get_value().value(), value); - EXPECT_DOUBLE_EQ(handle.get_value().value(), value); + EXPECT_DOUBLE_EQ(copy.get_optional().value(), value); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); EXPECT_NO_THROW({ copy.set_value(0.0); }); - EXPECT_DOUBLE_EQ(copy.get_value().value(), 0.0); - EXPECT_DOUBLE_EQ(handle.get_value().value(), value); + EXPECT_DOUBLE_EQ(copy.get_optional().value(), 0.0); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); EXPECT_NO_THROW({ copy.set_value(0.52); }); - EXPECT_DOUBLE_EQ(copy.get_value().value(), 0.52); - EXPECT_DOUBLE_EQ(handle.get_value().value(), value); + EXPECT_DOUBLE_EQ(copy.get_optional().value(), 0.52); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); } } @@ -144,9 +144,9 @@ TEST(TesHandle, move_constructor) double value = 1.337; hardware_interface::Handle handle{JOINT_NAME, FOO_INTERFACE, &value}; hardware_interface::Handle moved{std::move(handle)}; - EXPECT_DOUBLE_EQ(moved.get_value().value(), value); + EXPECT_DOUBLE_EQ(moved.get_optional().value(), value); EXPECT_NO_THROW({ moved.set_value(0.0); }); - EXPECT_DOUBLE_EQ(moved.get_value().value(), 0.0); + EXPECT_DOUBLE_EQ(moved.get_optional().value(), 0.0); } TEST(TestHandle, copy_assignment) @@ -156,14 +156,14 @@ TEST(TestHandle, copy_assignment) double value_2 = 2.337; hardware_interface::Handle handle{JOINT_NAME, FOO_INTERFACE, &value_1}; hardware_interface::Handle copy{JOINT_NAME, "random", &value_2}; - EXPECT_DOUBLE_EQ(copy.get_value().value(), value_2); - EXPECT_DOUBLE_EQ(handle.get_value().value(), value_1); + EXPECT_DOUBLE_EQ(copy.get_optional().value(), value_2); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), value_1); copy = handle; - EXPECT_DOUBLE_EQ(copy.get_value().value(), value_1); - EXPECT_DOUBLE_EQ(handle.get_value().value(), value_1); + EXPECT_DOUBLE_EQ(copy.get_optional().value(), value_1); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), value_1); EXPECT_NO_THROW({ copy.set_value(0.0); }); - EXPECT_DOUBLE_EQ(copy.get_value().value(), 0.0); - EXPECT_DOUBLE_EQ(handle.get_value().value(), 0.0); + EXPECT_DOUBLE_EQ(copy.get_optional().value(), 0.0); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), 0.0); EXPECT_DOUBLE_EQ(value_1, 0.0); EXPECT_DOUBLE_EQ(value_2, 2.337); } @@ -175,20 +175,20 @@ TEST(TestHandle, copy_assignment) info.data_type = "double"; InterfaceDescription itf_descr{JOINT_NAME, info}; hardware_interface::Handle handle{itf_descr}; - EXPECT_TRUE(std::isnan(handle.get_value().value())); + EXPECT_TRUE(std::isnan(handle.get_optional().value())); ASSERT_TRUE(handle.set_value(value)); hardware_interface::Handle copy = handle; EXPECT_EQ(copy.get_name(), handle.get_name()); EXPECT_EQ(copy.get_interface_name(), handle.get_interface_name()); EXPECT_EQ(copy.get_prefix_name(), handle.get_prefix_name()); - EXPECT_DOUBLE_EQ(copy.get_value().value(), value); - EXPECT_DOUBLE_EQ(handle.get_value().value(), value); + EXPECT_DOUBLE_EQ(copy.get_optional().value(), value); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); EXPECT_NO_THROW({ copy.set_value(0.0); }); - EXPECT_DOUBLE_EQ(copy.get_value().value(), 0.0); - EXPECT_DOUBLE_EQ(handle.get_value().value(), value); + EXPECT_DOUBLE_EQ(copy.get_optional().value(), 0.0); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); EXPECT_NO_THROW({ copy.set_value(0.52); }); - EXPECT_DOUBLE_EQ(copy.get_value().value(), 0.52); - EXPECT_DOUBLE_EQ(handle.get_value().value(), value); + EXPECT_DOUBLE_EQ(copy.get_optional().value(), 0.52); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); } } @@ -198,10 +198,10 @@ TEST(TestHandle, move_assignment) double value_2 = 2.337; hardware_interface::Handle handle{JOINT_NAME, FOO_INTERFACE, &value}; hardware_interface::Handle moved{JOINT_NAME, "random", &value_2}; - EXPECT_DOUBLE_EQ(moved.get_value().value(), value_2); - EXPECT_DOUBLE_EQ(handle.get_value().value(), value); + EXPECT_DOUBLE_EQ(moved.get_optional().value(), value_2); + EXPECT_DOUBLE_EQ(handle.get_optional().value(), value); moved = std::move(handle); - EXPECT_DOUBLE_EQ(moved.get_value().value(), value); + EXPECT_DOUBLE_EQ(moved.get_optional().value(), value); EXPECT_NO_THROW({ moved.set_value(0.0); }); - EXPECT_DOUBLE_EQ(moved.get_value().value(), 0.0); + EXPECT_DOUBLE_EQ(moved.get_optional().value(), 0.0); } diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index f79a4bc5e6..75ce7fe5f7 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1284,13 +1284,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().value(), 1.0); - EXPECT_EQ(claimed_itf3.get_value().value(), 3.0); + EXPECT_EQ(claimed_itf1.get_optional().value(), 1.0); + EXPECT_EQ(claimed_itf3.get_optional().value(), 3.0); claimed_itf1.set_value(11.1); claimed_itf3.set_value(33.3); - EXPECT_EQ(claimed_itf1.get_value().value(), 11.1); - EXPECT_EQ(claimed_itf3.get_value().value(), 33.3); + EXPECT_EQ(claimed_itf1.get_optional().value(), 11.1); + EXPECT_EQ(claimed_itf3.get_optional().value(), 33.3); EXPECT_EQ(reference_interface_values[0], 11.1); EXPECT_EQ(reference_interface_values[1], 2.0); @@ -1819,8 +1819,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().value(); - double prev_system_state_value = state_itfs[1].get_value().value(); + double prev_act_state_value = state_itfs[0].get_optional().value(); + double prev_system_state_value = state_itfs[1].get_optional().value(); for (size_t i = 1; i < 100; i++) { @@ -1830,19 +1830,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().value() / 2.0; - claimed_itfs[1].set_value(claimed_itfs[1].get_value().value() + 20.0); + prev_system_state_value = claimed_itfs[1].get_optional().value() / 2.0; + claimed_itfs[1].set_value(claimed_itfs[1].get_optional().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().value() / 2.0; - claimed_itfs[0].set_value(claimed_itfs[0].get_value().value() + 10.0); + prev_act_state_value = claimed_itfs[0].get_optional().value() / 2.0; + claimed_itfs[0].set_value(claimed_itfs[0].get_optional().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().value(), prev_act_state_value); - ASSERT_EQ(state_itfs[1].get_value().value(), prev_system_state_value); + ASSERT_EQ(state_itfs[0].get_optional().value(), prev_act_state_value); + ASSERT_EQ(state_itfs[1].get_optional().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()); @@ -2032,8 +2032,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().value(); - double prev_system_state_value = state_itfs[1].get_value().value(); + double prev_act_state_value = state_itfs[0].get_optional().value(); + double prev_system_state_value = state_itfs[1].get_optional().value(); const double actuator_increment = 10.0; const double system_increment = 20.0; for (size_t i = 1; i < 100; i++) @@ -2045,17 +2045,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().value() / 2.0; - prev_act_state_value = claimed_itfs[0].get_value().value() / 2.0; + prev_system_state_value = claimed_itfs[1].get_optional().value() / 2.0; + prev_act_state_value = claimed_itfs[0].get_optional().value() / 2.0; } - 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); + claimed_itfs[0].set_value(claimed_itfs[0].get_optional().value() + actuator_increment); + claimed_itfs[1].set_value(claimed_itfs[1].get_optional().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().value(), prev_act_state_value, actuator_increment / 2.0); + state_itfs[0].get_optional().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); + state_itfs[1].get_optional().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 db664af74d..9ce0058089 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().value(), 1.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 101.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 102.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 202.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 203.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 303.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().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().value(), 303.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 403.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 403.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 403.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 403.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 503.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 503.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 503.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 503.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 603.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 603.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().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().value(), 1.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 1.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 1.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 101.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 101.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 101.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 102.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 102.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 102.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 202.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 202.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 202.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 203.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 203.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 203.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 303.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 303.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().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().value(), 304.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 304.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 304.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 304.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 404.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 404.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 404.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 404.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 405.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 405.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 405.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 405.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 505.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 505.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 505.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 505.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 506.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 506.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 506.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 506.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 606.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 606.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 606.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().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().value(), 1.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 1.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 1.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 101.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 101.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 101.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 102.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 102.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 102.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 202.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 202.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 202.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 203.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 203.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 203.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 303.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 303.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().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().value(), 304.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 304.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 304.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 304.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 404.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 404.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 404.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 404.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 405.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 405.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 405.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 405.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 505.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 505.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 505.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 505.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 506.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 506.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 506.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 506.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 606.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 606.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 606.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().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().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 100.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 100.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 100.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 100.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 200.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 200.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 200.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 200.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 300.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().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().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 301.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 301.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 401.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 401.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 402.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 402.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 502.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 502.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 503.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 503.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 603.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().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().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().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().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value().value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value().value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0); }; int main(int argc, char ** argv)