Publish all castable data types to pal_statistics (#2633) (#2854)

Author: mergify[bot]

hardware_interface/include/hardware_interface/handle.hpp

@@ -28,7 +28,6 @@
 #include <stdexcept>
 #include <string>
 #include <utility>
-#include <variant>
 
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/introspection.hpp"
@@ -61,8 +60,6 @@ std::string get_type_name()
 namespace hardware_interface
 {
 
-using HANDLE_DATATYPE = std::variant<std::monostate, double, bool>;
-
 /// A handle used to get and set a value on a given interface.
 class Handle
 {
@@ -313,6 +310,11 @@ class Handle
   /// Returns true if the handle data type can be casted to double.
   bool is_castable_to_double() const { return data_type_.is_castable_to_double(); }
 
+  bool is_valid() const
+  {
+    return (value_ptr_ != nullptr) || !std::holds_alternative<std::monostate>(value_);
+  }
+
 private:
   void copy(const Handle & other) noexcept
   {
@@ -349,7 +351,7 @@ class Handle
   HandleDataType data_type_ = HandleDataType::DOUBLE;
   // BEGIN (Handle export change): for backward compatibility
   // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed
-  double * value_ptr_;
+  double * value_ptr_ = nullptr;
   // END
   mutable std::shared_mutex handle_mutex_;
 
@@ -370,17 +372,35 @@ class StateInterface : public Handle
 
   void registerIntrospection() const
   {
-    if (value_ptr_ || std::holds_alternative<double>(value_))
+    if (!is_valid())
+    {
+      RCLCPP_WARN(
+        rclcpp::get_logger(get_name()),
+        "Cannot register state introspection for state interface: %s without a valid value "
+        "pointer or initialized value.",
+        get_name().c_str());
+      return;
+    }
+    if (value_ptr_ || data_type_.is_castable_to_double())
     {
       std::function<double()> f = [this]()
-      { return value_ptr_ ? *value_ptr_ : std::get<double>(value_); };
+      {
+        if (value_ptr_)
+        {
+          return *value_ptr_;
+        }
+        else
+        {
+          return data_type_.cast_to_double(value_);
+        }
+      };
       DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name(), f);
     }
   }
 
   void unregisterIntrospection() const
   {
-    if (value_ptr_ || std::holds_alternative<double>(value_))
+    if (is_valid() && (value_ptr_ || data_type_.is_castable_to_double()))
     {
       DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name());
     }
@@ -444,10 +464,28 @@ class CommandInterface : public Handle
 
   void registerIntrospection() const
   {
-    if (value_ptr_ || std::holds_alternative<double>(value_))
+    if (!is_valid())
+    {
+      RCLCPP_WARN(
+        rclcpp::get_logger(get_name()),
+        "Cannot register command introspection for command interface: %s without a valid value "
+        "pointer or initialized value.",
+        get_name().c_str());
+      return;
+    }
+    if (value_ptr_ || data_type_.is_castable_to_double())
     {
       std::function<double()> f = [this]()
-      { return value_ptr_ ? *value_ptr_ : std::get<double>(value_); };
+      {
+        if (value_ptr_)
+        {
+          return *value_ptr_;
+        }
+        else
+        {
+          return data_type_.cast_to_double(value_);
+        }
+      };
       DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name(), f);
       DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION(
         "command_interface." + get_name() + ".is_limited", &is_command_limited_);
@@ -456,7 +494,7 @@ class CommandInterface : public Handle
 
   void unregisterIntrospection() const
   {
-    if (value_ptr_ || std::holds_alternative<double>(value_))
+    if (is_valid() && (value_ptr_ || data_type_.is_castable_to_double()))
     {
       DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name());
       DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION(

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -15,8 +15,11 @@
 #ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_
 #define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_
 
+#include <fmt/compile.h>
+
 #include <string>
 #include <unordered_map>
+#include <variant>
 #include <vector>
 
 #include "joint_limits/joint_limits.hpp"
@@ -133,6 +136,8 @@ struct TransmissionInfo
 /**
  * Hardware handles supported types
  */
+
+using HANDLE_DATATYPE = std::variant<std::monostate, double, bool>;
 class HandleDataType
 {
 public:
@@ -202,6 +207,27 @@ class HandleDataType
     }
   }
 
+  /**
+   * @brief Cast the given value to double.
+   * @param value The value to be casted.
+   * @return The casted value.
+   * @throw std::runtime_error if the HandleDataType cannot be casted to double.
+   * @note Once we add support for more data types, this function should be updated
+   */
+  double cast_to_double(const HANDLE_DATATYPE & value) const
+  {
+    switch (value_)
+    {
+      case DOUBLE:
+        return std::get<double>(value);
+      case BOOL:
+        return static_cast<double>(std::get<bool>(value));
+      default:
+        throw std::runtime_error(
+          fmt::format(FMT_COMPILE("Data type : '{}' cannot be casted to double."), to_string()));
+    }
+  }
+
   HandleDataType from_string(const std::string & data_type) { return HandleDataType(data_type); }
 
 private:

hardware_interface/test/test_handle.cpp

@@ -426,3 +426,51 @@ TEST(TestHandle, move_assignment)
   EXPECT_DOUBLE_EQ(moved.get_optional().value(), 0.0);
 }
 #pragma GCC diagnostic pop
+
+class TestableHandle : public hardware_interface::Handle
+{
+  FRIEND_TEST(TestHandle, handle_castable);
+  // Use generation of interface names
+  explicit TestableHandle(const InterfaceDescription & interface_description)
+  : hardware_interface::Handle(interface_description)
+  {
+  }
+};
+
+TEST(TestHandle, handle_castable)
+{
+  hardware_interface::InterfaceInfo info;
+  info.name = "position";
+  const std::string JOINT_NAME_1 = "joint1";
+  {
+    info.data_type = "double";
+    info.initial_value = "23.0";
+    hardware_interface::InterfaceDescription interface_description{JOINT_NAME_1, info};
+    TestableHandle handle{interface_description};
+
+    EXPECT_TRUE(handle.is_valid());
+    EXPECT_TRUE(handle.is_castable_to_double());
+    EXPECT_EQ(handle.data_type_.cast_to_double(handle.value_), 23.0);
+  }
+  {
+    info.data_type = "bool";
+    info.initial_value = "false";
+    hardware_interface::InterfaceDescription interface_description{JOINT_NAME_1, info};
+    TestableHandle handle{interface_description};
+
+    EXPECT_TRUE(handle.is_valid());
+    EXPECT_TRUE(handle.is_castable_to_double());
+    EXPECT_EQ(handle.data_type_.cast_to_double(handle.value_), 0.0);
+
+    handle.value_ = true;
+    EXPECT_EQ(handle.data_type_.cast_to_double(handle.value_), 1.0);
+  }
+  {
+    // handle with unsupported datatype can't be created right now
+    // extend with more datatypes once supported in Handle
+    hardware_interface::HandleDataType dt{"string"};
+    EXPECT_FALSE(dt.is_castable_to_double());
+    hardware_interface::HANDLE_DATATYPE value = std::monostate{};
+    EXPECT_THROW(dt.cast_to_double(value), std::runtime_error);
+  }
+}

hardware_interface_testing/test/test_components/test_actuator.cpp

@@ -75,7 +75,7 @@ class TestActuator : public ActuatorInterface
         &velocity_state_));
     state_interfaces.emplace_back(
       hardware_interface::StateInterface(
-        get_hardware_info().joints[0].name, "some_unlisted_interface", nullptr));
+        get_hardware_info().joints[0].name, "some_unlisted_interface", &unlisted_interface_));
 
     return state_interfaces;
   }
@@ -187,6 +187,7 @@ class TestActuator : public ActuatorInterface
   double velocity_state_ = 0.0;
   double velocity_command_ = 0.0;
   double max_velocity_command_ = 0.0;
+  double unlisted_interface_ = std::numeric_limits<double>::quiet_NaN();
 };
 
 class TestUninitializableActuator : public TestActuator