Prepare GenericSystem for other interface data types

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
 ########################

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_;