add interface for warning, error and report

* ddd basic test for error codes setting * docs for error and warning signals
StoglRobotics-forks · Jul 26, 2024 · 7caefee · 7caefee
1 parent bb92320
commit 7caefee
Show file tree

Hide file tree

Showing 13 changed files with 1,823 additions and 17 deletions.
diff --git a/doc/index.rst b/doc/index.rst
@@ -27,6 +27,7 @@ Concepts
    Controller Chaining / Cascade Control <../controller_manager/doc/controller_chaining.rst>
    Joint Kinematics <../hardware_interface/doc/joints_userdoc.rst>
    Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst>
+   Hardware Components <../hardware_interface/doc/error_and_warning_interfaces_userdoc.rst>
    Mock Components <../hardware_interface/doc/mock_components_userdoc.rst>
 
 =====================================

diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt
@@ -84,6 +84,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/doc/error_and_warning_interfaces_userdoc.rst b/hardware_interface/doc/error_and_warning_interfaces_userdoc.rst
@@ -0,0 +1,28 @@
+:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/error_and_warning_interfaces_userdoc.rst
+
+.. _error_and_warning_interfaces_userdoc:
+
+Error and Warning Interfaces
+============================
+
+By default we now create the following error and warning interfaces:
+
++-----------------+--------------+----------------------------------------------------------------------------------------------------------------------+
+| Type            | Datatype     | Description                                                                                                          |
++=================+====================+================================================================================================================+
+| Emergency Stop  | Bool               | Used for signaling that hardwares emergency stop is active. Only for Actuator and System.                      |
++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+
++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+
+| Error Code      | array<uint8_t, 32> | Used for sending 32 error codes (uint8_t) at the same time.                                                    |
++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+
+| Error Message   | array<string, 32>  | Used for sending 32 error messages where the message at position x corresponds to error code at position x.    |
++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+
++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+
+| Warning Code    | array<int8_t, 32>  | Used for sending 32 Warning codes (int8_t) at the same time.                                                   |
++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+
+| Warning Message | array<string, 32>  | Used for sending 32 warning messages where the message at position x corresponds to warning code at position x.|
++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+
+
+The error and warning interfaces are created as ``StateInterfaces`` and are stored inside the Actuator-, Sensor- or SystemInterface. They can be accessed via getter and setter methods. E.g. if you want to get/set the emergency stop signal you can do so with the ``get_emergency_stop()`` or ``set_emergency_stop(const bool & emergency_stop)`` methods. For the error and warning signals similar getters and setters exist.
+
+Note: The SensorInterface does not have a Emergency Stop interface.
diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp
@@ -25,7 +25,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"
@@ -125,6 +128,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;
   };
 
@@ -156,6 +160,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 <report_type>_MESSAGE hold the message for
+   * the corresponding report signal.
+   * The interfaces are named like <hardware_name>/<report_interface_type>. 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<StateInterface>(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 = "array<uint8_t>[32]";
+    InterfaceDescription error_interface_descr(info_.name, error_interface_info);
+    error_signal_ = std::make_shared<StateInterface>(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<string>[32]";
+    InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info);
+    error_signal_message_ = std::make_shared<StateInterface>(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 = "array<int8_t>[32]";
+    InterfaceDescription warning_interface_descr(info_.name, warning_interface_info);
+    warning_signal_ = std::make_shared<StateInterface>(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<string>[32]";
+    InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info);
+    warning_signal_message_ = std::make_shared<StateInterface>(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
@@ -230,6 +280,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;
   }
 
@@ -416,6 +474,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:
   /// Get the logger of the ActuatorInterface.
   /**
@@ -436,13 +523,19 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   std::unordered_map<std::string, InterfaceDescription> unlisted_state_interfaces_;
   std::unordered_map<std::string, InterfaceDescription> unlisted_command_interfaces_;
 
-  rclcpp_lifecycle::State lifecycle_state_;
-
 private:
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
   rclcpp::Logger actuator_logger_;
   std::unordered_map<std::string, std::shared_ptr<StateInterface>> actuator_states_;
   std::unordered_map<std::string, std::shared_ptr<CommandInterface>> actuator_commands_;
+
+  std::shared_ptr<StateInterface> emergency_stop_;
+  std::shared_ptr<StateInterface> error_signal_;
+  std::shared_ptr<StateInterface> error_signal_message_;
+  std::shared_ptr<StateInterface> warning_signal_;
+  std::shared_ptr<StateInterface> warning_signal_message_;
+
+  rclcpp_lifecycle::State lifecycle_state_;
 };
 
 }  // namespace hardware_interface

diff --git 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 <array>
 #include <limits>
 #include <memory>
 #include <string>
@@ -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
@@ -25,7 +25,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"
@@ -124,6 +126,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   {
     info_ = hardware_info;
     import_state_interface_descriptions(info_);
+    create_report_interfaces();
     return CallbackReturn::SUCCESS;
   };
 
@@ -141,6 +144,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 <report_type>_MESSAGE hold the message for
+   * the corresponding report signal.
+   * The interfaces are named like <hardware_name>/<report_interface_type>. 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 = "array<uint8_t>[32]";
+    InterfaceDescription error_interface_descr(info_.name, error_interface_info);
+    error_signal_ = std::make_shared<StateInterface>(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<string>[32]";
+    InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info);
+    error_signal_message_ = std::make_shared<StateInterface>(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 = "array<int8_t>[32]";
+    InterfaceDescription warning_interface_descr(info_.name, warning_interface_info);
+    warning_signal_ = std::make_shared<StateInterface>(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<string>[32]";
+    InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info);
+    warning_signal_message_ = std::make_shared<StateInterface>(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
@@ -217,6 +259,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;
   }
 
@@ -266,6 +314,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:
   /// Get the logger of the SensorInterface.
   /**
@@ -284,12 +354,17 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   std::unordered_map<std::string, InterfaceDescription> sensor_state_interfaces_;
   std::unordered_map<std::string, InterfaceDescription> unlisted_state_interfaces_;
 
-  rclcpp_lifecycle::State lifecycle_state_;
-
 private:
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
   rclcpp::Logger sensor_logger_;
   std::unordered_map<std::string, std::shared_ptr<StateInterface>> sensor_states_;
+
+  std::shared_ptr<StateInterface> error_signal_;
+  std::shared_ptr<StateInterface> error_signal_message_;
+  std::shared_ptr<StateInterface> warning_signal_;
+  std::shared_ptr<StateInterface> warning_signal_message_;
+
+  rclcpp_lifecycle::State lifecycle_state_;
 };
 
 }  // namespace hardware_interface