add interface for warning, error and report

hardware_interface/CMakeLists.txt

@@ -75,6 +75,10 @@ if(BUILD_TESTING)
  target_link_libraries(test_component_interfaces hardware_interface)
  ament_target_dependencies(test_component_interfaces 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)

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"
@@ -103,6 +106,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;
  };
 
@@ -134,6 +138,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 = "std::array<uint8_t>";
+ 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 = "std::array<std::string>";
+ 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 = "std::array<uint8_t>";
+ 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 = "std::array<std::string>";
+ 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.
  /**
  * Default implementation for exporting the StateInterfaces. The StateInterfaces are created
@@ -170,6 +220,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
  actuator_states_.insert(std::make_pair(name, std::make_shared<StateInterface>(descr)));
  state_interfaces.push_back(actuator_states_.at(name));
  }
+
+ // 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;
  }
  /// Exports all command interfaces for this hardware interface.
@@ -310,13 +368,48 @@ 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<std::string, InterfaceDescription> joint_state_interfaces_;
  std::map<std::string, InterfaceDescription> joint_command_interfaces_;
 
 private:
  std::map<std::string, std::shared_ptr<StateInterface>> actuator_states_;
+ 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_;
+
  std::map<std::string, std::shared_ptr<CommandInterface>> actuator_commands_;
 
  rclcpp_lifecycle::State lifecycle_state_;

hardware_interface/include/hardware_interface/handle.hpp

@@ -15,15 +15,17 @@
 #ifndef HARDWARE_INTERFACE__HANDLE_HPP_
 #define HARDWARE_INTERFACE__HANDLE_HPP_
 
+#include <array>
 #include <limits>
 #include <string>
 #include <utility>
 #include <variant>
 
 #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
 {
 

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"
@@ -102,6 +104,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  {
  info_ = hardware_info;
  import_state_interface_descriptions(info_);
+ create_report_interfaces();
  return CallbackReturn::SUCCESS;
  };
 
@@ -119,6 +122,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 = "std::array<uint8_t>";
+ 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 = "std::array<std::string>";
+ 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 = "std::array<uint8_t>";
+ 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 = "std::array<std::string>";
+ 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.
  /**
  * Default implementation for exporting the StateInterfaces. The StateInterfaces are created
@@ -157,6 +199,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
  state_interfaces.push_back(sensor_states_.at(name));
  }
 
+ // 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;
  }
 
@@ -200,13 +248,39 @@ 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_;
 
  std::map<std::string, InterfaceDescription> sensor_state_interfaces_;
 
 private:
  std::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_;
 };