add parsing of handle type for initialization of variant and adjust m…

…ost tests

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -159,27 +159,31 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
     // create error signal interface
     InterfaceInfo error_interface_info;
     error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME;
-    error_interface_info.data_type = "array<uint8_t>[32]";
+    error_interface_info.data_type = "vector<uint8_t>";
+    error_interface_info.size = 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]";
+    error_msg_interface_info.data_type = "vector<string>";
+    error_msg_interface_info.size = 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]";
+    warning_interface_info.data_type = "vector<int8_t>";
+    warning_interface_info.size = 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]";
+    warning_msg_interface_info.data_type = "vector<string>";
+    warning_msg_interface_info.size = 32;
     InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info);
     warning_signal_message_ = std::make_shared<StateInterface>(warning_msg_interface_descr);
   }
@@ -451,49 +455,41 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
 
   bool get_emergency_stop() const { return emergency_stop_->get_value<bool>(); }
 
-  void set_error_code(std::array<uint8_t, hardware_interface::error_signal_count> error_codes)
-  {
-    error_signal_->set_value(error_codes);
-  }
+  void set_error_code(std::vector<uint8_t> error_codes) { error_signal_->set_value(error_codes); }
 
-  std::array<uint8_t, hardware_interface::error_signal_count> get_error_code() const
+  std::vector<uint8_t> get_error_code() const
   {
-    return error_signal_->get_value<std::array<uint8_t, hardware_interface::error_signal_count>>();
+    return error_signal_->get_value<std::vector<uint8_t>>();
   }
 
-  void set_error_message(
-    std::array<std::string, hardware_interface::error_signal_count> error_messages)
+  void set_error_message(std::vector<std::string> error_messages)
   {
     error_signal_message_->set_value(error_messages);
   }
 
-  std::array<std::string, hardware_interface::error_signal_count> get_error_message() const
+  std::vector<std::string> get_error_message() const
   {
-    return error_signal_message_
-      ->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
+    return error_signal_message_->get_value<std::vector<std::string>>();
   }
 
-  void set_warning_code(std::array<int8_t, hardware_interface::warning_signal_count> warning_codes)
+  void set_warning_code(std::vector<int8_t> warning_codes)
   {
     warning_signal_->set_value(warning_codes);
   }
 
-  std::array<int8_t, hardware_interface::warning_signal_count> get_warning_code() const
+  std::vector<int8_t> get_warning_code() const
   {
-    return warning_signal_
-      ->get_value<std::array<int8_t, hardware_interface::warning_signal_count>>();
+    return warning_signal_->get_value<std::vector<int8_t>>();
   }
 
-  void set_warning_message(
-    std::array<std::string, hardware_interface::warning_signal_count> error_message)
+  void set_warning_message(std::vector<std::string> error_message)
   {
     warning_signal_message_->set_value(error_message);
   }
 
-  std::array<std::string, hardware_interface::error_signal_count> get_warning_message() const
+  std::vector<std::string> get_warning_message() const
   {
-    return warning_signal_message_
-      ->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
+    return warning_signal_message_->get_value<std::vector<std::string>>();
   }
 
 protected:

hardware_interface/include/hardware_interface/handle.hpp

@@ -15,35 +15,20 @@
 #ifndef HARDWARE_INTERFACE__HANDLE_HPP_
 #define HARDWARE_INTERFACE__HANDLE_HPP_
 
-#include <array>
 #include <limits>
 #include <memory>
 #include <string>
 #include <type_traits>
 #include <utility>
 #include <variant>
+#include <vector>
 
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/macros.hpp"
-#include "hardware_interface/types/hardware_interface_error_signals.hpp"
-#include "hardware_interface/types/hardware_interface_warning_signals.hpp"
-#include "hardware_interface/visibility_control.h"
-namespace hardware_interface
-{
+#include "hardware_interface/types/handle_datatype.hpp"
 
-using HANDLE_DATATYPE = std::variant<
-  bool, double, hardware_interface::WARNING_SIGNALS, hardware_interface::ERROR_SIGNALS,
-  hardware_interface::WARNING_MESSAGES>;
-
-// Define a type trait for allowed types
-template <typename T>
-struct HANDLE_DATATYPE_TYPES : std::disjunction<
-                                 std::is_same<T, bool>, std::is_same<T, double>,
-                                 std::is_same<T, hardware_interface::WARNING_SIGNALS>,
-                                 std::is_same<T, hardware_interface::ERROR_SIGNALS>,
-                                 std::is_same<T, hardware_interface::WARNING_MESSAGES>>
+namespace hardware_interface
 {
-};
 
 /// A handle used to get and set a value on a given interface.
 class Handle
@@ -56,30 +41,32 @@ class Handle
     double * value_ptr = nullptr)
   : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr)
   {
+    // default to double
+    value_ = std::numeric_limits<double>::quiet_NaN();
   }
 
   explicit Handle(const InterfaceDescription & interface_description)
   : prefix_name_(interface_description.prefix_name),
     interface_name_(interface_description.interface_info.name)
   {
-    // TODO(Manuel): implement this.
-    // As soon as multiple datatypes are used in HANDLE_DATATYPE
-    // we need to initialize according the type passed in interface description
-    // value_ = std::numeric_limits<double>::quiet_NaN();
+    init_handle_value(interface_description.interface_info);
   }
 
   [[deprecated("Use InterfaceDescription for initializing the Interface")]]
 
   explicit Handle(const std::string & interface_name)
   : interface_name_(interface_name), value_ptr_(nullptr)
   {
+    // default to double
+    value_ = std::numeric_limits<double>::quiet_NaN();
   }
 
   [[deprecated("Use InterfaceDescription for initializing the Interface")]]
 
   explicit Handle(const char * interface_name)
   : interface_name_(interface_name), value_ptr_(nullptr)
-  {
+  {  // default to double
+    value_ = std::numeric_limits<double>::quiet_NaN();
   }
 
   Handle(const Handle & other) = default;
@@ -121,6 +108,78 @@ class Handle
   }
 
 protected:
+  // used for the
+  bool correct_vector_size(const size_t & expected, const size_t & actual)
+  {
+    return expected == actual;
+  }
+
+  void init_handle_value(const InterfaceInfo & interface_info)
+  {
+    if (interface_info.data_type == "bool")
+    {
+      value_ = interface_info.initial_value.empty() ? false
+                                                    : (interface_info.initial_value == "true" ||
+                                                       interface_info.initial_value == "True");
+    }
+    else if (interface_info.data_type == "vector<int8_t>")
+    {
+      if (
+        interface_info.size != 0 && hardware_interface::warning_signal_count != interface_info.size)
+      {
+        throw std::runtime_error(
+          "The size:{" + std::to_string(interface_info.size) + "} for data_type{" +
+          interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name +
+          "} does not equal the expected size:{" +
+          std::to_string(hardware_interface::warning_signal_count) + "}.");
+      }
+      value_ = std::vector<int8_t>(hardware_interface::warning_signal_count, 0);
+    }
+    else if (interface_info.data_type == "vector<uint8_t>")
+    {
+      if (interface_info.size != 0 && hardware_interface::error_signal_count != interface_info.size)
+      {
+        throw std::runtime_error(
+          "The size:{" + std::to_string(interface_info.size) + "} for data_type{" +
+          interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name +
+          "} does not equal the expected size:{" +
+          std::to_string(hardware_interface::error_signal_count) + "}.");
+      }
+
+      value_ = std::vector<uint8_t>(hardware_interface::error_signal_count, 0);
+    }
+    else if (interface_info.data_type == "vector<string>")
+    {
+      if (
+        interface_info.size != 0 && hardware_interface::warning_signal_count != interface_info.size)
+      {
+        throw std::runtime_error(
+          "The size:{" + std::to_string(interface_info.size) + "} for data_type{" +
+          interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name +
+          "} does not equal the expected size:{" +
+          std::to_string(hardware_interface::warning_signal_count) + "}.");
+      }
+
+      value_ = std::vector<std::string>(hardware_interface::warning_signal_count, "");
+    }
+    // Default for empty is double
+    else if (interface_info.data_type.empty() || interface_info.data_type == "double")
+    {
+      value_ = interface_info.initial_value.empty() ? std::numeric_limits<double>::quiet_NaN()
+                                                    : std::stod(interface_info.initial_value);
+    }
+    // If not empty and it belongs to none of the above types, we still want to throw as there might
+    // be a typo in the data_type like "bol" or user wants some unsupported type
+    else
+    {
+      throw std::runtime_error(
+        "The data_type:{" + interface_info.data_type + "} for the InterfaceInfo with name:{" +
+        interface_info.name +
+        "} is not supported for Handles. Supported data_types are: bool, double, vector<int8_t>, "
+        "vector<uint8_t> and vector<string>.");
+    }
+  }
+
   std::string prefix_name_;
   std::string interface_name_;
   HANDLE_DATATYPE value_;

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -27,6 +27,17 @@ namespace hardware_interface
  */
 struct InterfaceInfo
 {
+  // Add default constructor, so that e.g. size is initialized to sensible value
+  InterfaceInfo()
+  {
+    // cpp_lint complains about min and max include otherwise
+    name = "";
+    min = "";
+    max = "";
+    initial_value = "";
+    data_type = "";
+    size = 0;
+  }
   /**
    * Name of the command interfaces that can be set, e.g. "position", "velocity", etc.
    * Used by joints and GPIOs.

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -136,27 +136,31 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     // create error signal interface
     InterfaceInfo error_interface_info;
     error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME;
-    error_interface_info.data_type = "array<uint8_t>[32]";
+    error_interface_info.data_type = "vector<uint8_t>";
+    error_interface_info.size = 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]";
+    error_msg_interface_info.data_type = "vector<string>";
+    error_msg_interface_info.size = 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]";
+    warning_interface_info.data_type = "vector<int8_t>";
+    warning_interface_info.size = 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]";
+    warning_msg_interface_info.data_type = "vector<string>";
+    warning_msg_interface_info.size = 32;
     InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info);
     warning_signal_message_ = std::make_shared<StateInterface>(warning_msg_interface_descr);
   }
@@ -285,49 +289,41 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     return sensor_states_.at(interface_name)->get_value<double>();
   }
 
-  void set_error_code(std::array<uint8_t, hardware_interface::error_signal_count> error_codes)
-  {
-    error_signal_->set_value(error_codes);
-  }
+  void set_error_code(std::vector<uint8_t> error_codes) { error_signal_->set_value(error_codes); }
 
-  std::array<uint8_t, hardware_interface::error_signal_count> get_error_code() const
+  std::vector<uint8_t> get_error_code() const
   {
-    return error_signal_->get_value<std::array<uint8_t, hardware_interface::error_signal_count>>();
+    return error_signal_->get_value<std::vector<uint8_t>>();
   }
 
-  void set_error_message(
-    std::array<std::string, hardware_interface::error_signal_count> error_messages)
+  void set_error_message(std::vector<std::string> error_messages)
   {
     error_signal_message_->set_value(error_messages);
   }
 
-  std::array<std::string, hardware_interface::error_signal_count> get_error_message() const
+  std::vector<std::string> get_error_message() const
   {
-    return error_signal_message_
-      ->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
+    return error_signal_message_->get_value<std::vector<std::string>>();
   }
 
-  void set_warning_code(std::array<int8_t, hardware_interface::warning_signal_count> warning_codes)
+  void set_warning_code(std::vector<int8_t> warning_codes)
   {
     warning_signal_->set_value(warning_codes);
   }
 
-  std::array<int8_t, hardware_interface::warning_signal_count> get_warning_code() const
+  std::vector<int8_t> get_warning_code() const
   {
-    return warning_signal_
-      ->get_value<std::array<int8_t, hardware_interface::warning_signal_count>>();
+    return warning_signal_->get_value<std::vector<int8_t>>();
   }
 
-  void set_warning_message(
-    std::array<std::string, hardware_interface::warning_signal_count> error_message)
+  void set_warning_message(std::vector<std::string> error_message)
   {
     warning_signal_message_->set_value(error_message);
   }
 
-  std::array<std::string, hardware_interface::error_signal_count> get_warning_message() const
+  std::vector<std::string> get_warning_message() const
   {
-    return warning_signal_message_
-      ->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
+    return warning_signal_message_->get_value<std::vector<std::string>>();
   }
 
 protected: