From 726bca7e4b677dd8f05dd7dc6b2eecff9ff5a9fb Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 24 Jan 2024 11:53:23 +0100 Subject: [PATCH] not finished, showcase for what hase to be changed --- .../test_chainable_controller_interface.cpp | 4 +- .../hardware_interface/actuator_interface.hpp | 49 +++- .../include/hardware_interface/handle.hpp | 40 +-- .../loaned_command_interface.hpp | 2 +- .../loaned_state_interface.hpp | 2 +- .../hardware_interface/sensor_interface.hpp | 43 ++- .../hardware_interface/system_interface.hpp | 50 +++- .../hardware_interface_error_signals.hpp | 4 + .../hardware_interface_warning_signals.hpp | 4 + .../test/test_component_interfaces.cpp | 188 +++++++------ ...est_component_interfaces_custom_export.cpp | 2 +- .../test/test_error_warning_codes.cpp | 257 ++++++++++-------- hardware_interface/test/test_handle.cpp | 12 +- .../differential_transmission.hpp | 31 ++- .../four_bar_linkage_transmission.hpp | 28 +- .../simple_transmission.hpp | 12 +- 16 files changed, 428 insertions(+), 300 deletions(-) diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index d81d2bfdad4..4a695682c4b 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -42,7 +42,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) EXPECT_EQ(reference_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf"); - EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size) @@ -88,7 +88,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode - EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index efdb990df51..54c1d091624 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -370,7 +370,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod double get_state(const std::string & interface_name) const { - return actuator_states_.at(interface_name)->get_value(); + return actuator_states_.at(interface_name)->get_value(); } void set_command(const std::string & interface_name, const double & value) @@ -380,37 +380,60 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod double get_command(const std::string & interface_name) const { - return actuator_commands_.at(interface_name)->get_value(); + return actuator_commands_.at(interface_name)->get_value(); } - void set_emergency_stop(const double & emergency_stop) + void set_emergency_stop(const bool & emergency_stop) { emergency_stop_->set_value(emergency_stop); } - double get_emergency_stop() const { return emergency_stop_->get_value(); } + bool get_emergency_stop() const { return emergency_stop_->get_value(); } - void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + void set_error_code(std::array error_codes) + { + error_signal_->set_value(error_codes); + } - double get_error_code() const { return error_signal_->get_value(); } + std::array get_error_code() const + { + return error_signal_->get_value>(); + } - void set_error_message(const double & error_message) + void set_error_message( + std::array error_messages) { - error_signal_message_->set_value(error_message); + error_signal_message_->set_value(error_messages); } - double get_error_message() const { return error_signal_message_->get_value(); } + std::array get_error_message() const + { + return error_signal_message_ + ->get_value>(); + } - void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + void set_warning_code(std::array warning_codes) + { + warning_signal_->set_value(warning_codes); + } - double get_warning_code() const { return warning_signal_->get_value(); } + std::array get_warning_code() const + { + return warning_signal_ + ->get_value>(); + } - void set_warning_message(const double & error_message) + void set_warning_message( + std::array error_message) { warning_signal_message_->set_value(error_message); } - double get_warning_message() const { return warning_signal_message_->get_value(); } + std::array get_warning_message() const + { + return warning_signal_message_ + ->get_value>(); + } protected: HardwareInfo info_; diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 476147ddc76..72ed4ae9378 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -30,7 +31,19 @@ namespace hardware_interface { -typedef std::variant HANDLE_DATATYPE; +using HANDLE_DATATYPE = std::variant< + bool, double, hardware_interface::WARNING_SIGNALS, hardware_interface::ERROR_SIGNALS, + hardware_interface::WARNING_MESSAGES>; + +// Define a type trait for allowed types +template +struct HANDLE_DATATYPE_TYPES : std::disjunction< + std::is_same, std::is_same, + std::is_same, + std::is_same, + std::is_same> +{ +}; /// A handle used to get and set a value on a given interface. class Handle @@ -49,10 +62,10 @@ class Handle : prefix_name_(interface_description.prefix_name_), interface_name_(interface_description.interface_info.name) { + // TODO(Manuel): implement this. // As soon as multiple datatypes are used in HANDLE_DATATYPE // we need to initialize according the type passed in interface description - value_ = std::numeric_limits::quiet_NaN(); - value_ptr_ = std::get_if(&value_); + // value_ = std::numeric_limits::quiet_NaN(); } [[deprecated("Use InterfaceDescription for initializing the Interface")]] @@ -95,32 +108,23 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } - double get_value() const + template ::value, int>::type = 0> + T get_value() const { - // BEGIN (Handle export change): for backward compatibility - // TODO(Manuel) return value_ if old functionality is removed - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; - // END + return std::get(value_); } - void set_value(double value) + template ::value, int>::type = 0> + void set_value(T value) { - // BEGIN (Handle export change): for backward compatibility - // TODO(Manuel) set value_ directly if old functionality is removed - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; - // END + value_ = value; } protected: std::string prefix_name_; std::string interface_name_; HANDLE_DATATYPE value_; - // BEGIN (Handle export change): for backward compatibility - // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; - // END }; class StateInterface : public Handle diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index bb5807c398f..6fc445ac294 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -65,7 +65,7 @@ class LoanedCommandInterface void set_value(double val) { command_interface_.set_value(val); } - double get_value() const { return command_interface_.get_value(); } + double get_value() const { return command_interface_.get_value(); } protected: CommandInterface & command_interface_; diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 4fe67d1290f..11abae41d09 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -63,7 +63,7 @@ class LoanedStateInterface const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } - double get_value() const { return state_interface_.get_value(); } + double get_value() const { return state_interface_.get_value(); } protected: StateInterface & state_interface_; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index e36c18e5cb0..824fc00437d 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -252,30 +252,53 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI double get_state(const std::string & interface_name) const { - return sensor_states_.at(interface_name)->get_value(); + return sensor_states_.at(interface_name)->get_value(); } - void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + void set_error_code(std::array error_codes) + { + error_signal_->set_value(error_codes); + } - double get_error_code() const { return error_signal_->get_value(); } + std::array get_error_code() const + { + return error_signal_->get_value>(); + } - void set_error_message(const double & error_message) + void set_error_message( + std::array error_messages) { - error_signal_message_->set_value(error_message); + error_signal_message_->set_value(error_messages); } - double get_error_message() const { return error_signal_message_->get_value(); } + std::array get_error_message() const + { + return error_signal_message_ + ->get_value>(); + } - void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + void set_warning_code(std::array warning_codes) + { + warning_signal_->set_value(warning_codes); + } - double get_warning_code() const { return warning_signal_->get_value(); } + std::array get_warning_code() const + { + return warning_signal_ + ->get_value>(); + } - void set_warning_message(const double & error_message) + void set_warning_message( + std::array error_message) { warning_signal_message_->set_value(error_message); } - double get_warning_message() const { return warning_signal_message_->get_value(); } + std::array get_warning_message() const + { + return warning_signal_message_ + ->get_value>(); + } protected: HardwareInfo info_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 6496d38069f..742dfdd0f81 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -402,7 +402,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return state. */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } - void set_state(const std::string & interface_name, const double & value) { system_states_.at(interface_name)->set_value(value); @@ -410,7 +409,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI double get_state(const std::string & interface_name) const { - return system_states_.at(interface_name)->get_value(); + return system_states_.at(interface_name)->get_value(); } void set_command(const std::string & interface_name, const double & value) @@ -420,37 +419,60 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI double get_command(const std::string & interface_name) const { - return system_commands_.at(interface_name)->get_value(); + return system_commands_.at(interface_name)->get_value(); } - void set_emergency_stop(const double & emergency_stop) + void set_emergency_stop(const bool & emergency_stop) { emergency_stop_->set_value(emergency_stop); } - double get_emergency_stop() const { return emergency_stop_->get_value(); } + bool get_emergency_stop() const { return emergency_stop_->get_value(); } - void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + void set_error_code(std::array error_codes) + { + error_signal_->set_value(error_codes); + } - double get_error_code() const { return error_signal_->get_value(); } + std::array get_error_code() const + { + return error_signal_->get_value>(); + } - void set_error_message(const double & error_message) + void set_error_message( + std::array error_messages) { - error_signal_message_->set_value(error_message); + error_signal_message_->set_value(error_messages); } - double get_error_message() const { return error_signal_message_->get_value(); } + std::array get_error_message() const + { + return error_signal_message_ + ->get_value>(); + } - void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + void set_warning_code(std::array warning_codes) + { + warning_signal_->set_value(warning_codes); + } - double get_warning_code() const { return warning_signal_->get_value(); } + std::array get_warning_code() const + { + return warning_signal_ + ->get_value>(); + } - void set_warning_message(const double & error_message) + void set_warning_message( + std::array error_message) { warning_signal_message_->set_value(error_message); } - double get_warning_message() const { return warning_signal_message_->get_value(); } + std::array get_warning_message() const + { + return warning_signal_message_ + ->get_value>(); + } protected: HardwareInfo info_; diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp index d5159f29c81..88848a7ca84 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ #include +#include #include namespace hardware_interface @@ -23,6 +24,7 @@ namespace hardware_interface // Count of how many different error signals there are that can be reported. const size_t error_signal_count = 32; +using ERROR_SIGNALS = std::array; constexpr char ERROR_SIGNAL_INTERFACE_NAME[] = "ERROR_SIGNAL"; // Available error signal names @@ -62,6 +64,8 @@ enum class error_signal : std::uint8_t ERROR_SIGNAL_31 = 31 }; +using ERROR_MESSAGES = std::array; + constexpr char ERROR_SIGNAL_MESSAGE_INTERFACE_NAME[] = "ERROR_SIGNAL_MESSAGE"; // Available WARNING signal message names enum class error_signal_message : std::uint8_t diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp index 58af04b80ae..b9ebcfdeb30 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp @@ -16,12 +16,14 @@ #define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ #include +#include #include namespace hardware_interface { // Count of how many different warn signals there are that can be reported. const size_t warning_signal_count = 32; +using WARNING_SIGNALS = std::array; constexpr char WARNING_SIGNAL_INTERFACE_NAME[] = "WARNING_SIGNAL"; // Available warning signals names mapping to position in the interface @@ -61,6 +63,8 @@ enum class warning_signal : std::uint8_t WARNING_SIGNAL_31 = 31 }; +using WARNING_MESSAGES = std::array; + constexpr char WARNING_SIGNAL_MESSAGE_INTERFACE_NAME[] = "WARNING_SIGNAL_MESSAGE"; // Available WARNING signal message names enum class warning_signal_message : std::uint8_t diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index ebf9b9bd765..905ceafa132 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -716,8 +716,8 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -731,8 +731,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()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -746,8 +746,9 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -761,8 +762,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()); // position value - EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -814,8 +815,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -829,8 +830,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -844,8 +845,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -859,8 +861,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -886,21 +888,21 @@ TEST(TestComponentInterfaces, dummy_sensor) EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); // Not updated because is is UNCONFIGURED sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); // Updated because is is INACTIVE state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[0]->get_value()); + EXPECT_EQ(0.0, state_interfaces[0]->get_value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[0]->get_value()); + EXPECT_EQ(0x666, state_interfaces[0]->get_value()); } // END @@ -924,21 +926,21 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); // Not updated because is is UNCONFIGURED sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); // Updated because is is INACTIVE state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[0]->get_value()); + EXPECT_EQ(0.0, state_interfaces[0]->get_value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[0]->get_value()); + EXPECT_EQ(0x666, state_interfaces[0]->get_value()); } // BEGIN (Handle export change): for backward compatibility @@ -995,12 +997,12 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1014,12 +1016,12 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1033,12 +1035,15 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[5]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[5]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1052,12 +1057,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()); // position value - EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1126,12 +1131,12 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1145,12 +1150,12 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1164,12 +1169,15 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[5]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[5]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1183,12 +1191,12 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1261,8 +1269,8 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1329,8 +1337,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1389,8 +1397,8 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1456,8 +1464,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1519,7 +1527,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) // activate again and expect reset values state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1584,7 +1592,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) // activate again and expect reset values state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1642,11 +1650,11 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1713,11 +1721,11 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1778,11 +1786,11 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1849,11 +1857,11 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 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 f61cd7214bd..359d87961d8 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -225,7 +225,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[interface_offset]->get_name()); EXPECT_EQ("some_unlisted_interface", state_interfaces[interface_offset]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[interface_offset]->get_prefix_name()); diff --git a/hardware_interface/test/test_error_warning_codes.cpp b/hardware_interface/test/test_error_warning_codes.cpp index 86b733adc7d..137a779070f 100644 --- a/hardware_interface/test/test_error_warning_codes.cpp +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -78,11 +78,11 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface { set_state("joint1/position", 0.0); set_state("joint1/velocity", 0.0); - set_emergency_stop(0.0); - set_error_code(0.0); - set_error_message(0.0); - set_warning_code(0.0); - set_warning_message(0.0); + set_emergency_stop(false); + set_error_code(hardware_interface::ERROR_SIGNALS{}); + set_error_message(hardware_interface::WARNING_MESSAGES{}); + set_warning_code(hardware_interface::WARNING_SIGNALS{}); + set_warning_message(hardware_interface::WARNING_MESSAGES{}); if (recoverable_error_happened_) { @@ -103,11 +103,16 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface ++read_calls_; if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_emergency_stop(1.0); - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_emergency_stop(true); + hardware_interface::ERROR_SIGNALS error_signals{1, 2, 3}; + set_error_code(error_signals); + hardware_interface::WARNING_MESSAGES error_messages{"Some", "error", "message that is split"}; + set_error_message(error_messages); + hardware_interface::WARNING_SIGNALS warning_signals{1, 2, 3}; + set_warning_code(warning_signals); + hardware_interface::WARNING_MESSAGES warning_messages{ + "Some", "warning", "message that is split"}; + set_warning_message(warning_messages); return hardware_interface::return_type::ERROR; } @@ -121,11 +126,16 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface ++write_calls_; if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_emergency_stop(1.0); - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_emergency_stop(true); + hardware_interface::ERROR_SIGNALS error_signals{1, 2, 3}; + set_error_code(error_signals); + hardware_interface::WARNING_MESSAGES error_messages{"Some", "error", "message that is split"}; + set_error_message(error_messages); + hardware_interface::WARNING_SIGNALS warning_signals{1, 2, 3}; + set_warning_code(warning_signals); + hardware_interface::WARNING_MESSAGES warning_messages{ + "Some", "warning", "message that is split"}; + set_warning_message(warning_messages); return hardware_interface::return_type::ERROR; } auto position_state = get_state("joint1/position"); @@ -183,10 +193,10 @@ class DummySensorDefault : public hardware_interface::SensorInterface { set_state(name, 0.0); } - set_error_code(0.0); - set_error_message(0.0); - set_warning_code(0.0); - set_warning_message(0.0); + set_error_code(hardware_interface::ERROR_SIGNALS{}); + set_error_message(hardware_interface::WARNING_MESSAGES{}); + set_warning_code(hardware_interface::WARNING_SIGNALS{}); + set_warning_message(hardware_interface::WARNING_MESSAGES{}); read_calls_ = 0; return CallbackReturn::SUCCESS; } @@ -199,10 +209,15 @@ class DummySensorDefault : public hardware_interface::SensorInterface ++read_calls_; if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + hardware_interface::ERROR_SIGNALS error_signals{1, 2, 3}; + set_error_code(error_signals); + hardware_interface::WARNING_MESSAGES error_messages{"Some", "error", "message that is split"}; + set_error_message(error_messages); + hardware_interface::WARNING_SIGNALS warning_signals{1, 2, 3}; + set_warning_code(warning_signals); + hardware_interface::WARNING_MESSAGES warning_messages{ + "Some", "warning", "message that is split"}; + set_warning_message(warning_messages); return hardware_interface::return_type::ERROR; } @@ -254,11 +269,11 @@ class DummySystemDefault : public hardware_interface::SystemInterface set_state(position_states_[i], 0.0); set_state(velocity_states_[i], 0.0); } - set_emergency_stop(0.0); - set_error_code(0.0); - set_error_message(0.0); - set_warning_code(0.0); - set_warning_message(0.0); + set_emergency_stop(false); + set_error_code(hardware_interface::ERROR_SIGNALS{}); + set_error_message(hardware_interface::WARNING_MESSAGES{}); + set_warning_code(hardware_interface::WARNING_SIGNALS{}); + set_warning_message(hardware_interface::WARNING_MESSAGES{}); // reset command only if error is initiated if (recoverable_error_happened_) { @@ -282,11 +297,16 @@ class DummySystemDefault : public hardware_interface::SystemInterface ++read_calls_; if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_emergency_stop(1.0); - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_emergency_stop(true); + hardware_interface::ERROR_SIGNALS error_signals{1, 2, 3}; + set_error_code(error_signals); + hardware_interface::WARNING_MESSAGES error_messages{"Some", "error", "message that is split"}; + set_error_message(error_messages); + hardware_interface::WARNING_SIGNALS warning_signals{1, 2, 3}; + set_warning_code(warning_signals); + hardware_interface::WARNING_MESSAGES warning_messages{ + "Some", "warning", "message that is split"}; + set_warning_message(warning_messages); return hardware_interface::return_type::ERROR; } @@ -300,11 +320,16 @@ class DummySystemDefault : public hardware_interface::SystemInterface ++write_calls_; if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_emergency_stop(1.0); - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_emergency_stop(true); + hardware_interface::ERROR_SIGNALS error_signals{1, 2, 3}; + set_error_code(error_signals); + hardware_interface::WARNING_MESSAGES error_messages{"Some", "error", "message that is split"}; + set_error_message(error_messages); + hardware_interface::WARNING_SIGNALS warning_signals{1, 2, 3}; + set_warning_code(warning_signals); + hardware_interface::WARNING_MESSAGES warning_messages{ + "Some", "warning", "message that is split"}; + set_warning_message(warning_messages); return hardware_interface::return_type::ERROR; } @@ -455,7 +480,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); // ERROR_SIGNAL EXPECT_EQ( "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME), @@ -608,18 +633,18 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -627,8 +652,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -641,18 +666,18 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -704,8 +729,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -718,18 +743,18 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -768,16 +793,16 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); state = sensor_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -785,7 +810,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) // activate again and expect reset values state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -838,18 +863,18 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -859,11 +884,11 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -876,18 +901,18 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -929,18 +954,18 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -950,11 +975,11 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 844adeaecbc..3d2b6f0b3c4 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -33,9 +33,9 @@ TEST(TestHandle, command_interface) CommandInterface interface { JOINT_NAME, FOO_INTERFACE, &value }; - EXPECT_DOUBLE_EQ(interface.get_value(), value); + EXPECT_DOUBLE_EQ(interface.get_value(), value); EXPECT_NO_THROW(interface.set_value(0.0)); - EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); + EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); } TEST(TestHandle, state_interface) @@ -44,7 +44,7 @@ TEST(TestHandle, state_interface) StateInterface interface { JOINT_NAME, FOO_INTERFACE, &value }; - EXPECT_DOUBLE_EQ(interface.get_value(), value); + EXPECT_DOUBLE_EQ(interface.get_value(), value); // interface.set_value(5); compiler error, no set_value function } @@ -59,7 +59,7 @@ TEST(TestHandle, name_getters_work) TEST(TestHandle, value_methods_throw_for_nullptr) { CommandInterface handle{JOINT_NAME, FOO_INTERFACE}; - EXPECT_ANY_THROW(handle.get_value()); + EXPECT_ANY_THROW(handle.get_value()); EXPECT_ANY_THROW(handle.set_value(0.0)); } @@ -67,9 +67,9 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) { double value = 1.337; CommandInterface handle{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(handle.get_value(), value); + EXPECT_DOUBLE_EQ(handle.get_value(), value); EXPECT_NO_THROW(handle.set_value(0.0)); - EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); + EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } TEST(TestHandle, interface_description_state_interface_name_getters_work) diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 2002f6f9d7e..53a084fe762 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -264,10 +264,12 @@ inline void DifferentialTransmission::actuator_to_joint() assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); joint_pos[0].set_value( - (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) + + (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / + (2.0 * jr[0]) + joint_offset_[0]); joint_pos[1].set_value( - (act_pos[0].get_value() / ar[0] - act_pos[1].get_value() / ar[1]) / (2.0 * jr[1]) + + (act_pos[0].get_value() / ar[0] - act_pos[1].get_value() / ar[1]) / + (2.0 * jr[1]) + joint_offset_[1]); } @@ -278,9 +280,11 @@ inline void DifferentialTransmission::actuator_to_joint() assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); joint_vel[0].set_value( - (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / (2.0 * jr[0])); + (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / + (2.0 * jr[0])); joint_vel[1].set_value( - (act_vel[0].get_value() / ar[0] - act_vel[1].get_value() / ar[1]) / (2.0 * jr[1])); + (act_vel[0].get_value() / ar[0] - act_vel[1].get_value() / ar[1]) / + (2.0 * jr[1])); } auto & act_eff = actuator_effort_; @@ -290,9 +294,9 @@ inline void DifferentialTransmission::actuator_to_joint() assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); joint_eff[0].set_value( - jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1])); + jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1])); joint_eff[1].set_value( - jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1])); + jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1])); } } @@ -308,7 +312,8 @@ inline void DifferentialTransmission::joint_to_actuator() assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); double joints_offset_applied[2] = { - joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; + joint_pos[0].get_value() - joint_offset_[0], + joint_pos[1].get_value() - joint_offset_[1]}; act_pos[0].set_value( (joints_offset_applied[0] * jr[0] + joints_offset_applied[1] * jr[1]) * ar[0]); act_pos[1].set_value( @@ -322,9 +327,11 @@ inline void DifferentialTransmission::joint_to_actuator() assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); act_vel[0].set_value( - (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * ar[0]); + (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * + ar[0]); act_vel[1].set_value( - (joint_vel[0].get_value() * jr[0] - joint_vel[1].get_value() * jr[1]) * ar[1]); + (joint_vel[0].get_value() * jr[0] - joint_vel[1].get_value() * jr[1]) * + ar[1]); } auto & act_eff = actuator_effort_; @@ -334,9 +341,11 @@ inline void DifferentialTransmission::joint_to_actuator() assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); act_eff[0].set_value( - (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / (2.0 * ar[0])); + (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / + (2.0 * ar[0])); act_eff[1].set_value( - (joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / (2.0 * ar[1])); + (joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / + (2.0 * ar[1])); } } diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index 39f5df6f61c..8b5fdd54338 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -261,9 +261,10 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - joint_pos[0].set_value(act_pos[0].get_value() / (jr[0] * ar[0]) + joint_offset_[0]); + joint_pos[0].set_value(act_pos[0].get_value() / (jr[0] * ar[0]) + joint_offset_[0]); joint_pos[1].set_value( - (act_pos[1].get_value() / ar[1] - act_pos[0].get_value() / (jr[0] * ar[0])) / jr[1] + + (act_pos[1].get_value() / ar[1] - act_pos[0].get_value() / (jr[0] * ar[0])) / + jr[1] + joint_offset_[1]); } @@ -274,9 +275,10 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - joint_vel[0].set_value(act_vel[0].get_value() / (jr[0] * ar[0])); + joint_vel[0].set_value(act_vel[0].get_value() / (jr[0] * ar[0])); joint_vel[1].set_value( - (act_vel[1].get_value() / ar[1] - act_vel[0].get_value() / (jr[0] * ar[0])) / jr[1]); + (act_vel[1].get_value() / ar[1] - act_vel[0].get_value() / (jr[0] * ar[0])) / + jr[1]); } // effort @@ -286,9 +288,10 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); + joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); joint_eff[1].set_value( - jr[1] * (act_eff[1].get_value() * ar[1] - act_eff[0].get_value() * ar[0] * jr[0])); + jr[1] * + (act_eff[1].get_value() * ar[1] - act_eff[0].get_value() * ar[0] * jr[0])); } } @@ -305,7 +308,8 @@ inline void FourBarLinkageTransmission::joint_to_actuator() assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); double joints_offset_applied[2] = { - joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; + joint_pos[0].get_value() - joint_offset_[0], + joint_pos[1].get_value() - joint_offset_[1]}; act_pos[0].set_value(joints_offset_applied[0] * jr[0] * ar[0]); act_pos[1].set_value((joints_offset_applied[0] + joints_offset_applied[1] * jr[1]) * ar[1]); } @@ -317,8 +321,9 @@ inline void FourBarLinkageTransmission::joint_to_actuator() { assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - act_vel[0].set_value(joint_vel[0].get_value() * jr[0] * ar[0]); - act_vel[1].set_value((joint_vel[0].get_value() + joint_vel[1].get_value() * jr[1]) * ar[1]); + act_vel[0].set_value(joint_vel[0].get_value() * jr[0] * ar[0]); + act_vel[1].set_value( + (joint_vel[0].get_value() + joint_vel[1].get_value() * jr[1]) * ar[1]); } // effort @@ -328,8 +333,9 @@ inline void FourBarLinkageTransmission::joint_to_actuator() { assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0])); - act_eff[1].set_value((joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]); + act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0])); + act_eff[1].set_value( + (joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]); } } diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index 809472ab754..b240c1e4895 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -218,17 +218,17 @@ inline void SimpleTransmission::actuator_to_joint() { if (joint_effort_ && actuator_effort_) { - joint_effort_.set_value(actuator_effort_.get_value() * reduction_); + joint_effort_.set_value(actuator_effort_.get_value() * reduction_); } if (joint_velocity_ && actuator_velocity_) { - joint_velocity_.set_value(actuator_velocity_.get_value() / reduction_); + joint_velocity_.set_value(actuator_velocity_.get_value() / reduction_); } if (joint_position_ && actuator_position_) { - joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_); + joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_); } } @@ -236,17 +236,17 @@ inline void SimpleTransmission::joint_to_actuator() { if (joint_effort_ && actuator_effort_) { - actuator_effort_.set_value(joint_effort_.get_value() / reduction_); + actuator_effort_.set_value(joint_effort_.get_value() / reduction_); } if (joint_velocity_ && actuator_velocity_) { - actuator_velocity_.set_value(joint_velocity_.get_value() * reduction_); + actuator_velocity_.set_value(joint_velocity_.get_value() * reduction_); } if (joint_position_ && actuator_position_) { - actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_); + actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_); } }