From a7996c356854887dd6df49f94237465d3137ff7a Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 11 Jan 2024 11:52:13 +0100 Subject: [PATCH] add interface for warning, error and report --- hardware_interface/CMakeLists.txt | 4 + .../hardware_interface/actuator_interface.hpp | 94 ++ .../include/hardware_interface/handle.hpp | 4 +- .../hardware_interface/sensor_interface.hpp | 76 ++ .../hardware_interface/system_interface.hpp | 94 ++ ...rdware_interface_emergency_stop_signal.hpp | 29 + .../hardware_interface_error_signals.hpp | 105 ++ .../hardware_interface_warning_signals.hpp | 104 ++ .../test/test_component_interfaces.cpp | 11 +- ...est_component_interfaces_custom_export.cpp | 40 +- .../test/test_error_warning_codes.cpp | 981 ++++++++++++++++++ 11 files changed, 1523 insertions(+), 19 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp create mode 100644 hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp create mode 100644 hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp create mode 100644 hardware_interface/test/test_error_warning_codes.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 39197cd659..d6083b8bd4 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -80,6 +80,10 @@ if(BUILD_TESTING) target_link_libraries(test_component_interfaces_custom_export hardware_interface) ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets) + ament_add_gmock(test_error_warning_codes test/test_error_warning_codes.cpp) + target_link_libraries(test_error_warning_codes hardware_interface) + ament_target_dependencies(test_error_warning_codes ros2_control_test_assets) + ament_add_gmock(test_component_parser test/test_component_parser.cpp) target_link_libraries(test_component_parser hardware_interface) ament_target_dependencies(test_component_parser ros2_control_test_assets) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index c526c0a4b6..2a7f81dc9a 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -26,7 +26,10 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" @@ -104,6 +107,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod info_ = hardware_info; import_state_interface_descriptions(info_); import_command_interface_descriptions(info_); + create_report_interfaces(); return CallbackReturn::SUCCESS; }; @@ -135,6 +139,52 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod } } + /** + * Creates all interfaces used for reporting emergency stop, warning and error messages. + * The available report interfaces are: EMERGENCY_STOP_SIGNAL, ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE, + * WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the _MESSAGE hold the message for + * the corresponding report signal. + * The interfaces are named like /. E.g. if hardware is + * called joint_1 -> interface for WARNING_SIGNAL is called: joint_1/WARNING_SIGNAL + */ + void create_report_interfaces() + { + // EMERGENCY STOP + InterfaceInfo emergency_interface_info; + emergency_interface_info.name = hardware_interface::EMERGENCY_STOP_SIGNAL; + emergency_interface_info.data_type = "bool"; + InterfaceDescription emergency_interface_descr(info_.name, emergency_interface_info); + emergency_stop_ = std::make_shared(emergency_interface_descr); + + // ERROR + // create error signal interface + InterfaceInfo error_interface_info; + error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; + error_interface_info.data_type = "std::array"; + 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 = "std::array"; + InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); + error_signal_message_ = std::make_shared(error_msg_interface_descr); + + // WARNING + // create warning signal interface + InterfaceInfo warning_interface_info; + warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; + warning_interface_info.data_type = "std::array"; + 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 = "std::array"; + InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); + warning_signal_message_ = std::make_shared(warning_msg_interface_descr); + } + /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then @@ -177,6 +227,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod actuator_states_.insert(std::make_pair(name, state_interface)); state_interfaces.push_back(state_interface); } + + // export warning signal interfaces + state_interfaces.push_back(emergency_stop_); + state_interfaces.push_back(error_signal_); + state_interfaces.push_back(error_signal_message_); + state_interfaces.push_back(warning_signal_); + state_interfaces.push_back(warning_signal_message_); + return state_interfaces; } @@ -325,6 +383,35 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod return actuator_commands_.at(interface_name)->get_value(); } + void set_emergency_stop(const double & emergency_stop) + { + emergency_stop_->set_value(emergency_stop); + } + + double get_emergency_stop() const { return emergency_stop_->get_value(); } + + void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + + double get_error_code() const { return error_signal_->get_value(); } + + void set_error_message(const double & error_message) + { + error_signal_message_->set_value(error_message); + } + + double get_error_message() const { return error_signal_message_->get_value(); } + + void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + + double get_warning_code() const { return warning_signal_->get_value(); } + + void set_warning_message(const double & error_message) + { + warning_signal_message_->set_value(error_message); + } + + double get_warning_message() const { return warning_signal_message_->get_value(); } + protected: HardwareInfo info_; std::map joint_state_interfaces_; @@ -333,6 +420,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::unordered_map actuator_states_; std::unordered_map actuator_commands_; +private: + std::shared_ptr emergency_stop_; + std::shared_ptr error_signal_; + std::shared_ptr error_signal_message_; + std::shared_ptr warning_signal_; + std::shared_ptr warning_signal_message_; + rclcpp_lifecycle::State lifecycle_state_; }; diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 7d51505777..476147ddc7 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include #include #include #include @@ -23,8 +24,9 @@ #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 { diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index b3e30d499c..701c00adea 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -26,7 +26,9 @@ #include "hardware_interface/component_parser.hpp" #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_return_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" @@ -103,6 +105,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { info_ = hardware_info; import_state_interface_descriptions(info_); + create_report_interfaces(); return CallbackReturn::SUCCESS; }; @@ -120,6 +123,45 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI } } + /** + * Creates all interfaces used for reporting warning and error messages. + * The available report interfaces are: ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE, + * WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the _MESSAGE hold the message for + * the corresponding report signal. + * The interfaces are named like /. E.g. if hardware is + * called sensor_1 -> interface for WARNING_SIGNAL is called: sensor_1/WARNING_SIGNAL + */ + void create_report_interfaces() + { + // ERROR + // create error signal interface + InterfaceInfo error_interface_info; + error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; + error_interface_info.data_type = "std::array"; + 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 = "std::array"; + InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); + error_signal_message_ = std::make_shared(error_msg_interface_descr); + + // WARNING + // create warning signal interface + InterfaceInfo warning_interface_info; + warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; + warning_interface_info.data_type = "std::array"; + 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 = "std::array"; + InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); + warning_signal_message_ = std::make_shared(warning_msg_interface_descr); + } + /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then @@ -164,6 +206,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI state_interfaces.push_back(state_interface); } + // export warning signal interfaces + state_interfaces.push_back(error_signal_); + state_interfaces.push_back(error_signal_message_); + state_interfaces.push_back(warning_signal_); + state_interfaces.push_back(warning_signal_message_); + return state_interfaces; } @@ -207,6 +255,28 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return sensor_states_.at(interface_name)->get_value(); } + void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + + double get_error_code() const { return error_signal_->get_value(); } + + void set_error_message(const double & error_message) + { + error_signal_message_->set_value(error_message); + } + + double get_error_message() const { return error_signal_message_->get_value(); } + + void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + + double get_warning_code() const { return warning_signal_->get_value(); } + + void set_warning_message(const double & error_message) + { + warning_signal_message_->set_value(error_message); + } + + double get_warning_message() const { return warning_signal_message_->get_value(); } + protected: HardwareInfo info_; @@ -214,6 +284,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map sensor_states_; +private: + std::shared_ptr error_signal_; + std::shared_ptr error_signal_message_; + std::shared_ptr warning_signal_; + std::shared_ptr warning_signal_message_; + rclcpp_lifecycle::State lifecycle_state_; }; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index ff3353fb97..51278339e0 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -26,8 +26,11 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" @@ -106,6 +109,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI info_ = hardware_info; import_state_interface_descriptions(info_); import_command_interface_descriptions(info_); + create_report_interfaces(); return CallbackReturn::SUCCESS; }; @@ -155,6 +159,52 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI } } + /** + * Creates all interfaces used for reporting emergency stop, warning and error messages. + * The available report interfaces are: EMERGENCY_STOP_SIGNAL, ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE, + * WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the _MESSAGE hold the message for + * the corresponding report signal. + * The interfaces are named like /. E.g. if hardware is + * called rrbot -> interface for WARNING_SIGNAL is called: rrbot/WARNING_SIGNAL + */ + void create_report_interfaces() + { + // EMERGENCY STOP + InterfaceInfo emergency_interface_info; + emergency_interface_info.name = hardware_interface::EMERGENCY_STOP_SIGNAL; + emergency_interface_info.data_type = "bool"; + InterfaceDescription emergency_interface_descr(info_.name, emergency_interface_info); + emergency_stop_ = std::make_shared(emergency_interface_descr); + + // ERROR + // create error signal interface + InterfaceInfo error_interface_info; + error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; + error_interface_info.data_type = "std::array"; + 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 = "std::array"; + InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); + error_signal_message_ = std::make_shared(error_msg_interface_descr); + + // WARNING + // create warning signal interface + InterfaceInfo warning_interface_info; + warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; + warning_interface_info.data_type = "std::array"; + 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 = "std::array"; + InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); + warning_signal_message_ = std::make_shared(warning_msg_interface_descr); + } + /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then @@ -210,6 +260,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI system_states_.insert(std::make_pair(name, state_interface)); state_interfaces.push_back(state_interface); } + + // export warning signal interfaces + state_interfaces.push_back(emergency_stop_); + state_interfaces.push_back(error_signal_); + state_interfaces.push_back(error_signal_message_); + state_interfaces.push_back(warning_signal_); + state_interfaces.push_back(warning_signal_message_); + return state_interfaces; } @@ -365,6 +423,35 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return system_commands_.at(interface_name)->get_value(); } + void set_emergency_stop(const double & emergency_stop) + { + emergency_stop_->set_value(emergency_stop); + } + + double get_emergency_stop() const { return emergency_stop_->get_value(); } + + void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + + double get_error_code() const { return error_signal_->get_value(); } + + void set_error_message(const double & error_message) + { + error_signal_message_->set_value(error_message); + } + + double get_error_message() const { return error_signal_message_->get_value(); } + + void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + + double get_warning_code() const { return warning_signal_->get_value(); } + + void set_warning_message(const double & error_message) + { + warning_signal_message_->set_value(error_message); + } + + double get_warning_message() const { return warning_signal_message_->get_value(); } + protected: HardwareInfo info_; std::map joint_state_interfaces_; @@ -378,6 +465,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map system_states_; std::unordered_map system_commands_; +private: + std::shared_ptr emergency_stop_; + std::shared_ptr error_signal_; + std::shared_ptr error_signal_message_; + std::shared_ptr warning_signal_; + std::shared_ptr warning_signal_message_; + rclcpp_lifecycle::State lifecycle_state_; }; diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp new file mode 100644 index 0000000000..1478556f5f --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp @@ -0,0 +1,29 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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__HARDWARE_INTERFACE_EMERGENCY_STOP_SIGNAL_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_EMERGENCY_STOP_SIGNAL_HPP_ + +#include +#include + +namespace hardware_interface +{ +// Count of how many different emergency stop signals there are that can be reported. +const size_t emergency_stop_signal_count = 1; + +constexpr char EMERGENCY_STOP_SIGNAL[] = "EMERGENCY_STOP_SIGNAL"; +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_EMERGENCY_STOP_SIGNAL_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 new file mode 100644 index 0000000000..4b5095ef39 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp @@ -0,0 +1,105 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ + +#include +#include + +namespace hardware_interface +{ + +// Count of how many different error signals there are that can be reported. +const size_t error_signal_count = 32; + +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 +}; + +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 + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ 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 new file mode 100644 index 0000000000..434f2ec988 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp @@ -0,0 +1,104 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ + +#include +#include + +namespace hardware_interface +{ +// Count of how many different warn signals there are that can be reported. +const size_t warning_signal_count = 32; + +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 +}; + +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 + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 7441f0c556..ebf9b9bd76 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -43,6 +43,11 @@ namespace const auto TIME = rclcpp::Time(0); const auto PERIOD = rclcpp::Duration::from_seconds(0.01); constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +const auto emergency_stop_signal_size = 1; +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; } // namespace using namespace ::testing; // NOLINT @@ -786,7 +791,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = actuator_hw.export_state_interfaces(); - ASSERT_EQ(2u, state_interfaces.size()); + ASSERT_EQ(2u + report_signals_size, 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()); @@ -915,7 +920,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = sensor_hw.export_state_interfaces(); - ASSERT_EQ(1u, state_interfaces.size()); + ASSERT_EQ(1u + warnig_signals_size + error_signals_size, 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()); @@ -1078,7 +1083,7 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = system_hw.export_state_interfaces(); - ASSERT_EQ(6u, state_interfaces.size()); + ASSERT_EQ(6u + report_signals_size, 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()); diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index e438983686..5e320e9cb8 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -39,11 +39,15 @@ // Values to send over command interface to trigger error in write and read methods +// Values to send over command interface to trigger error in write and read methods + namespace { -const auto TIME = rclcpp::Time(0); -const auto PERIOD = rclcpp::Duration::from_seconds(0.01); -constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +const auto emergency_stop_signal_size = 1; +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; } // namespace using namespace ::testing; // NOLINT @@ -175,17 +179,19 @@ TEST(TestComponentInterfaces, dummy_actuator_default_custom_export) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + const auto listed_interface_size = 2u; + const auto interface_offset = listed_interface_size + report_signals_size; auto state_interfaces = actuator_hw.export_state_interfaces(); - ASSERT_EQ(3u, state_interfaces.size()); + ASSERT_EQ(interface_offset + 1u, 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("joint1/some_unlisted_interface", state_interfaces[2]->get_name()); - EXPECT_EQ("some_unlisted_interface", state_interfaces[2]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[2]->get_prefix_name()); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[interface_offset]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[interface_offset]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[interface_offset]->get_prefix_name()); auto command_interfaces = actuator_hw.export_command_interfaces(); ASSERT_EQ(2u, command_interfaces.size()); @@ -212,15 +218,17 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + const auto listed_interface_size = 1u; + const auto interface_offset = listed_interface_size + error_signals_size + warnig_signals_size; auto state_interfaces = sensor_hw.export_state_interfaces(); - ASSERT_EQ(2u, state_interfaces.size()); + ASSERT_EQ(interface_offset + 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())); - EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[1]->get_name()); - EXPECT_EQ("some_unlisted_interface", state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[interface_offset]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[interface_offset]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[interface_offset]->get_prefix_name()); } TEST(TestComponentInterfaces, dummy_system_default_custom_export) @@ -238,8 +246,10 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + const auto listed_interface_size = 6u; + const auto interface_offset = listed_interface_size + report_signals_size; auto state_interfaces = system_hw.export_state_interfaces(); - ASSERT_EQ(7u, state_interfaces.size()); + ASSERT_EQ(interface_offset + 1u, 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()); @@ -258,9 +268,9 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) 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()); - EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[6]->get_name()); - EXPECT_EQ("some_unlisted_interface", state_interfaces[6]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[6]->get_prefix_name()); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[interface_offset]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[interface_offset]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[interface_offset]->get_prefix_name()); auto command_interfaces = system_hw.export_command_interfaces(); ASSERT_EQ(4u, command_interfaces.size()); diff --git a/hardware_interface/test/test_error_warning_codes.cpp b/hardware_interface/test/test_error_warning_codes.cpp new file mode 100644 index 0000000000..114169f0ac --- /dev/null +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -0,0 +1,981 @@ +// Copyright 2020 ros2_control development team +// +// 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. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/actuator.hpp" +#include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/sensor_interface.hpp" +#include "hardware_interface/system.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +// Values to send over command interface to trigger error in write and read methods + +namespace +{ +const auto TIME = rclcpp::Time(0); +const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +const auto emergency_stop_signal_size = 1; +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; +} // namespace + +using namespace ::testing; // NOLINT + +namespace test_components +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class DummyActuatorDefault : public hardware_interface::ActuatorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + // We hardcode the info + if ( + hardware_interface::ActuatorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/position", 0.0); + set_state("joint1/velocity", 0.0); + set_emergency_stop(0.0); + set_error_code(0.0); + set_error_message(0.0); + set_warning_code(0.0); + set_warning_message(0.0); + + if (recoverable_error_happened_) + { + set_command("joint1/velocity", 0.0); + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummyActuatorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_emergency_stop(1.0); + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + 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) + { + set_emergency_stop(1.0); + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + auto position_state = get_state("joint1/position"); + set_state("joint1/position", position_state + get_command("joint1/velocity")); + set_state("joint1/velocity", get_command("joint1/velocity")); + + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/velocity", 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: + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +class DummySensorDefault : public hardware_interface::SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SensorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // We hardcode the info + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (const auto & [name, descr] : sensor_state_interfaces_) + { + set_state(name, 0.0); + } + set_error_code(0.0); + set_error_message(0.0); + set_warning_code(0.0); + set_warning_message(0.0); + read_calls_ = 0; + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySensorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + + // no-op, static value + set_state("joint1/voltage", 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_hw_value_ = 0x666; + + // Helper variables to initiate error on read + int read_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +class DummySystemDefault : public hardware_interface::SystemInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + // We hardcode the info + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (auto i = 0ul; i < 3; ++i) + { + set_state(position_states_[i], 0.0); + set_state(velocity_states_[i], 0.0); + } + set_emergency_stop(0.0); + set_error_code(0.0); + set_error_message(0.0); + set_warning_code(0.0); + set_warning_message(0.0); + // reset command only if error is initiated + if (recoverable_error_happened_) + { + for (auto i = 0ul; i < 3; ++i) + { + set_command(velocity_commands_[i], 0.0); + } + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySystemDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_emergency_stop(1.0); + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + 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) + { + set_emergency_stop(1.0); + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + + for (auto i = 0; i < 3; ++i) + { + auto current_pos = get_state(position_states_[i]); + set_state(position_states_[i], current_pos + get_command(velocity_commands_[i])); + set_state(velocity_states_[i], get_command(velocity_commands_[i])); + } + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (const auto & velocity_state : velocity_states_) + { + set_state(velocity_state, 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::vector position_states_ = { + "joint1/position", "joint2/position", "joint3/position"}; + std::vector velocity_states_ = { + "joint1/velocity", "joint2/velocity", "joint3/velocity"}; + std::vector velocity_commands_ = { + "joint1/velocity", "joint2/velocity", "joint3/velocity"}; + + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +} // namespace test_components + +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()); + + const auto state_interface_offset = 2; + auto state_interfaces = actuator_hw.export_state_interfaces(); + ASSERT_EQ(state_interface_offset + report_signals_size, 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()); + // EMERGENCY STOP + EXPECT_EQ( + "ActuatorModularJoint1/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL), + state_interfaces[state_interface_offset]->get_name()); + EXPECT_EQ( + hardware_interface::EMERGENCY_STOP_SIGNAL, + state_interfaces[state_interface_offset]->get_interface_name()); + EXPECT_EQ("ActuatorModularJoint1", state_interfaces[state_interface_offset]->get_prefix_name()); + // ERROR_SIGNAL + EXPECT_EQ( + "ActuatorModularJoint1/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME), + state_interfaces[state_interface_offset + 1]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_INTERFACE_NAME, + state_interfaces[state_interface_offset + 1]->get_interface_name()); + EXPECT_EQ( + "ActuatorModularJoint1", state_interfaces[state_interface_offset + 1]->get_prefix_name()); + EXPECT_EQ( + "ActuatorModularJoint1/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[state_interface_offset + 2]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[state_interface_offset + 2]->get_interface_name()); + EXPECT_EQ( + "ActuatorModularJoint1", state_interfaces[state_interface_offset + 2]->get_prefix_name()); + // WARNING SIGNAL + EXPECT_EQ( + "ActuatorModularJoint1/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME), + state_interfaces[state_interface_offset + 3]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_INTERFACE_NAME, + state_interfaces[state_interface_offset + 3]->get_interface_name()); + EXPECT_EQ( + "ActuatorModularJoint1", state_interfaces[state_interface_offset + 3]->get_prefix_name()); + EXPECT_EQ( + "ActuatorModularJoint1/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[state_interface_offset + 4]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[state_interface_offset + 4]->get_interface_name()); + EXPECT_EQ( + "ActuatorModularJoint1", state_interfaces[state_interface_offset + 4]->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()); +} + +TEST(TestComponentInterfaces, dummy_sensor_default) +{ + hardware_interface::Sensor sensor_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_voltage_sensor_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 voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 1; + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ( + state_interface_offset + warnig_signals_size + error_signals_size, state_interfaces.size()); + // check that the normal interfaces get exported as expected + 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())); + // ERROR_SIGNAL + EXPECT_EQ( + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME), + state_interfaces[state_interface_offset]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_INTERFACE_NAME, + state_interfaces[state_interface_offset]->get_interface_name()); + EXPECT_EQ("SingleJointVoltage", state_interfaces[state_interface_offset]->get_prefix_name()); + EXPECT_EQ( + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[state_interface_offset + 1]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[state_interface_offset + 1]->get_interface_name()); + EXPECT_EQ("SingleJointVoltage", state_interfaces[state_interface_offset + 1]->get_prefix_name()); + // WARNING SIGNAL + EXPECT_EQ( + "SingleJointVoltage/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME), + state_interfaces[state_interface_offset + 2]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_INTERFACE_NAME, + state_interfaces[state_interface_offset + 2]->get_interface_name()); + EXPECT_EQ("SingleJointVoltage", state_interfaces[state_interface_offset + 2]->get_prefix_name()); + EXPECT_EQ( + "SingleJointVoltage/" + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[state_interface_offset + 3]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[state_interface_offset + 3]->get_interface_name()); + EXPECT_EQ("SingleJointVoltage", state_interfaces[state_interface_offset + 3]->get_prefix_name()); +} + +TEST(TestComponentInterfaces, dummy_system_default) +{ + hardware_interface::System system_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_system_robot + + 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_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 6; + auto state_interfaces = system_hw.export_state_interfaces(); + ASSERT_EQ(state_interface_offset + report_signals_size, 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()); + // EMERGENCY STOP + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL), + state_interfaces[state_interface_offset]->get_name()); + EXPECT_EQ( + hardware_interface::EMERGENCY_STOP_SIGNAL, + state_interfaces[state_interface_offset]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[state_interface_offset]->get_prefix_name()); + // ERROR_SIGNAL + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME), + state_interfaces[state_interface_offset + 1]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_INTERFACE_NAME, + state_interfaces[state_interface_offset + 1]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[state_interface_offset + 1]->get_prefix_name()); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[state_interface_offset + 2]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[state_interface_offset + 2]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[state_interface_offset + 2]->get_prefix_name()); + // WARNING SIGNAL + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME), + state_interfaces[state_interface_offset + 3]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_INTERFACE_NAME, + state_interfaces[state_interface_offset + 3]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[state_interface_offset + 3]->get_prefix_name()); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[state_interface_offset + 4]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[state_interface_offset + 4]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[state_interface_offset + 4]->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()); +} + +TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) +{ + 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()); + + const auto state_interface_offset = 2; + 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(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); + + 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(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); + + 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()); +} + +TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) +{ + 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()); + + const auto state_interface_offset = 2; + 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(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); + + 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()); +} + +TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) +{ + hardware_interface::Sensor sensor_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_voltage_sensor_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 voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + + const auto state_interface_offset = 1; + 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(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + + 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()); + + // 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()); + + // 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()); +} + +TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) +{ + hardware_interface::System system_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_system_robot + + 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_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 6; + 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(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); + + 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(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); + + 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()); +} + +TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) +{ + hardware_interface::System system_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_system_robot + + 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_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 6; + 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(state_interfaces[state_interface_offset]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[state_interface_offset]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 1]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 2]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 3]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[state_interface_offset + 4]->get_value(), 1.0); + + 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()); +}