From 0e24e3af12fefbeb9627f53dd2d4e4d32f77dd49 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 31 Jan 2024 15:52:22 +0100 Subject: [PATCH] add parsing of handle type for initialization of variant and adjust most tests --- .../hardware_interface/actuator_interface.hpp | 44 +- .../include/hardware_interface/handle.hpp | 105 +- .../hardware_interface/hardware_info.hpp | 11 + .../hardware_interface/sensor_interface.hpp | 44 +- .../hardware_interface/system_interface.hpp | 44 +- .../types/handle_datatype.hpp | 43 + .../hardware_interface_error_signals.hpp | 77 +- .../hardware_interface_warning_signals.hpp | 77 +- .../test/test_component_interfaces.cpp | 1008 ++--------------- .../test/test_error_warning_codes.cpp | 297 ++--- hardware_interface/test/test_handle.cpp | 535 ++++++++- 11 files changed, 951 insertions(+), 1334 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/types/handle_datatype.hpp diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index dc04c79cd0..47281f659b 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -159,13 +159,15 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // create error signal interface InterfaceInfo error_interface_info; error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; - error_interface_info.data_type = "array[32]"; + error_interface_info.data_type = "vector"; + error_interface_info.size = 32; InterfaceDescription error_interface_descr(info_.name, error_interface_info); error_signal_ = std::make_shared(error_interface_descr); // create error signal report message interface InterfaceInfo error_msg_interface_info; error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; - error_msg_interface_info.data_type = "array[32]"; + error_msg_interface_info.data_type = "vector"; + error_msg_interface_info.size = 32; InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); error_signal_message_ = std::make_shared(error_msg_interface_descr); @@ -173,13 +175,15 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // create warning signal interface InterfaceInfo warning_interface_info; warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; - warning_interface_info.data_type = "array[32]"; + warning_interface_info.data_type = "vector"; + warning_interface_info.size = 32; InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); warning_signal_ = std::make_shared(warning_interface_descr); // create warning signal report message interface InterfaceInfo warning_msg_interface_info; warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; - warning_msg_interface_info.data_type = "array[32]"; + warning_msg_interface_info.data_type = "vector"; + warning_msg_interface_info.size = 32; InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } @@ -451,49 +455,41 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod bool get_emergency_stop() const { return emergency_stop_->get_value(); } - void set_error_code(std::array error_codes) - { - error_signal_->set_value(error_codes); - } + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } - std::array get_error_code() const + std::vector get_error_code() const { - return error_signal_->get_value>(); + return error_signal_->get_value>(); } - void set_error_message( - std::array error_messages) + void set_error_message(std::vector error_messages) { error_signal_message_->set_value(error_messages); } - std::array get_error_message() const + std::vector get_error_message() const { - return error_signal_message_ - ->get_value>(); + return error_signal_message_->get_value>(); } - void set_warning_code(std::array warning_codes) + void set_warning_code(std::vector warning_codes) { warning_signal_->set_value(warning_codes); } - std::array get_warning_code() const + std::vector get_warning_code() const { - return warning_signal_ - ->get_value>(); + return warning_signal_->get_value>(); } - void set_warning_message( - std::array error_message) + void set_warning_message(std::vector error_message) { warning_signal_message_->set_value(error_message); } - std::array get_warning_message() const + std::vector get_warning_message() const { - return warning_signal_message_ - ->get_value>(); + return warning_signal_message_->get_value>(); } protected: diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index b10413bba9..31b459177a 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,35 +15,20 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ -#include #include #include #include #include #include #include +#include #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" -#include "hardware_interface/types/hardware_interface_error_signals.hpp" -#include "hardware_interface/types/hardware_interface_warning_signals.hpp" -#include "hardware_interface/visibility_control.h" -namespace hardware_interface -{ +#include "hardware_interface/types/handle_datatype.hpp" -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> +namespace hardware_interface { -}; /// A handle used to get and set a value on a given interface. class Handle @@ -56,16 +41,15 @@ class Handle double * value_ptr = nullptr) : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) { + // default to double + value_ = std::numeric_limits::quiet_NaN(); } explicit Handle(const InterfaceDescription & interface_description) : 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(); + init_handle_value(interface_description.interface_info); } [[deprecated("Use InterfaceDescription for initializing the Interface")]] @@ -73,13 +57,16 @@ class Handle explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { + // default to double + value_ = std::numeric_limits::quiet_NaN(); } [[deprecated("Use InterfaceDescription for initializing the Interface")]] explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) - { + { // default to double + value_ = std::numeric_limits::quiet_NaN(); } Handle(const Handle & other) = default; @@ -121,6 +108,78 @@ class Handle } protected: + // used for the + bool correct_vector_size(const size_t & expected, const size_t & actual) + { + return expected == actual; + } + + void init_handle_value(const InterfaceInfo & interface_info) + { + if (interface_info.data_type == "bool") + { + value_ = interface_info.initial_value.empty() ? false + : (interface_info.initial_value == "true" || + interface_info.initial_value == "True"); + } + else if (interface_info.data_type == "vector") + { + if ( + interface_info.size != 0 && hardware_interface::warning_signal_count != interface_info.size) + { + throw std::runtime_error( + "The size:{" + std::to_string(interface_info.size) + "} for data_type{" + + interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name + + "} does not equal the expected size:{" + + std::to_string(hardware_interface::warning_signal_count) + "}."); + } + value_ = std::vector(hardware_interface::warning_signal_count, 0); + } + else if (interface_info.data_type == "vector") + { + if (interface_info.size != 0 && hardware_interface::error_signal_count != interface_info.size) + { + throw std::runtime_error( + "The size:{" + std::to_string(interface_info.size) + "} for data_type{" + + interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name + + "} does not equal the expected size:{" + + std::to_string(hardware_interface::error_signal_count) + "}."); + } + + value_ = std::vector(hardware_interface::error_signal_count, 0); + } + else if (interface_info.data_type == "vector") + { + if ( + interface_info.size != 0 && hardware_interface::warning_signal_count != interface_info.size) + { + throw std::runtime_error( + "The size:{" + std::to_string(interface_info.size) + "} for data_type{" + + interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name + + "} does not equal the expected size:{" + + std::to_string(hardware_interface::warning_signal_count) + "}."); + } + + value_ = std::vector(hardware_interface::warning_signal_count, ""); + } + // Default for empty is double + else if (interface_info.data_type.empty() || interface_info.data_type == "double") + { + value_ = interface_info.initial_value.empty() ? std::numeric_limits::quiet_NaN() + : std::stod(interface_info.initial_value); + } + // If not empty and it belongs to none of the above types, we still want to throw as there might + // be a typo in the data_type like "bol" or user wants some unsupported type + else + { + throw std::runtime_error( + "The data_type:{" + interface_info.data_type + "} for the InterfaceInfo with name:{" + + interface_info.name + + "} is not supported for Handles. Supported data_types are: bool, double, vector, " + "vector and vector."); + } + } + std::string prefix_name_; std::string interface_name_; HANDLE_DATATYPE value_; diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 6322fdc17e..dd757ffaeb 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -27,6 +27,17 @@ namespace hardware_interface */ struct InterfaceInfo { + // Add default constructor, so that e.g. size is initialized to sensible value + InterfaceInfo() + { + // cpp_lint complains about min and max include otherwise + name = ""; + min = ""; + max = ""; + initial_value = ""; + data_type = ""; + size = 0; + } /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index bf014b0d09..545548104e 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -136,13 +136,15 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create error signal interface InterfaceInfo error_interface_info; error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; - error_interface_info.data_type = "array[32]"; + error_interface_info.data_type = "vector"; + error_interface_info.size = 32; InterfaceDescription error_interface_descr(info_.name, error_interface_info); error_signal_ = std::make_shared(error_interface_descr); // create error signal report message interface InterfaceInfo error_msg_interface_info; error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; - error_msg_interface_info.data_type = "array[32]"; + error_msg_interface_info.data_type = "vector"; + error_msg_interface_info.size = 32; InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); error_signal_message_ = std::make_shared(error_msg_interface_descr); @@ -150,13 +152,15 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create warning signal interface InterfaceInfo warning_interface_info; warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; - warning_interface_info.data_type = "array[32]"; + warning_interface_info.data_type = "vector"; + warning_interface_info.size = 32; InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); warning_signal_ = std::make_shared(warning_interface_descr); // create warning signal report message interface InterfaceInfo warning_msg_interface_info; warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; - warning_msg_interface_info.data_type = "array[32]"; + warning_msg_interface_info.data_type = "vector"; + warning_msg_interface_info.size = 32; InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } @@ -285,49 +289,41 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return sensor_states_.at(interface_name)->get_value(); } - void set_error_code(std::array error_codes) - { - error_signal_->set_value(error_codes); - } + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } - std::array get_error_code() const + std::vector get_error_code() const { - return error_signal_->get_value>(); + return error_signal_->get_value>(); } - void set_error_message( - std::array error_messages) + void set_error_message(std::vector error_messages) { error_signal_message_->set_value(error_messages); } - std::array get_error_message() const + std::vector get_error_message() const { - return error_signal_message_ - ->get_value>(); + return error_signal_message_->get_value>(); } - void set_warning_code(std::array warning_codes) + void set_warning_code(std::vector warning_codes) { warning_signal_->set_value(warning_codes); } - std::array get_warning_code() const + std::vector get_warning_code() const { - return warning_signal_ - ->get_value>(); + return warning_signal_->get_value>(); } - void set_warning_message( - std::array error_message) + void set_warning_message(std::vector error_message) { warning_signal_message_->set_value(error_message); } - std::array get_warning_message() const + std::vector get_warning_message() const { - return warning_signal_message_ - ->get_value>(); + return warning_signal_message_->get_value>(); } protected: diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index fd33af006a..399e59f3fb 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -179,13 +179,15 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create error signal interface InterfaceInfo error_interface_info; error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; - error_interface_info.data_type = "array[32]"; + error_interface_info.data_type = "vector"; + error_interface_info.size = 32; InterfaceDescription error_interface_descr(info_.name, error_interface_info); error_signal_ = std::make_shared(error_interface_descr); // create error signal report message interface InterfaceInfo error_msg_interface_info; error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; - error_msg_interface_info.data_type = "array[32]"; + error_msg_interface_info.data_type = "vector"; + error_msg_interface_info.size = 32; InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); error_signal_message_ = std::make_shared(error_msg_interface_descr); @@ -193,13 +195,15 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create warning signal interface InterfaceInfo warning_interface_info; warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; - warning_interface_info.data_type = "array[32]"; + warning_interface_info.data_type = "vector"; + warning_interface_info.size = 32; InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); warning_signal_ = std::make_shared(warning_interface_descr); // create warning signal report message interface InterfaceInfo warning_msg_interface_info; warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; - warning_msg_interface_info.data_type = "array[32]"; + warning_msg_interface_info.data_type = "vector"; + warning_msg_interface_info.size = 32; InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } @@ -490,49 +494,41 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI bool get_emergency_stop() const { return emergency_stop_->get_value(); } - void set_error_code(std::array error_codes) - { - error_signal_->set_value(error_codes); - } + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } - std::array get_error_code() const + std::vector get_error_code() const { - return error_signal_->get_value>(); + return error_signal_->get_value>(); } - void set_error_message( - std::array error_messages) + void set_error_message(std::vector error_messages) { error_signal_message_->set_value(error_messages); } - std::array get_error_message() const + std::vector get_error_message() const { - return error_signal_message_ - ->get_value>(); + return error_signal_message_->get_value>(); } - void set_warning_code(std::array warning_codes) + void set_warning_code(std::vector warning_codes) { warning_signal_->set_value(warning_codes); } - std::array get_warning_code() const + std::vector get_warning_code() const { - return warning_signal_ - ->get_value>(); + return warning_signal_->get_value>(); } - void set_warning_message( - std::array error_message) + void set_warning_message(std::vector error_message) { warning_signal_message_->set_value(error_message); } - std::array get_warning_message() const + std::vector get_warning_message() const { - return warning_signal_message_ - ->get_value>(); + return warning_signal_message_->get_value>(); } protected: diff --git a/hardware_interface/include/hardware_interface/types/handle_datatype.hpp b/hardware_interface/include/hardware_interface/types/handle_datatype.hpp new file mode 100644 index 0000000000..6a4d32c3c1 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/handle_datatype.hpp @@ -0,0 +1,43 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HANDLE_DATATYPE_HPP_ +#define HARDWARE_INTERFACE__TYPES__HANDLE_DATATYPE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" + +namespace hardware_interface +{ + +using HANDLE_DATATYPE = + std::variant, std::vector, std::vector>; + +// 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>> +{ +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HANDLE_DATATYPE_HPP_ 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 88848a7ca8..b40011eb3a 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 @@ -24,85 +24,12 @@ 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; +using ERROR_SIGNALS = std::vector; constexpr char ERROR_SIGNAL_INTERFACE_NAME[] = "ERROR_SIGNAL"; -// Available error signal names -enum class error_signal : std::uint8_t -{ - ERROR_SIGNAL_0 = 0, - ERROR_SIGNAL_1 = 1, - ERROR_SIGNAL_2 = 2, - ERROR_SIGNAL_3 = 3, - ERROR_SIGNAL_4 = 4, - ERROR_SIGNAL_5 = 5, - ERROR_SIGNAL_6 = 6, - ERROR_SIGNAL_7 = 7, - ERROR_SIGNAL_8 = 8, - ERROR_SIGNAL_9 = 9, - ERROR_SIGNAL_10 = 10, - ERROR_SIGNAL_11 = 11, - ERROR_SIGNAL_12 = 12, - ERROR_SIGNAL_13 = 13, - ERROR_SIGNAL_14 = 14, - ERROR_SIGNAL_15 = 15, - ERROR_SIGNAL_16 = 16, - ERROR_SIGNAL_17 = 17, - ERROR_SIGNAL_18 = 18, - ERROR_SIGNAL_19 = 19, - ERROR_SIGNAL_20 = 20, - ERROR_SIGNAL_21 = 21, - ERROR_SIGNAL_22 = 22, - ERROR_SIGNAL_23 = 23, - ERROR_SIGNAL_24 = 24, - ERROR_SIGNAL_25 = 25, - ERROR_SIGNAL_26 = 26, - ERROR_SIGNAL_27 = 27, - ERROR_SIGNAL_28 = 28, - ERROR_SIGNAL_29 = 29, - ERROR_SIGNAL_30 = 30, - ERROR_SIGNAL_31 = 31 -}; - -using ERROR_MESSAGES = std::array; +using ERROR_MESSAGES = std::vector; constexpr char ERROR_SIGNAL_MESSAGE_INTERFACE_NAME[] = "ERROR_SIGNAL_MESSAGE"; -// Available WARNING signal message names -enum class error_signal_message : std::uint8_t -{ - ERROR_SIGNAL_MESSAGE_0 = 0, - ERROR_SIGNAL_MESSAGE_1 = 1, - ERROR_SIGNAL_MESSAGE_2 = 2, - ERROR_SIGNAL_MESSAGE_3 = 3, - ERROR_SIGNAL_MESSAGE_4 = 4, - ERROR_SIGNAL_MESSAGE_5 = 5, - ERROR_SIGNAL_MESSAGE_6 = 6, - ERROR_SIGNAL_MESSAGE_7 = 7, - ERROR_SIGNAL_MESSAGE_8 = 8, - ERROR_SIGNAL_MESSAGE_9 = 9, - ERROR_SIGNAL_MESSAGE_10 = 10, - ERROR_SIGNAL_MESSAGE_11 = 11, - ERROR_SIGNAL_MESSAGE_12 = 12, - ERROR_SIGNAL_MESSAGE_13 = 13, - ERROR_SIGNAL_MESSAGE_14 = 14, - ERROR_SIGNAL_MESSAGE_15 = 15, - ERROR_SIGNAL_MESSAGE_16 = 16, - ERROR_SIGNAL_MESSAGE_17 = 17, - ERROR_SIGNAL_MESSAGE_18 = 18, - ERROR_SIGNAL_MESSAGE_19 = 19, - ERROR_SIGNAL_MESSAGE_20 = 20, - ERROR_SIGNAL_MESSAGE_21 = 21, - ERROR_SIGNAL_MESSAGE_22 = 22, - ERROR_SIGNAL_MESSAGE_23 = 23, - ERROR_SIGNAL_MESSAGE_24 = 24, - ERROR_SIGNAL_MESSAGE_25 = 25, - ERROR_SIGNAL_MESSAGE_26 = 26, - ERROR_SIGNAL_MESSAGE_27 = 27, - ERROR_SIGNAL_MESSAGE_28 = 28, - ERROR_SIGNAL_MESSAGE_29 = 29, - ERROR_SIGNAL_MESSAGE_30 = 30, - ERROR_SIGNAL_MESSAGE_31 = 31 -}; } // namespace hardware_interface 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 b9ebcfdeb3..cf4b5a8c0e 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 @@ -23,85 +23,12 @@ 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; +using WARNING_SIGNALS = std::vector; constexpr char WARNING_SIGNAL_INTERFACE_NAME[] = "WARNING_SIGNAL"; -// Available warning signals names mapping to position in the interface -enum class warning_signal : std::uint8_t -{ - WARNING_SIGNAL_0 = 0, - WARNING_SIGNAL_1 = 1, - WARNING_SIGNAL_2 = 2, - WARNING_SIGNAL_3 = 3, - WARNING_SIGNAL_4 = 4, - WARNING_SIGNAL_5 = 5, - WARNING_SIGNAL_6 = 6, - WARNING_SIGNAL_7 = 7, - WARNING_SIGNAL_8 = 8, - WARNING_SIGNAL_9 = 9, - WARNING_SIGNAL_10 = 10, - WARNING_SIGNAL_11 = 11, - WARNING_SIGNAL_12 = 12, - WARNING_SIGNAL_13 = 13, - WARNING_SIGNAL_14 = 14, - WARNING_SIGNAL_15 = 15, - WARNING_SIGNAL_16 = 16, - WARNING_SIGNAL_17 = 17, - WARNING_SIGNAL_18 = 18, - WARNING_SIGNAL_19 = 19, - WARNING_SIGNAL_20 = 20, - WARNING_SIGNAL_21 = 21, - WARNING_SIGNAL_22 = 22, - WARNING_SIGNAL_23 = 23, - WARNING_SIGNAL_24 = 24, - WARNING_SIGNAL_25 = 25, - WARNING_SIGNAL_26 = 26, - WARNING_SIGNAL_27 = 27, - WARNING_SIGNAL_28 = 28, - WARNING_SIGNAL_29 = 29, - WARNING_SIGNAL_30 = 30, - WARNING_SIGNAL_31 = 31 -}; - -using WARNING_MESSAGES = std::array; +using WARNING_MESSAGES = std::vector; constexpr char WARNING_SIGNAL_MESSAGE_INTERFACE_NAME[] = "WARNING_SIGNAL_MESSAGE"; -// Available WARNING signal message names -enum class warning_signal_message : std::uint8_t -{ - WARNING_SIGNAL_MESSAGE_0 = 0, - WARNING_SIGNAL_MESSAGE_1 = 1, - WARNING_SIGNAL_MESSAGE_2 = 2, - WARNING_SIGNAL_MESSAGE_3 = 3, - WARNING_SIGNAL_MESSAGE_4 = 4, - WARNING_SIGNAL_MESSAGE_5 = 5, - WARNING_SIGNAL_MESSAGE_6 = 6, - WARNING_SIGNAL_MESSAGE_7 = 7, - WARNING_SIGNAL_MESSAGE_8 = 8, - WARNING_SIGNAL_MESSAGE_9 = 9, - WARNING_SIGNAL_MESSAGE_10 = 10, - WARNING_SIGNAL_MESSAGE_11 = 11, - WARNING_SIGNAL_MESSAGE_12 = 12, - WARNING_SIGNAL_MESSAGE_13 = 13, - WARNING_SIGNAL_MESSAGE_14 = 14, - WARNING_SIGNAL_MESSAGE_15 = 15, - WARNING_SIGNAL_MESSAGE_16 = 16, - WARNING_SIGNAL_MESSAGE_17 = 17, - WARNING_SIGNAL_MESSAGE_18 = 18, - WARNING_SIGNAL_MESSAGE_19 = 19, - WARNING_SIGNAL_MESSAGE_20 = 20, - WARNING_SIGNAL_MESSAGE_21 = 21, - WARNING_SIGNAL_MESSAGE_22 = 22, - WARNING_SIGNAL_MESSAGE_23 = 23, - WARNING_SIGNAL_MESSAGE_24 = 24, - WARNING_SIGNAL_MESSAGE_25 = 25, - WARNING_SIGNAL_MESSAGE_26 = 26, - WARNING_SIGNAL_MESSAGE_27 = 27, - WARNING_SIGNAL_MESSAGE_28 = 28, - WARNING_SIGNAL_MESSAGE_29 = 29, - WARNING_SIGNAL_MESSAGE_30 = 30, - WARNING_SIGNAL_MESSAGE_31 = 31 -}; } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index e836ed3427..f995d5d4d2 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -58,115 +58,6 @@ namespace test_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -// BEGIN (Handle export change): for backward compatibility -class DummyActuator : public hardware_interface::ActuatorInterface -{ - CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override - { - // We hardcode the info - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - position_state_ = 0.0; - velocity_state_ = 0.0; - - if (recoverable_error_happened_) - { - velocity_command_ = 0.0; - } - - read_calls_ = 0; - write_calls_ = 0; - - return CallbackReturn::SUCCESS; - } - - std::vector export_state_interfaces() override - { - // We can read a position and a velocity - std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_POSITION, &position_state_)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_state_)); - - return state_interfaces; - } - - std::vector export_command_interfaces() override - { - // We can command in velocity - std::vector command_interfaces; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_command_)); - - return command_interfaces; - } - - std::string get_name() const override { return "DummyActuator"; } - - hardware_interface::return_type read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++read_calls_; - if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - // no-op, state is getting propagated within write. - return hardware_interface::return_type::OK; - } - - hardware_interface::return_type write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++write_calls_; - if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - position_state_ += velocity_command_; - velocity_state_ = velocity_command_; - - return hardware_interface::return_type::OK; - } - - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override - { - velocity_state_ = 0; - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override - { - if (!recoverable_error_happened_) - { - recoverable_error_happened_ = true; - return CallbackReturn::SUCCESS; - } - else - { - return CallbackReturn::ERROR; - } - return CallbackReturn::FAILURE; - } - -private: - double position_state_ = std::numeric_limits::quiet_NaN(); - double velocity_state_ = std::numeric_limits::quiet_NaN(); - double velocity_command_ = 0.0; - - // Helper variables to initiate error on read - unsigned int read_calls_ = 0; - unsigned int write_calls_ = 0; - bool recoverable_error_happened_ = false; -}; -// END - class DummyActuatorDefault : public hardware_interface::ActuatorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override @@ -254,72 +145,6 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface bool recoverable_error_happened_ = false; }; -// BEGIN (Handle export change): for backward compatibility -class DummySensor : public hardware_interface::SensorInterface -{ - CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override - { - // We hardcode the info - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - voltage_level_ = 0.0; - read_calls_ = 0; - return CallbackReturn::SUCCESS; - } - - std::vector export_state_interfaces() override - { - // We can read some voltage level - std::vector state_interfaces; - state_interfaces.emplace_back( - hardware_interface::StateInterface("joint1", "voltage", &voltage_level_)); - - return state_interfaces; - } - - std::string get_name() const override { return "DummySensor"; } - - hardware_interface::return_type read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++read_calls_; - if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - // no-op, static value - voltage_level_ = voltage_level_hw_value_; - return hardware_interface::return_type::OK; - } - - CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override - { - if (!recoverable_error_happened_) - { - recoverable_error_happened_ = true; - return CallbackReturn::SUCCESS; - } - else - { - return CallbackReturn::ERROR; - } - return CallbackReturn::FAILURE; - } - -private: - double voltage_level_ = std::numeric_limits::quiet_NaN(); - double voltage_level_hw_value_ = 0x666; - - // Helper variables to initiate error on read - int read_calls_ = 0; - bool recoverable_error_happened_ = false; -}; -// END - class DummySensorDefault : public hardware_interface::SensorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override @@ -382,143 +207,6 @@ class DummySensorDefault : public hardware_interface::SensorInterface int read_calls_ = 0; bool recoverable_error_happened_ = false; }; - -// BEGIN (Handle export change): for backward compatibility -class DummySystem : public hardware_interface::SystemInterface -{ - CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override - { - // We hardcode the info - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - for (auto i = 0ul; i < 3; ++i) - { - position_state_[i] = 0.0; - velocity_state_[i] = 0.0; - } - // reset command only if error is initiated - if (recoverable_error_happened_) - { - for (auto i = 0ul; i < 3; ++i) - { - velocity_command_[i] = 0.0; - } - } - - read_calls_ = 0; - write_calls_ = 0; - - return CallbackReturn::SUCCESS; - } - - std::vector export_state_interfaces() override - { - // We can read a position and a velocity - std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_POSITION, &position_state_[0])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_state_[0])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint2", hardware_interface::HW_IF_POSITION, &position_state_[1])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint2", hardware_interface::HW_IF_VELOCITY, &velocity_state_[1])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint3", hardware_interface::HW_IF_POSITION, &position_state_[2])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint3", hardware_interface::HW_IF_VELOCITY, &velocity_state_[2])); - - return state_interfaces; - } - - std::vector export_command_interfaces() override - { - // We can command in velocity - std::vector command_interfaces; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_command_[0])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "joint2", hardware_interface::HW_IF_VELOCITY, &velocity_command_[1])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "joint3", hardware_interface::HW_IF_VELOCITY, &velocity_command_[2])); - - return command_interfaces; - } - - std::string get_name() const override { return "DummySystem"; } - - hardware_interface::return_type read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++read_calls_; - if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - // no-op, state is getting propagated within write. - return hardware_interface::return_type::OK; - } - - hardware_interface::return_type write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++write_calls_; - if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - for (auto i = 0; i < 3; ++i) - { - position_state_[i] += velocity_command_[0]; - velocity_state_[i] = velocity_command_[0]; - } - return hardware_interface::return_type::OK; - } - - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override - { - for (auto i = 0ul; i < 3; ++i) - { - velocity_state_[i] = 0.0; - } - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override - { - if (!recoverable_error_happened_) - { - recoverable_error_happened_ = true; - return CallbackReturn::SUCCESS; - } - else - { - return CallbackReturn::ERROR; - } - return CallbackReturn::FAILURE; - } - -private: - std::array position_state_ = { - {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}}; - std::array velocity_state_ = { - {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}}; - std::array velocity_command_ = {{0.0, 0.0, 0.0}}; - - // Helper variables to initiate error on read - unsigned int read_calls_ = 0; - unsigned int write_calls_ = 0; - bool recoverable_error_happened_ = false; -}; -// END - class DummySystemDefault : public hardware_interface::SystemInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override @@ -683,134 +371,40 @@ class DummySystemPreparePerform : public hardware_interface::SystemInterface }; } // namespace test_components - -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_actuator) +TEST(TestComponentInterfaces, dummy_actuator_default) { - hardware_interface::Actuator actuator_hw(std::make_unique()); + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = actuator_hw.initialize(mock_hw_info); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = actuator_hw.export_state_interfaces(); - ASSERT_EQ(2u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); - - auto command_interfaces = actuator_hw.export_command_interfaces(); - ASSERT_EQ(1u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); - - double velocity_value = 1.0; - command_interfaces[0]->set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Noting should change because it is UNCONFIGURED - for (auto step = 0u; step < 10; ++step) - { - 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_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - - state = actuator_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - - // Read and Write are working because it is INACTIVE - for (auto step = 0u; step < 10; ++step) - { - 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 - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - // Read and Write are working because it is ACTIVE - for (auto step = 0u; step < 10; ++step) - { - 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 - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - - state = actuator_hw.shutdown(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // Noting should change because it is FINALIZED - for (auto step = 0u; step < 10; ++step) - { - 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 - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - - EXPECT_EQ( - hardware_interface::return_type::OK, actuator_hw.prepare_command_mode_switch({""}, {""})); - EXPECT_EQ( - hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); -} -// END - -TEST(TestComponentInterfaces, dummy_actuator_default) -{ - hardware_interface::Actuator actuator_hw( - std::make_unique()); - const std::string urdf_to_test = - std::string(ros2_control_test_assets::urdf_head) + - ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + - ros2_control_test_assets::urdf_tail; - const std::vector control_resources = - hardware_interface::parse_control_resources_from_urdf(urdf_to_test); - const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - auto state = actuator_hw.initialize(dummy_actuator); - - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = actuator_hw.export_state_interfaces(); - ASSERT_EQ(2u + report_signals_size, state_interfaces.size()); - { - auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/position"); - EXPECT_TRUE(contains); - EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - } - { - auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/velocity"); - EXPECT_TRUE(contains); - EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - } + ASSERT_EQ(2u + report_signals_size, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } auto command_interfaces = actuator_hw.export_command_interfaces(); ASSERT_EQ(1u, command_interfaces.size()); @@ -836,7 +430,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())); // position value + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); @@ -852,8 +447,10 @@ TEST(TestComponentInterfaces, dummy_actuator_default) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + step * velocity_value, + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -869,7 +466,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint1_pos]->get_value()); // position value + state_interfaces[si_joint1_pos]->get_value()); // position value EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); @@ -884,8 +481,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -896,39 +494,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default) hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_sensor) -{ - hardware_interface::Sensor sensor_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = sensor_hw.initialize(mock_hw_info); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = sensor_hw.export_state_interfaces(); - ASSERT_EQ(1u, state_interfaces.size()); - 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())); - - // Not updated because is is UNCONFIGURED - sensor_hw.read(TIME, PERIOD); - 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()); - - // It can read now - sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[0]->get_value()); -} -// END - TEST(TestComponentInterfaces, dummy_sensor_default) { hardware_interface::Sensor sensor_hw(std::make_unique()); @@ -972,135 +537,6 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ(0x666, state_interfaces[si_joint1_vol]->get_value()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_system) -{ - hardware_interface::System system_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = system_hw.export_state_interfaces(); - ASSERT_EQ(6u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); - EXPECT_EQ("joint2/position", state_interfaces[2]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2]->get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[2]->get_prefix_name()); - EXPECT_EQ("joint2/velocity", state_interfaces[3]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3]->get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[3]->get_prefix_name()); - EXPECT_EQ("joint3/position", state_interfaces[4]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4]->get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[4]->get_prefix_name()); - EXPECT_EQ("joint3/velocity", state_interfaces[5]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5]->get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[5]->get_prefix_name()); - - auto command_interfaces = system_hw.export_command_interfaces(); - ASSERT_EQ(3u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint2/velocity", command_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint2", command_interfaces[1]->get_prefix_name()); - EXPECT_EQ("joint3/velocity", command_interfaces[2]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2]->get_interface_name()); - EXPECT_EQ("joint3", command_interfaces[2]->get_prefix_name()); - - double velocity_value = 1.0; - command_interfaces[0]->set_value(velocity_value); // velocity - command_interfaces[1]->set_value(velocity_value); // velocity - command_interfaces[2]->set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Noting should change because it is UNCONFIGURED - for (auto step = 0u; step < 10; ++step) - { - 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_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - - state = system_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - - // Read and Write are working because it is INACTIVE - for (auto step = 0u; step < 10; ++step) - { - 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 - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - // Read and Write are working because it is ACTIVE - for (auto step = 0u; step < 10; ++step) - { - 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 - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - - state = system_hw.shutdown(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // Noting should change because it is FINALIZED - for (auto step = 0u; step < 10; ++step) - { - 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 - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - - EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); - EXPECT_EQ(hardware_interface::return_type::OK, system_hw.perform_command_mode_switch({}, {})); -} -// END - TEST(TestComponentInterfaces, dummy_system_default) { hardware_interface::System system_hw(std::make_unique()); @@ -1220,11 +656,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())); // position value + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_pos]->get_value())); // position value + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint2_pos]->get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_vel]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_pos]->get_value())); // position value + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint3_pos]->get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_vel]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); @@ -1240,14 +679,20 @@ TEST(TestComponentInterfaces, dummy_system_default) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + step * velocity_value, + state_interfaces[si_joint1_pos]->get_value()); // position value EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint2_vel]->get_value()); // velocity + step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint3_vel]->get_value()); // velocity + step * velocity_value, + state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, + state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1263,15 +708,15 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint1_pos]->get_value()); // position value + state_interfaces[si_joint1_pos]->get_value()); // position value EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint2_pos]->get_value()); // position value + state_interfaces[si_joint2_pos]->get_value()); // position value EXPECT_EQ(velocity_value, state_interfaces[si_joint2_vel]->get_value()); // velocity EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint3_pos]->get_value()); // position value + state_interfaces[si_joint3_pos]->get_value()); // position value EXPECT_EQ(velocity_value, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); @@ -1286,12 +731,15 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1331,67 +779,6 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) system_hw.perform_command_mode_switch(two_keys, one_key)); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) -{ - hardware_interface::Actuator actuator_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = actuator_hw.initialize(mock_hw_info); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = actuator_hw.export_state_interfaces(); - auto command_interfaces = actuator_hw.export_command_interfaces(); - state = actuator_hw.configure(); - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is first time therefore recoverable) - 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(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - - state = actuator_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // 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); - - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is the second time therefore unrecoverable) - 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(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - - state = actuator_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = actuator_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) { hardware_interface::Actuator actuator_hw( @@ -1462,67 +849,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) -{ - hardware_interface::Actuator actuator_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = actuator_hw.initialize(mock_hw_info); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = actuator_hw.export_state_interfaces(); - auto command_interfaces = actuator_hw.export_command_interfaces(); - state = actuator_hw.configure(); - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is first time therefore recoverable) - 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(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - - state = actuator_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // 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); - - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is the second time therefore unrecoverable) - 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(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - - state = actuator_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = actuator_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) { hardware_interface::Actuator actuator_hw( @@ -1592,72 +918,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) -{ - hardware_interface::Sensor sensor_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = sensor_hw.initialize(mock_hw_info); - - auto state_interfaces = sensor_hw.export_state_interfaces(); - // Updated because is is INACTIVE - state = sensor_hw.configure(); - state = sensor_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - - // Initiate recoverable error - call read 99 times OK and on 100-time will return error - 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(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - - state = sensor_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // Noting should change because it is UNCONFIGURED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - } - - // activate again and expect reset values - state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - - state = sensor_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error - for (auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - - state = sensor_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // Noting should change because it is FINALIZED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - } - - // can not change state anymore - state = sensor_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) { hardware_interface::Sensor sensor_hw(std::make_unique()); @@ -1717,72 +977,6 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_system_read_error_behavior) -{ - hardware_interface::System system_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = system_hw.export_state_interfaces(); - auto command_interfaces = system_hw.export_command_interfaces(); - state = system_hw.configure(); - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is first time therefore recoverable) - 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(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - - state = system_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // activate again and expect reset values - state = system_hw.configure(); - for (auto index = 0ul; index < 6; ++index) - { - 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); - } - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is the second time therefore unrecoverable) - 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(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - - state = system_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = system_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) { hardware_interface::System system_hw(std::make_unique()); @@ -1853,72 +1047,6 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_system_write_error_behavior) -{ - hardware_interface::System system_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = system_hw.export_state_interfaces(); - auto command_interfaces = system_hw.export_command_interfaces(); - state = system_hw.configure(); - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is first time therefore recoverable) - 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(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - - state = system_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // activate again and expect reset values - state = system_hw.configure(); - for (auto index = 0ul; index < 6; ++index) - { - 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); - } - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is the second time therefore unrecoverable) - 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(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - - state = system_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = system_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) { hardware_interface::System system_hw(std::make_unique()); diff --git a/hardware_interface/test/test_error_warning_codes.cpp b/hardware_interface/test/test_error_warning_codes.cpp index d04bbcbab3..3b7a66f9ac 100644 --- a/hardware_interface/test/test_error_warning_codes.cpp +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -52,6 +52,17 @@ const auto warnig_signals_size = 2; const auto error_signals_size = 2; const auto report_signals_size = emergency_stop_signal_size + warnig_signals_size + error_signals_size; + +const hardware_interface::WARNING_MESSAGES empty_error_msg_vec(32, ""); +const hardware_interface::ERROR_MESSAGES empty_warn_msg_vec(32, ""); +const hardware_interface::ERROR_SIGNALS empty_error_code_vec(32, 0); +const hardware_interface::WARNING_SIGNALS empty_warn_code_vec(32, 0); +const hardware_interface::WARNING_MESSAGES error_msg_vec{ + "Some", "error", "msgs", "so vector is", "not empty."}; +const hardware_interface::ERROR_MESSAGES warn_msg_vec{"Some", "warning", "warn", + "msgs", "so vector is", "not empty."}; +const hardware_interface::ERROR_SIGNALS error_code_vec{1, 2, 3, 4, 5, 6, 7}; +const hardware_interface::WARNING_SIGNALS warn_code_vec{1, 2, 3, 4, 5, 6, 7}; } // namespace using namespace ::testing; // NOLINT @@ -79,10 +90,10 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface set_state("joint1/position", 0.0); set_state("joint1/velocity", 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{}); + set_error_code(empty_error_code_vec); + set_error_message(empty_error_msg_vec); + set_warning_code(empty_warn_code_vec); + set_warning_message(empty_warn_msg_vec); if (recoverable_error_happened_) { @@ -104,16 +115,10 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { 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); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } @@ -128,16 +133,10 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { 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); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } auto position_state = get_state("joint1/position"); @@ -195,10 +194,10 @@ class DummySensorDefault : public hardware_interface::SensorInterface { set_state(name, 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{}); + set_error_code(empty_error_code_vec); + set_error_message(empty_error_msg_vec); + set_warning_code(empty_warn_code_vec); + set_warning_message(empty_warn_msg_vec); read_calls_ = 0; return CallbackReturn::SUCCESS; } @@ -211,16 +210,10 @@ class DummySensorDefault : public hardware_interface::SensorInterface ++read_calls_; if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - 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); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } @@ -273,10 +266,10 @@ class DummySystemDefault : public hardware_interface::SystemInterface set_state(velocity_states_[i], 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{}); + set_error_code(empty_error_code_vec); + set_error_message(empty_error_msg_vec); + set_warning_code(empty_warn_code_vec); + set_warning_message(empty_warn_msg_vec); // reset command only if error is initiated if (recoverable_error_happened_) { @@ -301,16 +294,10 @@ class DummySystemDefault : public hardware_interface::SystemInterface if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { 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); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } @@ -325,16 +312,10 @@ class DummySystemDefault : public hardware_interface::SystemInterface if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { 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); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } @@ -813,18 +794,26 @@ 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[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -849,18 +838,26 @@ 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[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -951,18 +948,26 @@ 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[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -1021,16 +1026,24 @@ 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[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = sensor_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -1116,18 +1129,26 @@ 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[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -1154,18 +1175,26 @@ 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[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -1232,18 +1261,26 @@ 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[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index aea7bbb04c..52007fde5a 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -13,8 +13,12 @@ // limitations under the License. #include +#include + #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" using hardware_interface::CommandInterface; using hardware_interface::InterfaceDescription; @@ -27,21 +31,510 @@ constexpr auto JOINT_NAME = "joint_1"; constexpr auto FOO_INTERFACE = "FooInterface"; } // namespace -TEST(TestHandle, command_interface) +TEST(TestHandle, ci_empty_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface.get_value(), false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); +} + +TEST(TestHandle, ci_true_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + info.initial_value = "true"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface.get_value(), true); + EXPECT_NO_THROW(interface.set_value(false)); + EXPECT_EQ(interface.get_value(), false); +} + +TEST(TestHandle, ci_empty_double_initialization) { - double value = 1.337; - CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(interface.get_value(), value); + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_TRUE(std::isnan(interface.get_value())); + EXPECT_NO_THROW(interface.set_value(1.5)); + EXPECT_EQ(interface.get_value(), 1.5); +} + +TEST(TestHandle, ci_pos_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface.get_value(), 1.5); EXPECT_NO_THROW(interface.set_value(0.0)); - EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); + EXPECT_EQ(interface.get_value(), 0.0); } -TEST(TestHandle, state_interface) +TEST(TestHandle, ci_negative_double_initialization) { - double value = 1.337; - StateInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(interface.get_value(), value); - // interface.set_value(5); compiler error, no set_value function + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "-1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface.get_value(), -1.5); + EXPECT_NO_THROW(interface.set_value(0.0)); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, ci_invalid_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "abc"; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::invalid_argument); +} + +TEST(TestHandle, ci_int8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_int8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, ci_int8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_uint8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_uint8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, ci_uint8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_string_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_string_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, ci_string_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector empty_str_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface.get_value>(), empty_str_vector); + empty_str_vector[1] = "Warning message number 1"; + empty_str_vector[2] = "Warn msg no. 2"; + empty_str_vector[4] = "message no. 3"; + empty_str_vector[8] = "Warning message no. 4"; + + EXPECT_NO_THROW(interface.set_value(empty_str_vector)); + EXPECT_EQ(interface.get_value>(), empty_str_vector); +} + +TEST(TestHandle, ci_throw_on_get_wrong_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + EXPECT_THROW(interface.get_value(), std::bad_variant_access); +} + +TEST(TestHandle, ci_throw_on_not_know_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "banana"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr};, std::runtime_error); +} + +TEST(TestHandle, ci_throw_on_empty_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_TRUE(std::isnan(interface.get_value())); +} + +TEST(TestHandle, si_empty_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface.get_value(), false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); +} + +TEST(TestHandle, si_true_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + info.initial_value = "true"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface.get_value(), true); + EXPECT_NO_THROW(interface.set_value(false)); + EXPECT_EQ(interface.get_value(), false); +} + +TEST(TestHandle, si_empty_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_TRUE(std::isnan(interface.get_value())); + EXPECT_NO_THROW(interface.set_value(1.5)); + EXPECT_EQ(interface.get_value(), 1.5); +} + +TEST(TestHandle, si_pos_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface.get_value(), 1.5); + EXPECT_NO_THROW(interface.set_value(0.0)); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, si_negative_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "-1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface.get_value(), -1.5); + EXPECT_NO_THROW(interface.set_value(0.0)); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, si_invalid_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "abc"; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::invalid_argument); +} + +TEST(TestHandle, si_int8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_int8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, si_int8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_uint8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_uint8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, si_uint8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_string_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_string_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, si_string_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector empty_str_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface.get_value>(), empty_str_vector); + empty_str_vector[1] = "Warning message number 1"; + empty_str_vector[2] = "Warn msg no. 2"; + empty_str_vector[4] = "message no. 3"; + empty_str_vector[8] = "Warning message no. 4"; + + EXPECT_NO_THROW(interface.set_value(empty_str_vector)); + EXPECT_EQ(interface.get_value>(), empty_str_vector); +} + +TEST(TestHandle, si_throw_on_get_wrong_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + EXPECT_THROW(interface.get_value(), std::bad_variant_access); +} + +TEST(TestHandle, si_throw_on_not_know_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "banana"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr};, std::runtime_error); +} + +TEST(TestHandle, si_throw_on_empty_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_TRUE(std::isnan(interface.get_value())); } TEST(TestHandle, name_getters_work) @@ -52,20 +545,22 @@ TEST(TestHandle, name_getters_work) EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME); } -TEST(TestHandle, value_methods_throw_for_nullptr) +TEST(TestHandle, ci_value_methods) { CommandInterface handle{JOINT_NAME, FOO_INTERFACE}; - EXPECT_ANY_THROW(handle.get_value()); - EXPECT_ANY_THROW(handle.set_value(0.0)); + EXPECT_NO_THROW(handle.get_value()); + EXPECT_TRUE(std::isnan(handle.get_value())); + EXPECT_NO_THROW(handle.set_value(0.0)); + EXPECT_EQ(handle.get_value(), 0.0); } -TEST(TestHandle, value_methods_work_on_non_nullptr) +TEST(TestHandle, si_value_methods) { - double value = 1.337; - CommandInterface handle{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(handle.get_value(), value); + StateInterface handle{JOINT_NAME, FOO_INTERFACE}; + EXPECT_NO_THROW(handle.get_value()); + EXPECT_TRUE(std::isnan(handle.get_value())); EXPECT_NO_THROW(handle.set_value(0.0)); - EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); + EXPECT_EQ(handle.get_value(), 0.0); } TEST(TestHandle, interface_description_state_interface_name_getters_work) @@ -74,12 +569,13 @@ TEST(TestHandle, interface_description_state_interface_name_getters_work) const std::string JOINT_NAME_1 = "joint1"; InterfaceInfo info; info.name = POSITION_INTERFACE; + info.data_type = "double"; InterfaceDescription interface_descr(JOINT_NAME_1, info); StateInterface handle{interface_descr}; EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); - EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); + (handle.get_prefix_name(), JOINT_NAME_1); } TEST(TestHandle, interface_description_command_interface_name_getters_work) @@ -87,6 +583,7 @@ TEST(TestHandle, interface_description_command_interface_name_getters_work) const std::string POSITION_INTERFACE = "position"; const std::string JOINT_NAME_1 = "joint1"; InterfaceInfo info; + info.data_type = "double"; info.name = POSITION_INTERFACE; InterfaceDescription interface_descr(JOINT_NAME_1, info); CommandInterface handle{interface_descr};