diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst
index 8eca65c52b..f5cd6b1196 100644
--- a/hardware_interface/doc/mock_components_userdoc.rst
+++ b/hardware_interface/doc/mock_components_userdoc.rst
@@ -76,7 +76,7 @@ calculate_dynamics (optional; boolean; default: false)
   Calculation of states from commands by using Euler-forward integration or finite differences.
 
 custom_interface_with_following_offset (optional; string; default: "")
-  Mapping of offsetted commands to a custom interface.
+  Mapping of offsetted commands to a custom interface (see ``position_state_following_offset``).
 
 disable_commands (optional; boolean; default: false)
   Disables mirroring commands to states.
@@ -92,7 +92,9 @@ mock_sensor_commands (optional; boolean; default: false)
   Those interfaces are usually used by a :ref:`forward controller <forward_command_controller_userdoc>` to provide access from ROS-world.
 
 position_state_following_offset (optional; double; default: 0.0)
-  Following offset added to the commanded values when mirrored to states. Only applied, if ``custom_interface_with_following_offset`` is false.
+  Following offset added to the state values when commands are mirrored to states.
+  If ``custom_interface_with_following_offset`` is empty, the offset is applied to the ``position`` state interface.
+  If a custom interface is set, the ``position`` state value + offset is applied to that interface.
 
 Per-Interface Parameters
 ########################
diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp
index 5919120f42..14e041f6d1 100644
--- a/hardware_interface/include/mock_components/generic_system.hpp
+++ b/hardware_interface/include/mock_components/generic_system.hpp
@@ -45,9 +45,11 @@ class GenericSystem : public hardware_interface::SystemInterface
   CallbackReturn on_init(
     const hardware_interface::HardwareComponentInterfaceParams & params) override;
 
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+  hardware_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
 
-  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+  std::vector<hardware_interface::InterfaceDescription>
+  export_unlisted_command_interface_descriptions() override;
 
   return_type prepare_command_mode_switch(
     const std::vector<std::string> & start_interfaces,
@@ -75,51 +77,22 @@ class GenericSystem : public hardware_interface::SystemInterface
   const std::vector<std::string> standard_interfaces_ = {
     hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY,
     hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT};
-
-  /// The size of this vector is (standard_interfaces_.size() x nr_joints)
-  std::vector<std::vector<double>> joint_command_values_;
-  std::vector<std::vector<double>> joint_state_values_;
+  std::vector<std::string> skip_interfaces_;
 
   std::vector<std::string> other_interfaces_;
-  /// The size of this vector is (other_interfaces_.size() x nr_joints)
-  std::vector<std::vector<double>> other_command_values_;
-  std::vector<std::vector<double>> other_state_values_;
-
   std::vector<std::string> sensor_interfaces_;
-  /// The size of this vector is (sensor_interfaces_.size() x nr_joints)
-  std::vector<std::vector<double>> sensor_mock_command_values_;
-  std::vector<std::vector<double>> sensor_state_values_;
-
-  std::vector<std::string> gpio_interfaces_;
-  /// The size of this vector is (gpio_interfaces_.size() x nr_joints)
-  std::vector<std::vector<double>> gpio_mock_command_values_;
-  std::vector<std::vector<double>> gpio_command_values_;
-  std::vector<std::vector<double>> gpio_state_values_;
+  std::vector<std::string> gpio_mock_interfaces_;
 
 private:
-  template <typename HandleType>
-  bool get_interface(
-    const std::string & name, const std::vector<std::string> & interface_list,
-    const std::string & interface_name, const size_t vector_index,
-    std::vector<std::vector<double>> & values, std::vector<HandleType> & interfaces);
-
-  void initialize_storage_vectors(
-    std::vector<std::vector<double>> & commands, std::vector<std::vector<double>> & states,
-    const std::vector<std::string> & interfaces,
-    const std::vector<hardware_interface::ComponentInfo> & component_infos);
-
-  template <typename InterfaceType>
   bool populate_interfaces(
     const std::vector<hardware_interface::ComponentInfo> & components,
-    std::vector<std::string> & interfaces, std::vector<std::vector<double>> & storage,
-    std::vector<InterfaceType> & target_interfaces, bool using_state_interfaces);
+    std::vector<hardware_interface::InterfaceDescription> & command_interface_descriptions) const;
 
   bool use_mock_gpio_command_interfaces_;
   bool use_mock_sensor_command_interfaces_;
 
   double position_state_following_offset_;
   std::string custom_interface_with_following_offset_;
-  size_t index_custom_interface_with_following_offset_;
 
   bool calculate_dynamics_;
   std::vector<size_t> joint_control_mode_;
diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp
index a05fc40142..6b44780b34 100644
--- a/hardware_interface/src/mock_components/generic_system.cpp
+++ b/hardware_interface/src/mock_components/generic_system.cpp
@@ -17,6 +17,7 @@
 #include "mock_components/generic_system.hpp"
 
 #include <algorithm>
+#include <array>
 #include <cmath>
 #include <iterator>
 #include <limits>
@@ -103,6 +104,13 @@ CallbackReturn GenericSystem::on_init(
   {
     calculate_dynamics_ = false;
   }
+  // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 2 are position,
+  // velocity, and acceleration interface
+  // Create a subvector of standard_interfaces_ with the given indices
+  for (size_t i = 0; i < (calculate_dynamics_ ? 3 : 1); ++i)
+  {
+    skip_interfaces_.push_back(standard_interfaces_[i]);
+  }
 
   // process parameters about state following
   position_state_following_offset_ = 0.0;
@@ -118,23 +126,6 @@ CallbackReturn GenericSystem::on_init(
       custom_interface_with_following_offset_ = it->second;
     }
   }
-  // it's extremely improbable that std::distance results in this value - therefore default
-  index_custom_interface_with_following_offset_ = std::numeric_limits<size_t>::max();
-
-  // Initialize storage for standard interfaces
-  initialize_storage_vectors(
-    joint_command_values_, joint_state_values_, standard_interfaces_, get_hardware_info().joints);
-  // set all values without initial values to 0
-  for (auto i = 0u; i < get_hardware_info().joints.size(); i++)
-  {
-    for (auto j = 0u; j < standard_interfaces_.size(); j++)
-    {
-      if (std::isnan(joint_state_values_[j][i]))
-      {
-        joint_state_values_[j][i] = 0.0;
-      }
-    }
-  }
 
   // search for non-standard joint interfaces
   for (const auto & joint : get_hardware_info().joints)
@@ -146,10 +137,6 @@ CallbackReturn GenericSystem::on_init(
     populate_non_standard_interfaces(joint.state_interfaces, other_interfaces_);
   }
 
-  // Initialize storage for non-standard interfaces
-  initialize_storage_vectors(
-    other_command_values_, other_state_values_, other_interfaces_, get_hardware_info().joints);
-
   // when following offset is used on custom interface then find its index
   if (!custom_interface_with_following_offset_.empty())
   {
@@ -157,12 +144,9 @@ CallbackReturn GenericSystem::on_init(
       other_interfaces_.begin(), other_interfaces_.end(), custom_interface_with_following_offset_);
     if (if_it != other_interfaces_.end())
     {
-      index_custom_interface_with_following_offset_ =
-        static_cast<size_t>(std::distance(other_interfaces_.begin(), if_it));
       RCLCPP_INFO(
-        get_logger(), "Custom interface with following offset '%s' found at index: %zu.",
-        custom_interface_with_following_offset_.c_str(),
-        index_custom_interface_with_following_offset_);
+        get_logger(), "Custom interface with following offset '%s' found.",
+        custom_interface_with_following_offset_.c_str());
     }
     else
     {
@@ -185,147 +169,27 @@ CallbackReturn GenericSystem::on_init(
       }
     }
   }
-  initialize_storage_vectors(
-    sensor_mock_command_values_, sensor_state_values_, sensor_interfaces_,
-    get_hardware_info().sensors);
-
-  // search for gpio interfaces
-  for (const auto & gpio : get_hardware_info().gpios)
-  {
-    // populate non-standard command interfaces to gpio_interfaces_
-    populate_non_standard_interfaces(gpio.command_interfaces, gpio_interfaces_);
-
-    // populate non-standard state interfaces to gpio_interfaces_
-    populate_non_standard_interfaces(gpio.state_interfaces, gpio_interfaces_);
-  }
-
-  // Mock gpio command interfaces
-  if (use_mock_gpio_command_interfaces_)
-  {
-    initialize_storage_vectors(
-      gpio_mock_command_values_, gpio_state_values_, gpio_interfaces_, get_hardware_info().gpios);
-  }
-  // Real gpio command interfaces
-  else
-  {
-    initialize_storage_vectors(
-      gpio_command_values_, gpio_state_values_, gpio_interfaces_, get_hardware_info().gpios);
-  }
 
   return CallbackReturn::SUCCESS;
 }
 
-std::vector<hardware_interface::StateInterface> GenericSystem::export_state_interfaces()
+std::vector<hardware_interface::InterfaceDescription>
+GenericSystem::export_unlisted_command_interface_descriptions()
 {
-  std::vector<hardware_interface::StateInterface> state_interfaces;
-
-  // Joints' state interfaces
-  for (auto i = 0u; i < get_hardware_info().joints.size(); i++)
-  {
-    const auto & joint = get_hardware_info().joints[i];
-    for (const auto & interface : joint.state_interfaces)
-    {
-      // Add interface: if not in the standard list then use "other" interface list
-      if (!get_interface(
-            joint.name, standard_interfaces_, interface.name, i, joint_state_values_,
-            state_interfaces))
-      {
-        if (!get_interface(
-              joint.name, other_interfaces_, interface.name, i, other_state_values_,
-              state_interfaces))
-        {
-          throw std::runtime_error(
-            "Interface is not found in the standard nor other list. "
-            "This should never happen!");
-        }
-      }
-    }
-  }
-
-  // Sensor state interfaces
-  if (!populate_interfaces(
-        get_hardware_info().sensors, sensor_interfaces_, sensor_state_values_, state_interfaces,
-        true))
-  {
-    throw std::runtime_error(
-      "Interface is not found in the standard nor other list. This should never happen!");
-  };
-
-  // GPIO state interfaces
-  if (!populate_interfaces(
-        get_hardware_info().gpios, gpio_interfaces_, gpio_state_values_, state_interfaces, true))
-  {
-    throw std::runtime_error("Interface is not found in the gpio list. This should never happen!");
-  }
-
-  return state_interfaces;
-}
-
-std::vector<hardware_interface::CommandInterface> GenericSystem::export_command_interfaces()
-{
-  std::vector<hardware_interface::CommandInterface> command_interfaces;
-
-  // Joints' state interfaces
-  for (size_t i = 0; i < get_hardware_info().joints.size(); ++i)
-  {
-    const auto & joint = get_hardware_info().joints[i];
-    for (const auto & interface : joint.command_interfaces)
-    {
-      // Add interface: if not in the standard list than use "other" interface list
-      if (!get_interface(
-            joint.name, standard_interfaces_, interface.name, i, joint_command_values_,
-            command_interfaces))
-      {
-        if (!get_interface(
-              joint.name, other_interfaces_, interface.name, i, other_command_values_,
-              command_interfaces))
-        {
-          throw std::runtime_error(
-            "Interface is not found in the standard nor other list. "
-            "This should never happen!");
-        }
-      }
-    }
-  }
-  // Set position control mode per default
-  joint_control_mode_.resize(get_hardware_info().joints.size(), POSITION_INTERFACE_INDEX);
-
+  std::vector<hardware_interface::InterfaceDescription> command_interface_descriptions;
   // Mock sensor command interfaces
   if (use_mock_sensor_command_interfaces_)
   {
-    if (!populate_interfaces(
-          get_hardware_info().sensors, sensor_interfaces_, sensor_mock_command_values_,
-          command_interfaces, true))
-    {
-      throw std::runtime_error(
-        "Interface is not found in the standard nor other list. This should never happen!");
-    }
+    populate_interfaces(get_hardware_info().sensors, command_interface_descriptions);
   }
 
   // Mock gpio command interfaces (consider all state interfaces for command interfaces)
   if (use_mock_gpio_command_interfaces_)
   {
-    if (!populate_interfaces(
-          get_hardware_info().gpios, gpio_interfaces_, gpio_mock_command_values_,
-          command_interfaces, true))
-    {
-      throw std::runtime_error(
-        "Interface is not found in the gpio list. This should never happen!");
-    }
-  }
-  // GPIO command interfaces (real command interfaces)
-  else
-  {
-    if (!populate_interfaces(
-          get_hardware_info().gpios, gpio_interfaces_, gpio_command_values_, command_interfaces,
-          false))
-    {
-      throw std::runtime_error(
-        "Interface is not found in the gpio list. This should never happen!");
-    }
+    populate_interfaces(get_hardware_info().gpios, command_interface_descriptions);
   }
 
-  return command_interfaces;
+  return command_interface_descriptions;
 }
 
 return_type GenericSystem::prepare_command_mode_switch(
@@ -433,6 +297,9 @@ return_type GenericSystem::perform_command_mode_switch(
     return hardware_interface::return_type::OK;
   }
 
+  // Set position control mode per default
+  joint_control_mode_.resize(get_hardware_info().joints.size(), POSITION_INTERFACE_INDEX);
+
   for (const auto & key : start_interfaces)
   {
     // check if interface is joint
@@ -445,7 +312,6 @@ return_type GenericSystem::perform_command_mode_switch(
     {
       const size_t joint_index =
         static_cast<size_t>(std::distance(info.joints.begin(), joint_it_found));
-
       if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION)
       {
         joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX;
@@ -464,6 +330,49 @@ return_type GenericSystem::perform_command_mode_switch(
   return hardware_interface::return_type::OK;
 }
 
+hardware_interface::CallbackReturn GenericSystem::on_activate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  for (const auto & joint_state : joint_state_interfaces_)
+  {
+    const std::string & name = joint_state.second.get_name();
+    switch (joint_state.second.get_data_type())
+    {
+      case hardware_interface::HandleDataType::DOUBLE:
+      {
+        double val = joint_state.second.interface_info.initial_value.empty()
+                       ? 0.0
+                       : hardware_interface::stod(joint_state.second.interface_info.initial_value);
+        if (
+          joint_state.second.get_interface_name() ==
+            standard_interfaces_[POSITION_INTERFACE_INDEX] &&
+          custom_interface_with_following_offset_.empty())
+        {
+          val += position_state_following_offset_;
+        }
+        set_state(name, val);
+        break;
+      }
+      case hardware_interface::HandleDataType::BOOL:
+      {
+        bool bval =
+          joint_state.second.interface_info.initial_value.empty()
+            ? false
+            : hardware_interface::parse_bool(joint_state.second.interface_info.initial_value);
+        set_state<bool>(name, bval);
+        break;
+      }
+      default:
+      {
+        RCLCPP_WARN(
+          get_logger(), "Data type of joint state interface '%s' will not be handled.",
+          joint_state.second.get_interface_name().c_str());
+      }
+    }
+  }
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
 return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
 {
   if (command_propagation_disabled_)
@@ -472,233 +381,285 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
     return return_type::OK;
   }
 
-  auto mirror_command_to_state =
-    [](auto & states_, auto commands_, size_t start_index = 0) -> return_type
+  auto mirror_command_to_state = [this](const auto & name, const auto & data_type) -> return_type
   {
-    for (size_t i = start_index; i < states_.size(); ++i)
+    switch (data_type)
     {
-      for (size_t j = 0; j < states_[i].size(); ++j)
+      case hardware_interface::HandleDataType::DOUBLE:
       {
-        if (!std::isnan(commands_[i][j]))
+        auto cmd = get_command(name);
+        if (std::isinf(cmd))
         {
-          states_[i][j] = commands_[i][j];
+          return return_type::ERROR;
         }
-        if (std::isinf(commands_[i][j]))
+        if (std::isfinite(cmd))
         {
-          return return_type::ERROR;
+          set_state(name, cmd);
         }
+        break;
+      }
+      case hardware_interface::HandleDataType::BOOL:
+      {
+        set_state<bool>(name, get_command<bool>(name));
+        break;
       }
+      default:
+      {
+      }
+        // not handling other types
     }
     return return_type::OK;
   };
 
-  for (size_t j = 0; j < joint_state_values_[POSITION_INTERFACE_INDEX].size(); ++j)
+  for (size_t j = 0; j < get_hardware_info().joints.size(); ++j)
   {
     if (calculate_dynamics_)
     {
+      std::array<double, 3> joint_state_values_ = {
+        {std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(),
+         std::numeric_limits<double>::quiet_NaN()}};
+      std::array<double, 3> joint_command_values_ = {
+        {std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(),
+         std::numeric_limits<double>::quiet_NaN()}};
+      const auto & joint_name = get_hardware_info().joints[j].name;
+      for (size_t i = 0; i < 3; ++i)
+      {
+        const auto full_name = joint_name + "/" + standard_interfaces_[i];
+        if (joint_command_interfaces_.find(full_name) != joint_command_interfaces_.end())
+        {
+          joint_command_values_[i] =
+            get_command(joint_command_interfaces_.at(full_name).get_name());
+        }
+        if (joint_state_interfaces_.find(full_name) != joint_state_interfaces_.end())
+        {
+          joint_state_values_[i] = get_state(joint_state_interfaces_.at(full_name).get_name());
+        }
+      }
+
       switch (joint_control_mode_[j])
       {
         case ACCELERATION_INTERFACE_INDEX:
         {
           // currently we do backward integration
-          joint_state_values_[POSITION_INTERFACE_INDEX][j] +=  // apply offset to positions only
-            joint_state_values_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() +
+          joint_state_values_[POSITION_INTERFACE_INDEX] +=  // apply offset to positions only
+            joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds() +
             (custom_interface_with_following_offset_.empty() ? position_state_following_offset_
                                                              : 0.0);
 
-          joint_state_values_[VELOCITY_INTERFACE_INDEX][j] +=
-            joint_state_values_[ACCELERATION_INTERFACE_INDEX][j] * period.seconds();
+          joint_state_values_[VELOCITY_INTERFACE_INDEX] +=
+            joint_state_values_[ACCELERATION_INTERFACE_INDEX] * period.seconds();
 
-          if (!std::isnan(joint_command_values_[ACCELERATION_INTERFACE_INDEX][j]))
+          if (!std::isnan(joint_command_values_[ACCELERATION_INTERFACE_INDEX]))
           {
-            joint_state_values_[ACCELERATION_INTERFACE_INDEX][j] =
-              joint_command_values_[ACCELERATION_INTERFACE_INDEX][j];
+            joint_state_values_[ACCELERATION_INTERFACE_INDEX] =
+              joint_command_values_[ACCELERATION_INTERFACE_INDEX];
           }
           break;
         }
         case VELOCITY_INTERFACE_INDEX:
         {
           // currently we do backward integration
-          joint_state_values_[POSITION_INTERFACE_INDEX][j] +=  // apply offset to positions only
-            joint_state_values_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() +
+          joint_state_values_[POSITION_INTERFACE_INDEX] +=  // apply offset to positions only
+            joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds() +
             (custom_interface_with_following_offset_.empty() ? position_state_following_offset_
                                                              : 0.0);
 
-          if (!std::isnan(joint_command_values_[VELOCITY_INTERFACE_INDEX][j]))
+          if (!std::isnan(joint_command_values_[VELOCITY_INTERFACE_INDEX]))
           {
-            const double old_velocity = joint_state_values_[VELOCITY_INTERFACE_INDEX][j];
+            const double old_velocity = joint_state_values_[VELOCITY_INTERFACE_INDEX];
 
-            joint_state_values_[VELOCITY_INTERFACE_INDEX][j] =
-              joint_command_values_[VELOCITY_INTERFACE_INDEX][j];
+            joint_state_values_[VELOCITY_INTERFACE_INDEX] =
+              joint_command_values_[VELOCITY_INTERFACE_INDEX];
 
-            joint_state_values_[ACCELERATION_INTERFACE_INDEX][j] =
-              (joint_state_values_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds();
+            joint_state_values_[ACCELERATION_INTERFACE_INDEX] =
+              (joint_state_values_[VELOCITY_INTERFACE_INDEX] - old_velocity) / period.seconds();
           }
           break;
         }
         case POSITION_INTERFACE_INDEX:
         {
-          if (!std::isnan(joint_command_values_[POSITION_INTERFACE_INDEX][j]))
+          if (!std::isnan(joint_command_values_[POSITION_INTERFACE_INDEX]))
           {
-            const double old_position = joint_state_values_[POSITION_INTERFACE_INDEX][j];
-            const double old_velocity = joint_state_values_[VELOCITY_INTERFACE_INDEX][j];
+            const double old_position = joint_state_values_[POSITION_INTERFACE_INDEX];
+            const double old_velocity = joint_state_values_[VELOCITY_INTERFACE_INDEX];
 
-            joint_state_values_[POSITION_INTERFACE_INDEX][j] =  // apply offset to positions only
-              joint_command_values_[POSITION_INTERFACE_INDEX][j] +
+            joint_state_values_[POSITION_INTERFACE_INDEX] =  // apply offset to positions only
+              joint_command_values_[POSITION_INTERFACE_INDEX] +
               (custom_interface_with_following_offset_.empty() ? position_state_following_offset_
                                                                : 0.0);
 
-            joint_state_values_[VELOCITY_INTERFACE_INDEX][j] =
-              (joint_state_values_[POSITION_INTERFACE_INDEX][j] - old_position) / period.seconds();
+            joint_state_values_[VELOCITY_INTERFACE_INDEX] =
+              (joint_state_values_[POSITION_INTERFACE_INDEX] - old_position) / period.seconds();
 
-            joint_state_values_[ACCELERATION_INTERFACE_INDEX][j] =
-              (joint_state_values_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds();
+            joint_state_values_[ACCELERATION_INTERFACE_INDEX] =
+              (joint_state_values_[VELOCITY_INTERFACE_INDEX] - old_velocity) / period.seconds();
           }
           break;
         }
       }
+      // mirror them back
+      for (size_t i = 0; i < 3; ++i)
+      {
+        if (!std::isnan(joint_state_values_[i]))
+        {
+          set_state(joint_name + "/" + standard_interfaces_[i], joint_state_values_[i]);
+        }
+      }
     }
     else
     {
-      for (size_t k = 0; k < joint_state_values_[POSITION_INTERFACE_INDEX].size(); ++k)
+      for (const auto & joint_command : joint_commands_)
       {
-        if (!std::isnan(joint_command_values_[POSITION_INTERFACE_INDEX][k]))
+        if (
+          joint_command.get()->get_interface_name() ==
+          standard_interfaces_[POSITION_INTERFACE_INDEX])
         {
-          joint_state_values_[POSITION_INTERFACE_INDEX][k] =  // apply offset to positions only
-            joint_command_values_[POSITION_INTERFACE_INDEX][k] +
-            (custom_interface_with_following_offset_.empty() ? position_state_following_offset_
-                                                             : 0.0);
+          const std::string & name = joint_command.get()->get_name();
+          if (joint_state_interfaces_.find(name) != joint_state_interfaces_.end())
+          {
+            set_state(
+              name, get_command(name) + (custom_interface_with_following_offset_.empty()
+                                           ? position_state_following_offset_
+                                           : 0.0));
+          }
         }
       }
     }
   }
 
-  // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position,
-  // velocity, and acceleration interface
-  if (
-    mirror_command_to_state(
-      joint_state_values_, joint_command_values_, calculate_dynamics_ ? 3 : 1) != return_type::OK)
+  // do loopback on all other interfaces
+  for (const auto & joint_state : joint_states_)
   {
-    return return_type::ERROR;
+    if (
+      std::find(
+        skip_interfaces_.begin(), skip_interfaces_.end(),
+        joint_state.get()->get_interface_name()) != skip_interfaces_.end())
+    {
+      continue;
+    }
+    const std::string & full_interface_name = joint_state.get()->get_name();
+    if (joint_command_interfaces_.find(full_interface_name) != joint_command_interfaces_.end())
+    {
+      if (
+        mirror_command_to_state(full_interface_name, joint_state.get()->get_data_type()) !=
+        return_type::OK)
+      {
+        return return_type::ERROR;
+      }
+    }
+    if (custom_interface_with_following_offset_ == joint_state.get()->get_interface_name())
+    {
+      const auto cmd = get_command(
+                         joint_state.get()->get_prefix_name() + "/" +
+                         standard_interfaces_[POSITION_INTERFACE_INDEX]) +
+                       position_state_following_offset_;
+      set_state(full_interface_name, cmd);
+    }
   }
 
+  // Update mimic joints
+  const auto & joints = get_hardware_info().joints;
   for (const auto & mimic_joint : get_hardware_info().mimic_joints)
   {
-    for (auto i = 0u; i < joint_state_values_.size(); ++i)
+    const auto & mimic_joint_name = joints.at(mimic_joint.joint_index).name;
+    const auto & mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name;
+    if (
+      joint_state_interfaces_.find(
+        mimic_joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX]) !=
+      joint_state_interfaces_.end())
     {
-      joint_state_values_[i][mimic_joint.joint_index] =
+      set_state(
+        mimic_joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX],
         mimic_joint.offset +
-        mimic_joint.multiplier * joint_state_values_[i][mimic_joint.mimicked_joint_index];
+          mimic_joint.multiplier *
+            get_state(mimicked_joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX]));
     }
-  }
-
-  for (size_t i = 0; i < other_state_values_.size(); ++i)
-  {
-    for (size_t j = 0; j < other_state_values_[i].size(); ++j)
+    if (
+      joint_state_interfaces_.find(
+        mimic_joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX]) !=
+      joint_state_interfaces_.end())
     {
-      if (
-        i == index_custom_interface_with_following_offset_ &&
-        !std::isnan(joint_command_values_[POSITION_INTERFACE_INDEX][j]))
-      {
-        other_state_values_[i][j] =
-          joint_command_values_[POSITION_INTERFACE_INDEX][j] + position_state_following_offset_;
-      }
-      else if (!std::isnan(other_command_values_[i][j]))
-      {
-        other_state_values_[i][j] = other_command_values_[i][j];
-      }
+      set_state(
+        mimic_joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX],
+        mimic_joint.multiplier *
+          get_state(mimicked_joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX]));
+    }
+    if (
+      joint_state_interfaces_.find(
+        mimic_joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX]) !=
+      joint_state_interfaces_.end())
+    {
+      set_state(
+        mimic_joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX],
+        mimic_joint.multiplier *
+          get_state(
+            mimicked_joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX]));
     }
   }
 
   if (use_mock_sensor_command_interfaces_)
   {
-    mirror_command_to_state(sensor_state_values_, sensor_mock_command_values_);
+    // do loopback on all sensor interfaces as we have exported them all
+    for (const auto & sensor_state : sensor_states_)
+    {
+      const std::string & name = sensor_state.get()->get_name();
+      mirror_command_to_state(name, sensor_state.get()->get_data_type());
+    }
   }
 
   if (use_mock_gpio_command_interfaces_)
   {
-    mirror_command_to_state(gpio_state_values_, gpio_mock_command_values_);
+    // do loopback on all gpio interfaces as we have exported them all
+    // from gpio_command_interfaces_ maybe?
+    for (const auto & gpio_state : gpio_states_)
+    {
+      const std::string & name = gpio_state.get()->get_name();
+      mirror_command_to_state(name, gpio_state.get()->get_data_type());
+    }
   }
   else
   {
-    // do loopback on all gpio interfaces
-    mirror_command_to_state(gpio_state_values_, gpio_command_values_);
-  }
-
-  return return_type::OK;
-}
-
-// Private methods
-template <typename HandleType>
-bool GenericSystem::get_interface(
-  const std::string & name, const std::vector<std::string> & interface_list,
-  const std::string & interface_name, const size_t vector_index,
-  std::vector<std::vector<double>> & values, std::vector<HandleType> & interfaces)
-{
-  auto it = std::find(interface_list.begin(), interface_list.end(), interface_name);
-  if (it != interface_list.end())
-  {
-    auto j = static_cast<size_t>(std::distance(interface_list.begin(), it));
-    interfaces.emplace_back(name, *it, &values[j][vector_index]);
-    return true;
-  }
-  return false;
-}
-
-void GenericSystem::initialize_storage_vectors(
-  std::vector<std::vector<double>> & commands, std::vector<std::vector<double>> & states,
-  const std::vector<std::string> & interfaces,
-  const std::vector<hardware_interface::ComponentInfo> & component_infos)
-{
-  // Initialize storage for all joints, regardless of their existence
-  commands.resize(interfaces.size());
-  states.resize(interfaces.size());
-  for (auto i = 0u; i < interfaces.size(); i++)
-  {
-    commands[i].resize(component_infos.size(), std::numeric_limits<double>::quiet_NaN());
-    states[i].resize(component_infos.size(), std::numeric_limits<double>::quiet_NaN());
-  }
-
-  // Initialize with values from URDF
-  for (auto i = 0u; i < component_infos.size(); i++)
-  {
-    const auto & component = component_infos[i];
-    for (const auto & interface : component.state_interfaces)
+    // do loopback on all gpio interfaces, where they exist
+    for (const auto & gpio_command : gpio_commands_)
     {
-      auto it = std::find(interfaces.begin(), interfaces.end(), interface.name);
-
-      // If interface name is found in the interfaces list
-      if (it != interfaces.end())
+      const std::string & name = gpio_command.get()->get_name();
+      if (gpio_state_interfaces_.find(name) != gpio_state_interfaces_.end())
       {
-        auto index = static_cast<size_t>(std::distance(interfaces.begin(), it));
-
-        // Check the initial_value param is used
-        if (!interface.initial_value.empty())
-        {
-          states[index][i] = hardware_interface::stod(interface.initial_value);
-        }
+        mirror_command_to_state(name, gpio_command.get()->get_data_type());
       }
     }
   }
+
+  return return_type::OK;
 }
 
-template <typename InterfaceType>
+// Private methods
 bool GenericSystem::populate_interfaces(
   const std::vector<hardware_interface::ComponentInfo> & components,
-  std::vector<std::string> & interface_names, std::vector<std::vector<double>> & storage,
-  std::vector<InterfaceType> & target_interfaces, bool using_state_interfaces)
+  std::vector<hardware_interface::InterfaceDescription> & command_interface_descriptions) const
 {
-  for (auto i = 0u; i < components.size(); i++)
+  for (const auto & component : components)
   {
-    const auto & component = components[i];
-    const auto interfaces =
-      (using_state_interfaces) ? component.state_interfaces : component.command_interfaces;
-    for (const auto & interface : interfaces)
+    for (const auto & state_interface : component.state_interfaces)
     {
-      if (!get_interface(
-            component.name, interface_names, interface.name, i, storage, target_interfaces))
+      // add to state interface to command interface list if not already there
+      if (
+        std::find_if(
+          command_interface_descriptions.begin(), command_interface_descriptions.end(),
+          [&component, &state_interface](const auto & desc)
+          { return (desc.get_name() == (component.name + "/" + state_interface.name)); }) ==
+          command_interface_descriptions.end() &&
+        std::find_if(
+          component.command_interfaces.begin(), component.command_interfaces.end(),
+          [&component, &state_interface](const auto & cmd_if)
+          {
+            return (
+              (component.name + "/" + cmd_if.name) ==
+              (component.name + "/" + state_interface.name));
+          }) == component.command_interfaces.end())
       {
-        return false;
+        hardware_interface::InterfaceDescription description(component.name, state_interface);
+        command_interface_descriptions.push_back(description);
       }
     }
   }
diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp
index e88df213cd..d69b901630 100644
--- a/hardware_interface/test/mock_components/test_generic_system.cpp
+++ b/hardware_interface/test/mock_components/test_generic_system.cpp
@@ -46,7 +46,7 @@ class TestGenericSystem : public ::testing::Test
   void test_generic_system_with_mock_sensor_commands(
     std::string & urdf, const std::string & component_name);
   void test_generic_system_with_mock_gpio_commands(
-    std::string & urdf, const std::string & component_name);
+    std::string & urdf, const std::string & component_name, const bool data_type_bool = false);
 
 protected:
   void SetUp() override
@@ -425,6 +425,42 @@ class TestGenericSystem : public ::testing::Test
   </ros2_control>
 )";
 
+    valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command_ =
+      R"(
+  <ros2_control name="MockHardwareSystem" type="system">
+    <hardware>
+      <plugin>mock_components/GenericSystem</plugin>
+      <param name="mock_gpio_commands">true</param>
+    </hardware>
+    <joint name="joint1">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <state_interface name="position">
+        <param name="initial_value">3.45</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="joint2">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <state_interface name="position">
+        <param name="initial_value">2.78</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <gpio name="flange_analog_IOs">
+      <command_interface name="analog_output1" data_type="double"/>
+      <state_interface name="analog_output1"/>
+      <state_interface name="analog_input1"/>
+      <state_interface name="analog_input2"/>
+    </gpio>
+    <gpio name="flange_vacuum">
+      <command_interface name="vacuum" data_type="bool"/>
+      <state_interface name="vacuum" data_type="bool"/>
+    </gpio>
+  </ros2_control>
+)";
+
     valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ =
       R"(
   <ros2_control name="MockHardwareSystem" type="system">
@@ -661,6 +697,7 @@ class TestGenericSystem : public ::testing::Test
   std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_;
   std::string valid_urdf_ros2_control_system_robot_with_gpio_;
   std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_;
+  std::string valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command_;
   std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_;
   std::string sensor_with_initial_value_;
   std::string gpio_with_initial_value_;
@@ -843,18 +880,18 @@ void generic_system_functional_test(
   TestableResourceManager rm(node, urdf);
   // check is hardware is configured
   auto status_map = rm.get_components_status();
-  EXPECT_EQ(
+  ASSERT_EQ(
     status_map[component_name].state.label(),
     hardware_interface::lifecycle_state_names::UNCONFIGURED);
   EXPECT_EQ(status_map[component_name].rw_rate, 100u);
   configure_components(rm, {component_name});
   status_map = rm.get_components_status();
-  EXPECT_EQ(
+  ASSERT_EQ(
     status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE);
   EXPECT_EQ(status_map[component_name].rw_rate, 100u);
   activate_components(rm, {component_name});
   status_map = rm.get_components_status();
-  EXPECT_EQ(
+  ASSERT_EQ(
     status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE);
   EXPECT_EQ(status_map[component_name].rw_rate, 100u);
 
@@ -869,14 +906,14 @@ void generic_system_functional_test(
   hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity");
 
   // State interfaces without initial value are set to 0
-  ASSERT_EQ(3.45, j1p_s.get_optional().value());
-  ASSERT_EQ(0.0, j1v_s.get_optional().value());
-  ASSERT_EQ(2.78, j2p_s.get_optional().value());
-  ASSERT_EQ(0.0, j2v_s.get_optional().value());
-  ASSERT_TRUE(std::isnan(j1p_c.get_optional().value()));
-  ASSERT_TRUE(std::isnan(j1v_c.get_optional().value()));
-  ASSERT_TRUE(std::isnan(j2p_c.get_optional().value()));
-  ASSERT_TRUE(std::isnan(j2v_c.get_optional().value()));
+  EXPECT_EQ(3.45 + offset, j1p_s.get_optional().value());
+  EXPECT_EQ(0.0, j1v_s.get_optional().value());
+  EXPECT_EQ(2.78 + offset, j2p_s.get_optional().value());
+  EXPECT_EQ(0.0, j2v_s.get_optional().value());
+  EXPECT_TRUE(std::isnan(j1p_c.get_optional().value()));
+  EXPECT_TRUE(std::isnan(j1v_c.get_optional().value()));
+  EXPECT_TRUE(std::isnan(j2p_c.get_optional().value()));
+  EXPECT_TRUE(std::isnan(j2v_c.get_optional().value()));
 
   // set some new values in commands
   ASSERT_TRUE(j1p_c.set_value(0.11));
@@ -885,36 +922,36 @@ void generic_system_functional_test(
   ASSERT_TRUE(j2v_c.set_value(0.44));
 
   // State values should not be changed
-  ASSERT_EQ(3.45, j1p_s.get_optional().value());
-  ASSERT_EQ(0.0, j1v_s.get_optional().value());
-  ASSERT_EQ(2.78, j2p_s.get_optional().value());
-  ASSERT_EQ(0.0, j2v_s.get_optional().value());
-  ASSERT_EQ(0.11, j1p_c.get_optional().value());
-  ASSERT_EQ(0.22, j1v_c.get_optional().value());
-  ASSERT_EQ(0.33, j2p_c.get_optional().value());
-  ASSERT_EQ(0.44, j2v_c.get_optional().value());
+  EXPECT_EQ(3.45 + offset, j1p_s.get_optional().value());
+  EXPECT_EQ(0.0, j1v_s.get_optional().value());
+  EXPECT_EQ(2.78 + offset, j2p_s.get_optional().value());
+  EXPECT_EQ(0.0, j2v_s.get_optional().value());
+  EXPECT_EQ(0.11, j1p_c.get_optional().value());
+  EXPECT_EQ(0.22, j1v_c.get_optional().value());
+  EXPECT_EQ(0.33, j2p_c.get_optional().value());
+  EXPECT_EQ(0.44, j2v_c.get_optional().value());
 
   // write() does not change values
-  EXPECT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK);
-  ASSERT_EQ(3.45, j1p_s.get_optional().value());
-  ASSERT_EQ(0.0, j1v_s.get_optional().value());
-  ASSERT_EQ(2.78, j2p_s.get_optional().value());
-  ASSERT_EQ(0.0, j2v_s.get_optional().value());
-  ASSERT_EQ(0.11, j1p_c.get_optional().value());
-  ASSERT_EQ(0.22, j1v_c.get_optional().value());
-  ASSERT_EQ(0.33, j2p_c.get_optional().value());
-  ASSERT_EQ(0.44, j2v_c.get_optional().value());
+  ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK);
+  EXPECT_EQ(3.45 + offset, j1p_s.get_optional().value());
+  EXPECT_EQ(0.0, j1v_s.get_optional().value());
+  EXPECT_EQ(2.78 + offset, j2p_s.get_optional().value());
+  EXPECT_EQ(0.0, j2v_s.get_optional().value());
+  EXPECT_EQ(0.11, j1p_c.get_optional().value());
+  EXPECT_EQ(0.22, j1v_c.get_optional().value());
+  EXPECT_EQ(0.33, j2p_c.get_optional().value());
+  EXPECT_EQ(0.44, j2v_c.get_optional().value());
 
   // read() mirrors commands + offset to states
-  EXPECT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK);
-  ASSERT_EQ(0.11 + offset, j1p_s.get_optional().value());
-  ASSERT_EQ(0.22, j1v_s.get_optional().value());
-  ASSERT_EQ(0.33 + offset, j2p_s.get_optional().value());
-  ASSERT_EQ(0.44, j2v_s.get_optional().value());
-  ASSERT_EQ(0.11, j1p_c.get_optional().value());
-  ASSERT_EQ(0.22, j1v_c.get_optional().value());
-  ASSERT_EQ(0.33, j2p_c.get_optional().value());
-  ASSERT_EQ(0.44, j2v_c.get_optional().value());
+  ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK);
+  EXPECT_EQ(0.11 + offset, j1p_s.get_optional().value());
+  EXPECT_EQ(0.22, j1v_s.get_optional().value());
+  EXPECT_EQ(0.33 + offset, j2p_s.get_optional().value());
+  EXPECT_EQ(0.44, j2v_s.get_optional().value());
+  EXPECT_EQ(0.11, j1p_c.get_optional().value());
+  EXPECT_EQ(0.22, j1v_c.get_optional().value());
+  EXPECT_EQ(0.33, j2p_c.get_optional().value());
+  EXPECT_EQ(0.44, j2v_c.get_optional().value());
 
   // set some new values in commands
   ASSERT_TRUE(j1p_c.set_value(0.55));
@@ -923,14 +960,14 @@ void generic_system_functional_test(
   ASSERT_TRUE(j2v_c.set_value(0.88));
 
   // state values should not be changed
-  ASSERT_EQ(0.11 + offset, j1p_s.get_optional().value());
-  ASSERT_EQ(0.22, j1v_s.get_optional().value());
-  ASSERT_EQ(0.33 + offset, j2p_s.get_optional().value());
-  ASSERT_EQ(0.44, j2v_s.get_optional().value());
-  ASSERT_EQ(0.55, j1p_c.get_optional().value());
-  ASSERT_EQ(0.66, j1v_c.get_optional().value());
-  ASSERT_EQ(0.77, j2p_c.get_optional().value());
-  ASSERT_EQ(0.88, j2v_c.get_optional().value());
+  EXPECT_EQ(0.11 + offset, j1p_s.get_optional().value());
+  EXPECT_EQ(0.22, j1v_s.get_optional().value());
+  EXPECT_EQ(0.33 + offset, j2p_s.get_optional().value());
+  EXPECT_EQ(0.44, j2v_s.get_optional().value());
+  EXPECT_EQ(0.55, j1p_c.get_optional().value());
+  EXPECT_EQ(0.66, j1v_c.get_optional().value());
+  EXPECT_EQ(0.77, j2p_c.get_optional().value());
+  EXPECT_EQ(0.88, j2v_c.get_optional().value());
 
   deactivate_components(rm, {component_name});
   status_map = rm.get_components_status();
@@ -949,17 +986,17 @@ void generic_system_error_group_test(
   auto status_map = rm.get_components_status();
   for (auto component : {component1, component2})
   {
-    EXPECT_EQ(
+    ASSERT_EQ(
       status_map[component].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED);
     EXPECT_EQ(status_map[component].rw_rate, 200u);
     configure_components(rm, {component});
     status_map = rm.get_components_status();
-    EXPECT_EQ(
+    ASSERT_EQ(
       status_map[component].state.label(), hardware_interface::lifecycle_state_names::INACTIVE);
     EXPECT_EQ(status_map[component].rw_rate, 200u);
     activate_components(rm, {component});
     status_map = rm.get_components_status();
-    EXPECT_EQ(
+    ASSERT_EQ(
       status_map[component].state.label(), hardware_interface::lifecycle_state_names::ACTIVE);
     EXPECT_EQ(status_map[component].rw_rate, 200u);
   }
@@ -975,10 +1012,10 @@ void generic_system_error_group_test(
   hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity");
 
   // State interfaces without initial value are set to 0
-  ASSERT_EQ(3.45, j1p_s.get_optional().value());
-  ASSERT_EQ(0.0, j1v_s.get_optional().value());
-  ASSERT_EQ(2.78, j2p_s.get_optional().value());
-  ASSERT_EQ(0.0, j2v_s.get_optional().value());
+  EXPECT_EQ(3.45, j1p_s.get_optional().value());
+  EXPECT_EQ(0.0, j1v_s.get_optional().value());
+  EXPECT_EQ(2.78, j2p_s.get_optional().value());
+  EXPECT_EQ(0.0, j2v_s.get_optional().value());
   ASSERT_TRUE(std::isnan(j1p_c.get_optional().value()));
   ASSERT_TRUE(std::isnan(j1v_c.get_optional().value()));
   ASSERT_TRUE(std::isnan(j2p_c.get_optional().value()));
@@ -991,36 +1028,36 @@ void generic_system_error_group_test(
   ASSERT_TRUE(j2v_c.set_value(0.44));
 
   // State values should not be changed
-  ASSERT_EQ(3.45, j1p_s.get_optional().value());
-  ASSERT_EQ(0.0, j1v_s.get_optional().value());
-  ASSERT_EQ(2.78, j2p_s.get_optional().value());
-  ASSERT_EQ(0.0, j2v_s.get_optional().value());
-  ASSERT_EQ(0.11, j1p_c.get_optional().value());
-  ASSERT_EQ(0.22, j1v_c.get_optional().value());
-  ASSERT_EQ(0.33, j2p_c.get_optional().value());
-  ASSERT_EQ(0.44, j2v_c.get_optional().value());
+  EXPECT_EQ(3.45, j1p_s.get_optional().value());
+  EXPECT_EQ(0.0, j1v_s.get_optional().value());
+  EXPECT_EQ(2.78, j2p_s.get_optional().value());
+  EXPECT_EQ(0.0, j2v_s.get_optional().value());
+  EXPECT_EQ(0.11, j1p_c.get_optional().value());
+  EXPECT_EQ(0.22, j1v_c.get_optional().value());
+  EXPECT_EQ(0.33, j2p_c.get_optional().value());
+  EXPECT_EQ(0.44, j2v_c.get_optional().value());
 
   // write() does not change values
-  EXPECT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK);
-  ASSERT_EQ(3.45, j1p_s.get_optional().value());
-  ASSERT_EQ(0.0, j1v_s.get_optional().value());
-  ASSERT_EQ(2.78, j2p_s.get_optional().value());
-  ASSERT_EQ(0.0, j2v_s.get_optional().value());
-  ASSERT_EQ(0.11, j1p_c.get_optional().value());
-  ASSERT_EQ(0.22, j1v_c.get_optional().value());
-  ASSERT_EQ(0.33, j2p_c.get_optional().value());
-  ASSERT_EQ(0.44, j2v_c.get_optional().value());
+  ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK);
+  EXPECT_EQ(3.45, j1p_s.get_optional().value());
+  EXPECT_EQ(0.0, j1v_s.get_optional().value());
+  EXPECT_EQ(2.78, j2p_s.get_optional().value());
+  EXPECT_EQ(0.0, j2v_s.get_optional().value());
+  EXPECT_EQ(0.11, j1p_c.get_optional().value());
+  EXPECT_EQ(0.22, j1v_c.get_optional().value());
+  EXPECT_EQ(0.33, j2p_c.get_optional().value());
+  EXPECT_EQ(0.44, j2v_c.get_optional().value());
 
   // read() mirrors commands to states
-  EXPECT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK);
-  ASSERT_EQ(0.11, j1p_s.get_optional().value());
-  ASSERT_EQ(0.22, j1v_s.get_optional().value());
-  ASSERT_EQ(0.33, j2p_s.get_optional().value());
-  ASSERT_EQ(0.44, j2v_s.get_optional().value());
-  ASSERT_EQ(0.11, j1p_c.get_optional().value());
-  ASSERT_EQ(0.22, j1v_c.get_optional().value());
-  ASSERT_EQ(0.33, j2p_c.get_optional().value());
-  ASSERT_EQ(0.44, j2v_c.get_optional().value());
+  ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK);
+  EXPECT_EQ(0.11, j1p_s.get_optional().value());
+  EXPECT_EQ(0.22, j1v_s.get_optional().value());
+  EXPECT_EQ(0.33, j2p_s.get_optional().value());
+  EXPECT_EQ(0.44, j2v_s.get_optional().value());
+  EXPECT_EQ(0.11, j1p_c.get_optional().value());
+  EXPECT_EQ(0.22, j1v_c.get_optional().value());
+  EXPECT_EQ(0.33, j2p_c.get_optional().value());
+  EXPECT_EQ(0.44, j2v_c.get_optional().value());
 
   // set some new values in commands
   ASSERT_TRUE(j1p_c.set_value(0.55));
@@ -1029,14 +1066,14 @@ void generic_system_error_group_test(
   ASSERT_TRUE(j2v_c.set_value(0.88));
 
   // state values should not be changed
-  ASSERT_EQ(0.11, j1p_s.get_optional().value());
-  ASSERT_EQ(0.22, j1v_s.get_optional().value());
-  ASSERT_EQ(0.33, j2p_s.get_optional().value());
-  ASSERT_EQ(0.44, j2v_s.get_optional().value());
-  ASSERT_EQ(0.55, j1p_c.get_optional().value());
-  ASSERT_EQ(0.66, j1v_c.get_optional().value());
-  ASSERT_EQ(0.77, j2p_c.get_optional().value());
-  ASSERT_EQ(0.88, j2v_c.get_optional().value());
+  EXPECT_EQ(0.11, j1p_s.get_optional().value());
+  EXPECT_EQ(0.22, j1v_s.get_optional().value());
+  EXPECT_EQ(0.33, j2p_s.get_optional().value());
+  EXPECT_EQ(0.44, j2v_s.get_optional().value());
+  EXPECT_EQ(0.55, j1p_c.get_optional().value());
+  EXPECT_EQ(0.66, j1v_c.get_optional().value());
+  EXPECT_EQ(0.77, j2p_c.get_optional().value());
+  EXPECT_EQ(0.88, j2v_c.get_optional().value());
 
   // Error testing
   ASSERT_TRUE(j1p_c.set_value(std::numeric_limits<double>::infinity()));
@@ -1751,7 +1788,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio)
 }
 
 void TestGenericSystem::test_generic_system_with_mock_gpio_commands(
-  std::string & urdf, const std::string & component_name)
+  std::string & urdf, const std::string & component_name, const bool data_type_bool)
 {
   TestableResourceManager rm(node_, urdf);
 
@@ -1812,49 +1849,90 @@ void TestGenericSystem::test_generic_system_with_mock_gpio_commands(
   EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value()));
   EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_optional().value()));
   EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_optional().value()));
-  EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value()));
+  if (data_type_bool)
+  {
+    // for bool data type initial value is false
+    EXPECT_FALSE(gpio2_vac_s.get_optional<bool>().value());
+    EXPECT_FALSE(gpio2_vac_c.get_optional<bool>().value());
+  }
+  else
+  {
+    EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value()));
+    EXPECT_TRUE(std::isnan(gpio2_vac_c.get_optional().value()));
+  }
   EXPECT_TRUE(std::isnan(gpio1_a_o1_c.get_optional().value()));
   EXPECT_TRUE(std::isnan(gpio1_a_i1_c.get_optional().value()));
   EXPECT_TRUE(std::isnan(gpio1_a_i2_c.get_optional().value()));
-  EXPECT_TRUE(std::isnan(gpio2_vac_c.get_optional().value()));
 
   // set some new values in commands
   ASSERT_TRUE(gpio1_a_o1_c.set_value(0.11));
   ASSERT_TRUE(gpio1_a_i1_c.set_value(0.33));
   ASSERT_TRUE(gpio1_a_i2_c.set_value(1.11));
-  ASSERT_TRUE(gpio2_vac_c.set_value(2.22));
+  if (data_type_bool)
+  {
+    ASSERT_TRUE(gpio2_vac_c.set_value(true));
+  }
+  else
+  {
+    ASSERT_TRUE(gpio2_vac_c.set_value(2.22));
+  }
 
   // State values should not be changed
   EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value()));
   EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_optional().value()));
   EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_optional().value()));
-  EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value()));
+  if (data_type_bool)
+  {
+    EXPECT_FALSE(gpio2_vac_s.get_optional<bool>().value());
+    EXPECT_TRUE(gpio2_vac_c.get_optional<bool>().value());
+  }
+  else
+  {
+    EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value()));
+    ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value());
+  }
   ASSERT_EQ(0.11, gpio1_a_o1_c.get_optional().value());
   ASSERT_EQ(0.33, gpio1_a_i1_c.get_optional().value());
   ASSERT_EQ(1.11, gpio1_a_i2_c.get_optional().value());
-  ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value());
 
   // write() does not change values
   rm.write(TIME, PERIOD);
   EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value()));
   EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_optional().value()));
   EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_optional().value()));
-  EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value()));
+  if (data_type_bool)
+  {
+    // for bool data type initial value is false
+    EXPECT_FALSE(gpio2_vac_s.get_optional<bool>().value());
+    EXPECT_TRUE(gpio2_vac_c.get_optional<bool>().value());
+  }
+  else
+  {
+    EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value()));
+    ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value());
+  }
   ASSERT_EQ(0.11, gpio1_a_o1_c.get_optional().value());
   ASSERT_EQ(0.33, gpio1_a_i1_c.get_optional().value());
   ASSERT_EQ(1.11, gpio1_a_i2_c.get_optional().value());
-  ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value());
 
   // read() mirrors commands to states
   rm.read(TIME, PERIOD);
   ASSERT_EQ(0.11, gpio1_a_o1_s.get_optional().value());
   ASSERT_EQ(0.33, gpio1_a_i1_s.get_optional().value());
   ASSERT_EQ(1.11, gpio1_a_o2_s.get_optional().value());
-  ASSERT_EQ(2.22, gpio2_vac_s.get_optional().value());
+  if (data_type_bool)
+  {
+    EXPECT_TRUE(gpio2_vac_s.get_optional<bool>().value());
+    EXPECT_TRUE(gpio2_vac_c.get_optional<bool>().value());
+  }
+  else
+  {
+    ASSERT_EQ(2.22, gpio2_vac_s.get_optional().value());
+    ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value());
+  }
   ASSERT_EQ(0.11, gpio1_a_o1_c.get_optional().value());
   ASSERT_EQ(0.33, gpio1_a_i1_c.get_optional().value());
   ASSERT_EQ(1.11, gpio1_a_i2_c.get_optional().value());
-  ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value());
 }
 
 TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command)
@@ -1866,6 +1944,15 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_co
   test_generic_system_with_mock_gpio_commands(urdf, "MockHardwareSystem");
 }
 
+TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command)
+{
+  auto urdf = ros2_control_test_assets::urdf_head +
+              valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command_ +
+              ros2_control_test_assets::urdf_tail;
+
+  test_generic_system_with_mock_gpio_commands(urdf, "MockHardwareSystem", true);
+}
+
 TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True)
 {
   auto urdf = ros2_control_test_assets::urdf_head +
@@ -2198,6 +2285,8 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag
   ASSERT_TRUE(check_prepare_command_mode_switch(valid_urdf_ros2_control_system_robot_with_gpio_));
   ASSERT_TRUE(check_prepare_command_mode_switch(
     valid_urdf_ros2_control_system_robot_with_gpio_mock_command_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(
+    valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command_));
   ASSERT_TRUE(check_prepare_command_mode_switch(
     valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_));
   ASSERT_TRUE(check_prepare_command_mode_switch(sensor_with_initial_value_));