Add new Handle constructor for easier initialization (#2253)

Author: saikishor

hardware_interface/include/hardware_interface/handle.hpp

@@ -31,6 +31,7 @@
 
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/introspection.hpp"
+#include "hardware_interface/lexical_casts.hpp"
 #include "hardware_interface/macros.hpp"
 
 namespace
@@ -55,43 +56,64 @@ class Handle
 {
 public:
   [[deprecated("Use InterfaceDescription for initializing the Interface")]]
-  Handle(
-    const std::string & prefix_name, const std::string & interface_name,
-    double * value_ptr = nullptr)
+  Handle(const std::string & prefix_name, const std::string & interface_name, double * value_ptr)
   : prefix_name_(prefix_name),
     interface_name_(interface_name),
     handle_name_(prefix_name_ + "/" + interface_name_),
     value_ptr_(value_ptr)
   {
   }
 
-  explicit Handle(const InterfaceDescription & interface_description)
-  : prefix_name_(interface_description.get_prefix_name()),
-    interface_name_(interface_description.get_interface_name()),
-    handle_name_(interface_description.get_name())
+  explicit Handle(
+    const std::string & prefix_name, const std::string & interface_name,
+    const std::string & data_type = "double", const std::string & initial_value = "")
+  : prefix_name_(prefix_name),
+    interface_name_(interface_name),
+    handle_name_(prefix_name_ + "/" + interface_name_),
+    data_type_(data_type)
   {
-    data_type_ = interface_description.get_data_type();
     // As soon as multiple datatypes are used in HANDLE_DATATYPE
     // we need to initialize according the type passed in interface description
     if (data_type_ == hardware_interface::HandleDataType::DOUBLE)
     {
-      value_ = std::numeric_limits<double>::quiet_NaN();
-      value_ptr_ = std::get_if<double>(&value_);
+      try
+      {
+        value_ = initial_value.empty() ? std::numeric_limits<double>::quiet_NaN()
+                                       : hardware_interface::stod(initial_value);
+        value_ptr_ = std::get_if<double>(&value_);
+      }
+      catch (const std::invalid_argument & err)
+      {
+        throw std::invalid_argument(
+          fmt::format(
+            FMT_COMPILE(
+              "Invalid initial value : '{}' parsed for interface : '{}' with type : '{}'"),
+            initial_value, handle_name_, data_type_.to_string()));
+      }
     }
     else if (data_type_ == hardware_interface::HandleDataType::BOOL)
     {
       value_ptr_ = nullptr;
-      value_ = false;
+      value_ = initial_value.empty() ? false : hardware_interface::parse_bool(initial_value);
     }
     else
     {
       throw std::runtime_error(
         fmt::format(
-          FMT_COMPILE("Invalid data type : '{}' for interface : {}"),
-          interface_description.interface_info.data_type, interface_description.get_name()));
+          FMT_COMPILE(
+            "Invalid data type : '{}' for interface : {}. Supported types are double and bool."),
+          data_type, handle_name_));
     }
   }
 
+  explicit Handle(const InterfaceDescription & interface_description)
+  : Handle(
+      interface_description.get_prefix_name(), interface_description.get_interface_name(),
+      interface_description.get_data_type_string(),
+      interface_description.interface_info.initial_value)
+  {
+  }
+
   [[deprecated("Use InterfaceDescription for initializing the Interface")]]
 
   explicit Handle(const std::string & interface_name)

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -35,11 +35,11 @@ struct InterfaceInfo
    */
   std::string name;
   /// (Optional) Minimal allowed values of the interface.
-  std::string min;
+  std::string min = "";
   /// (Optional) Maximal allowed values of the interface.
-  std::string max;
+  std::string max = "";
   /// (Optional) Initial value of the interface.
-  std::string initial_value;
+  std::string initial_value = "";
   /// (Optional) The datatype of the interface, e.g. "bool", "int".
   std::string data_type = "double";
   /// (Optional) If the handle is an array, the size of the array.
@@ -224,6 +224,8 @@ struct InterfaceDescription
 
   const std::string & get_name() const { return interface_name; }
 
+  const std::string & get_data_type_string() const { return interface_info.data_type; }
+
   HandleDataType get_data_type() const { return HandleDataType(interface_info.data_type); }
 };
 

hardware_interface/test/test_handle.cpp

@@ -62,15 +62,15 @@ TEST(TestHandle, state_interface)
 
 TEST(TestHandle, name_getters_work)
 {
-  StateInterface handle{JOINT_NAME, FOO_INTERFACE};
+  StateInterface handle{JOINT_NAME, FOO_INTERFACE, nullptr};
   EXPECT_EQ(handle.get_name(), std::string(JOINT_NAME) + "/" + std::string(FOO_INTERFACE));
   EXPECT_EQ(handle.get_interface_name(), FOO_INTERFACE);
   EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME);
 }
 
 TEST(TestHandle, value_methods_throw_for_nullptr)
 {
-  CommandInterface handle{JOINT_NAME, FOO_INTERFACE};
+  CommandInterface handle{JOINT_NAME, FOO_INTERFACE, nullptr};
   EXPECT_ANY_THROW(handle.get_optional().value());
   EXPECT_ANY_THROW(std::ignore = handle.set_value(0.0));
 }
@@ -234,6 +234,55 @@ TEST(TestHandle, interface_description_bool_data_type)
   ASSERT_THROW({ std::ignore = handle.get_optional<double>(); }, std::runtime_error);
 }
 
+TEST(TestHandle, handle_constructor_double_data_type)
+{
+  const std::string POSITION_INTERFACE = "position";
+  const std::string JOINT_NAME_1 = "joint1";
+  StateInterface handle{JOINT_NAME_1, POSITION_INTERFACE, "double", "23.0"};
+
+  ASSERT_EQ(hardware_interface::HandleDataType::DOUBLE, handle.get_data_type());
+  EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE);
+  EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE);
+  EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1);
+  EXPECT_NO_THROW({ std::ignore = handle.get_optional<double>(); });
+  ASSERT_EQ(handle.get_optional<double>().value(), 23.0);
+  ASSERT_TRUE(handle.get_optional<double>().has_value());
+  ASSERT_TRUE(handle.set_value(0.0));
+  ASSERT_EQ(handle.get_optional<double>().value(), 0.0);
+  ASSERT_TRUE(handle.set_value(1.0));
+  ASSERT_EQ(handle.get_optional<double>().value(), 1.0);
+
+  // Test the assertions
+  ASSERT_THROW({ std::ignore = handle.get_optional<bool>(); }, std::runtime_error);
+  ASSERT_THROW({ std::ignore = handle.set_value(true); }, std::runtime_error);
+  EXPECT_ANY_THROW({ StateInterface bad_itf("joint1", POSITION_INTERFACE, "double", "233NaN0"); })
+    << "Invalid double value should throw";
+}
+
+TEST(TestHandle, handle_constructor_bool_data_type)
+{
+  const std::string collision_interface = "collision";
+  const std::string itf_name = "joint1";
+  StateInterface handle{itf_name, collision_interface, "bool", "true"};
+
+  ASSERT_EQ(hardware_interface::HandleDataType::BOOL, handle.get_data_type());
+  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({ std::ignore = handle.get_optional<bool>(); });
+  ASSERT_TRUE(handle.get_optional<bool>().value())
+    << "Default value should be true as it is initialized";
+  ASSERT_TRUE(handle.set_value(false));
+  ASSERT_FALSE(handle.get_optional<bool>().value());
+  ASSERT_TRUE(handle.set_value(true));
+  ASSERT_TRUE(handle.get_optional<bool>().value());
+
+  // Test the assertions
+  ASSERT_THROW({ std::ignore = handle.set_value(-1.0); }, std::runtime_error);
+  ASSERT_THROW({ std::ignore = handle.set_value(0.0); }, std::runtime_error);
+  ASSERT_THROW({ std::ignore = handle.get_optional<double>(); }, std::runtime_error);
+}
+
 TEST(TestHandle, interface_description_unknown_data_type)
 {
   const std::string collision_interface = "collision";
@@ -245,6 +294,8 @@ TEST(TestHandle, interface_description_unknown_data_type)
 
   ASSERT_EQ(hardware_interface::HandleDataType::UNKNOWN, interface_descr.get_data_type());
   EXPECT_ANY_THROW({ StateInterface handle{interface_descr}; }) << "Unknown data type should throw";
+  EXPECT_ANY_THROW({ StateInterface handle("joint1", "collision", "UNKNOWN"); })
+    << "Unknown data type should throw";
 }
 
 TEST(TestHandle, interface_description_command_interface_name_getters_work)