add support for booleans in the handles

ros-controls · Feb 20, 2025 · aa9fd1a · aa9fd1a
1 parent 201d3a7
commit aa9fd1a
Show file tree

Hide file tree

Showing 3 changed files with 83 additions and 7 deletions.
diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp
@@ -32,7 +32,7 @@
 namespace hardware_interface
 {
 
-using HANDLE_DATATYPE = std::variant<std::monostate, double>;
+using HANDLE_DATATYPE = std::variant<std::monostate, double, bool>;
 
 /// A handle used to get and set a value on a given interface.
 class Handle
@@ -57,8 +57,22 @@ class Handle
   {
     // 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();
-    value_ptr_ = std::get_if<double>(&value_);
+    if (interface_description.get_data_type() == HandleDataType::DOUBLE)
+    {
+      value_ = std::numeric_limits<double>::quiet_NaN();
+      value_ptr_ = std::get_if<double>(&value_);
+    }
+    else if (interface_description.get_data_type() == HandleDataType::BOOL)
+    {
+      value_ptr_ = nullptr;
+      value_ = false;
+    }
+    else
+    {
+      throw std::runtime_error(
+        "Invalid data type : '" + interface_description.interface_info.data_type +
+        "' for interface : " + interface_description.get_name());
+    }
   }
 
   [[deprecated("Use InterfaceDescription for initializing the Interface")]]
@@ -147,9 +161,13 @@ class Handle
     {
       return std::nullopt;
     }
-    THROW_ON_NULLPTR(this->value_ptr_);
     // BEGIN (Handle export change): for backward compatibility
     // TODO(saikishor) return value_ if old functionality is removed
+    if constexpr (std::is_same_v<T, double>)
+    {
+      // If the template is of type double, check if the value_ptr_ is not nullptr
+      THROW_ON_NULLPTR(value_ptr_);
+    }
     return value_ptr_ != nullptr ? *value_ptr_ : std::get<T>(value_);
     // END
   }
@@ -201,8 +219,19 @@ class Handle
     }
     // BEGIN (Handle export change): for backward compatibility
     // TODO(Manuel) set value_ directly if old functionality is removed
-    THROW_ON_NULLPTR(this->value_ptr_);
-    *this->value_ptr_ = value;
+    if constexpr (std::is_same_v<T, double>)
+    {
+      // If the template is of type double, check if the value_ptr_ is not nullptr
+      THROW_ON_NULLPTR(value_ptr_);
+    }
+    if (value_ptr_ != nullptr)
+    {
+      *value_ptr_ = value;
+    }
+    else
+    {
+      value_ = value;
+    }
     return true;
     // END
   }

diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp
@@ -41,7 +41,7 @@ struct InterfaceInfo
   /// (Optional) Initial value of the interface.
   std::string initial_value;
   /// (Optional) The datatype of the interface, e.g. "bool", "int".
-  std::string data_type;
+  std::string data_type = "double";
   /// (Optional) If the handle is an array, the size of the array.
   int size;
   /// (Optional) enable or disable the limits for the command interfaces
@@ -130,6 +130,16 @@ struct TransmissionInfo
   std::unordered_map<std::string, std::string> parameters;
 };
 
+/**
+ * Hardware handles supported types
+ */
+enum class HandleDataType
+{
+  UNKNOWN,
+  DOUBLE,
+  BOOL
+};
+
 /**
  * This structure stores information about an interface for a specific hardware which should be
  * instantiated internally.
@@ -163,6 +173,22 @@ struct InterfaceDescription
   const std::string & get_interface_name() const { return interface_info.name; }
 
   const std::string & get_name() const { return interface_name; }
+
+  HandleDataType get_data_type() const
+  {
+    if (interface_info.data_type == "double")
+    {
+      return HandleDataType::DOUBLE;
+    }
+    else if (interface_info.data_type == "bool")
+    {
+      return HandleDataType::BOOL;
+    }
+    else
+    {
+      return HandleDataType::UNKNOWN;
+    }
+  }
 };
 
 /// This structure stores information about hardware defined in a robot's URDF.

diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp
@@ -89,6 +89,27 @@ TEST(TestHandle, interface_description_state_interface_name_getters_work)
   EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1);
 }
 
+TEST(TestHandle, interface_description_bool_data_type)
+{
+  const std::string collision_interface = "collision";
+  const std::string itf_name = "joint1";
+  InterfaceInfo info;
+  info.name = collision_interface;
+  info.data_type = "bool";
+  InterfaceDescription interface_descr(itf_name, info);
+  StateInterface handle{interface_descr};
+
+  EXPECT_EQ(handle.get_name(), itf_name + "/" + collision_interface);
+  EXPECT_EQ(handle.get_interface_name(), collision_interface);
+  EXPECT_EQ(handle.get_prefix_name(), itf_name);
+  EXPECT_NO_THROW({ handle.get_value<bool>(); });
+  ASSERT_FALSE(handle.get_value<bool>().value()) << "Default value should be false";
+  EXPECT_NO_THROW({ handle.set_value(true); });
+  ASSERT_TRUE(handle.get_value<bool>().value());
+  EXPECT_NO_THROW({ handle.set_value(false); });
+  ASSERT_FALSE(handle.get_value<bool>().value());
+}
+
 TEST(TestHandle, interface_description_command_interface_name_getters_work)
 {
   const std::string POSITION_INTERFACE = "position";