[Handle] Add support to more data types (backport #2879)

doc/release_notes.rst

@@ -195,6 +195,7 @@ hardware_interface
 * The controllers are now deactivated when a hardware component returns DEACTIVATE on the write cycle. The parameter ``deactivate_controllers_on_hardware_self_deactivate`` is added to control this behavior temporarily. It is recommended to set this parameter to true in order to avoid controllers to use inactive hardware components and to avoid any unexpected behavior. This feature parameter will be removed in future releases and will be defaulted to true (`#2334 <https://github.com/ros-controls/ros2_control/pull/2334>`_ & `#2501 <https://github.com/ros-controls/ros2_control/pull/2501>`_).
 * The controllers are not allowed to be activated when the hardware component is in INACTIVE state. The parameter ``allow_controller_activation_with_inactive_hardware`` is added to control this behavior temporarily. It is recommended to set this parameter to false in order to avoid controllers to use inactive hardware components and to avoid any unexpected behavior. This feature parameter will be removed in future releases and will be defaulted to false (`#2347 <https://github.com/ros-controls/ros2_control/pull/2347>`_).
 * The lifecycle ID is cached internally in the controller to avoid calls to get_lifecycle_state() in the real-time control loop. (`#2884 <https://github.com/ros-controls/ros2_control/pull/2884>`__)
+* Handles now also support ``float32``, ``uint8``, ``int8``, ``uint16``, ``int16``, ``uint32``, ``int32`` data types in addition to double and bool. (`#2879 <https://github.com/ros-controls/ros2_control/pull/2879>`__)
 
 joint_limits
 ************

hardware_interface/doc/hardware_interface_types_userdoc.rst

@@ -75,6 +75,22 @@ Hardware Component Groups serve as a critical organizational mechanism within co
 
 Hardware Component Groups play a vital role in propagating errors across interconnected hardware components. For instance, in a manipulator system, grouping actuators together allows for error propagation. If one actuator fails within the group, the error can propagate to the other actuators, signaling a potential issue across the system. By default, the actuator errors are isolated to their own hardware component, allowing the rest to continue operation unaffected. In the provided ros2_control configuration, the ``<group>`` tag within each ``<ros2_control>`` block signifies the grouping of hardware components, enabling error propagation mechanisms within the system.
 
+Data Types
+*****************************
+By default, command and state interfaces use the ``double`` data type.
+However, other data types can be specified using the optional ``data_type`` argument in the ``<command_interface>`` and ``<state_interface>`` tags.
+The following data types are supported, with their default initial value if not specified:
+
+* double (default): NaN
+* float32: NaN
+* bool: false
+* uint8: 255
+* int8: 127
+* uint16: 65535
+* int16: 32767
+* uint32: 4294967295
+* int32: 2147483647
+
 Examples
 *****************************
 The following examples show how to use the different hardware interface types in a ``ros2_control`` URDF.
@@ -83,9 +99,9 @@ They can be combined together within the different hardware component types (sys
 1. Robot with multiple GPIO interfaces
 
    - RRBot System
-   - Digital: 4 inputs and 2 outputs
-   - Analog: 2 inputs and 1 output
-   - Vacuum valve at the flange (on/off)
+   - Digital: 4 inputs and 2 outputs (bool)
+   - Analog: 2 inputs and 1 output (uint16)
+   - Vacuum valve at the flange (bool)
 
 
   .. code:: xml
@@ -112,24 +128,24 @@ They can be combined together within the different hardware component types (sys
         <state_interface name="position"/>
       </joint>
       <gpio name="flange_digital_IOs">
-        <command_interface name="digital_output1"/>
-        <state_interface name="digital_output1"/>    <!-- Needed to know current state of the output -->
-        <command_interface name="digital_output2"/>
-        <state_interface name="digital_output2"/>
-        <state_interface name="digital_input1"/>
-        <state_interface name="digital_input2"/>
+        <command_interface name="digital_output1" data_type="bool"/>
+        <state_interface name="digital_output1" data_type="bool"/>    <!-- Needed to know current state of the output -->
+        <command_interface name="digital_output2" data_type="bool"/>
+        <state_interface name="digital_output2" data_type="bool"/>
+        <state_interface name="digital_input1" data_type="bool"/>
+        <state_interface name="digital_input2" data_type="bool"/>
       </gpio>
       <gpio name="flange_analog_IOs">
-        <command_interface name="analog_output1"/>
-        <state_interface name="analog_output1">    <!-- Needed to know current state of the output -->
+        <command_interface name="analog_output1" data_type="uint16"/>
+        <state_interface name="analog_output1" data_type="uint16">    <!-- Needed to know current state of the output -->
           <param name="initial_value">3.1</param>  <!-- Optional initial value for mock_hardware -->
         </state_interface>
-        <state_interface name="analog_input1"/>
-        <state_interface name="analog_input2"/>
+        <state_interface name="analog_input1" data_type="uint16"/>
+        <state_interface name="analog_input2" data_type="uint16"/>
       </gpio>
       <gpio name="flange_vacuum">
-        <command_interface name="vacuum"/>
-        <state_interface name="vacuum"/>    <!-- Needed to know current state of the output -->
+        <command_interface name="vacuum" data_type="bool"/>
+        <state_interface name="vacuum" data_type="bool"/>    <!-- Needed to know current state of the output -->
       </gpio>
     </ros2_control>
 

hardware_interface/include/hardware_interface/handle.hpp

@@ -78,7 +78,6 @@ class Handle
     handle_name_(prefix_name_ + "/" + interface_name_),
     data_type_(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)
     {
@@ -97,17 +96,145 @@ class Handle
             initial_value, handle_name_, data_type_.to_string()));
       }
     }
+    else if (data_type_ == hardware_interface::HandleDataType::FLOAT32)
+    {
+      try
+      {
+        value_ptr_ = nullptr;
+        value_ = initial_value.empty()
+                   ? std::numeric_limits<float>::quiet_NaN()
+                   : static_cast<float>(hardware_interface::stof(initial_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_ = initial_value.empty() ? false : hardware_interface::parse_bool(initial_value);
+      try
+      {
+        value_ptr_ = nullptr;
+        value_ = initial_value.empty() ? false : hardware_interface::parse_bool(initial_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::UINT8)
+    {
+      try
+      {
+        value_ptr_ = nullptr;
+        value_ = initial_value.empty()
+                   ? static_cast<uint8_t>(std::numeric_limits<uint8_t>::max())
+                   : static_cast<uint8_t>(hardware_interface::stoui8(initial_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::INT8)
+    {
+      try
+      {
+        value_ptr_ = nullptr;
+        value_ = initial_value.empty()
+                   ? static_cast<int8_t>(std::numeric_limits<int8_t>::max())
+                   : static_cast<int8_t>(hardware_interface::stoi8(initial_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::UINT16)
+    {
+      try
+      {
+        value_ptr_ = nullptr;
+        value_ = initial_value.empty()
+                   ? static_cast<uint16_t>(std::numeric_limits<uint16_t>::max())
+                   : static_cast<uint16_t>(hardware_interface::stoui16(initial_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::INT16)
+    {
+      try
+      {
+        value_ptr_ = nullptr;
+        value_ = initial_value.empty()
+                   ? static_cast<int16_t>(std::numeric_limits<int16_t>::max())
+                   : static_cast<int16_t>(hardware_interface::stoi16(initial_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::UINT32)
+    {
+      try
+      {
+        value_ptr_ = nullptr;
+        value_ = initial_value.empty()
+                   ? static_cast<uint32_t>(std::numeric_limits<uint32_t>::max())
+                   : static_cast<uint32_t>(hardware_interface::stoui32(initial_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::INT32)
+    {
+      try
+      {
+        value_ptr_ = nullptr;
+        value_ = initial_value.empty()
+                   ? static_cast<int32_t>(std::numeric_limits<int32_t>::max())
+                   : static_cast<int32_t>(hardware_interface::stoi32(initial_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
     {
       throw std::runtime_error(
         fmt::format(
-          FMT_COMPILE(
-            "Invalid data type : '{}' for interface : {}. Supported types are double and bool."),
+          FMT_COMPILE("Invalid data type: '{}' for interface: {}. Check supported types."),
           data_type, handle_name_));
     }
   }
@@ -226,9 +353,31 @@ class Handle
     // 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_;
+      switch (data_type_)
+      {
+        case HandleDataType::DOUBLE:
+          THROW_ON_NULLPTR(value_ptr_);
+          return *value_ptr_;
+        case HandleDataType::BOOL:     // fallthrough
+        case HandleDataType::FLOAT32:  // fallthrough
+        case HandleDataType::UINT8:    // fallthrough
+        case HandleDataType::INT8:     // fallthrough
+        case HandleDataType::UINT16:   // fallthrough
+        case HandleDataType::INT16:    // fallthrough
+        case HandleDataType::UINT32:   // fallthrough
+        case HandleDataType::INT32:    // fallthrough
+          throw std::runtime_error(
+            fmt::format(
+              FMT_COMPILE(
+                "Data type: '{}' will not be casted to double for interface: {}. Use "
+                "get_optional<{}>() instead."),
+              data_type_.to_string(), get_name(), data_type_.to_string()));
+        default:
+          throw std::runtime_error(
+            fmt::format(
+              FMT_COMPILE("Data type: '{}' cannot be casted to double for interface: {}"),
+              data_type_.to_string(), get_name()));
+      }
     }
     try
     {
@@ -446,16 +595,20 @@ class Handle
           THROW_ON_NULLPTR(value_ptr_);
           value = *value_ptr_;
           return true;
-        case HandleDataType::BOOL:
-          // TODO(christophfroehlich): replace with RCLCPP_WARN_ONCE once
-          // https://github.com/ros2/rclcpp/issues/2587 is fixed
-          RCLCPP_WARN_ONCE(
-            rclcpp::get_logger(get_name()),
-            "Casting bool to double for interface: %s. Better use get_optional<bool>(). This will "
-            "only print once for all interfaces with this issue.",
-            get_name().c_str());
-          value = static_cast<double>(std::get<bool>(value_));
-          return true;
+        case HandleDataType::BOOL:     // fallthrough
+        case HandleDataType::FLOAT32:  // fallthrough
+        case HandleDataType::UINT8:    // fallthrough
+        case HandleDataType::INT8:     // fallthrough
+        case HandleDataType::UINT16:   // fallthrough
+        case HandleDataType::INT16:    // fallthrough
+        case HandleDataType::UINT32:   // fallthrough
+        case HandleDataType::INT32:    // fallthrough
+          throw std::runtime_error(
+            fmt::format(
+              FMT_COMPILE(
+                "Data type: '{}' will not be casted to double for interface: {}. Use "
+                "get_optional<{}>() instead."),
+              data_type_.to_string(), get_name(), data_type_.to_string()));
         default:
           throw std::runtime_error(
             fmt::format(