Cleanup GenericSystem component code 🧹 (#2706)

Author: christophfroehlich

hardware_interface/include/mock_components/generic_system.hpp

@@ -82,10 +82,6 @@ class GenericSystem : public hardware_interface::SystemInterface
     hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT};
   std::vector<std::string> skip_interfaces_;
 
-  std::vector<std::string> other_interfaces_;
-  std::vector<std::string> sensor_interfaces_;
-  std::vector<std::string> gpio_mock_interfaces_;
-
 private:
   bool populate_interfaces(
     const std::vector<hardware_interface::ComponentInfo> & components,

hardware_interface/src/mock_components/generic_system.cpp

@@ -39,27 +39,6 @@ CallbackReturn GenericSystem::on_init(
     return CallbackReturn::ERROR;
   }
 
-  auto populate_non_standard_interfaces =
-    [this](auto interface_list, auto & non_standard_interfaces)
-  {
-    for (const auto & interface : interface_list)
-    {
-      // add to list if non-standard interface
-      if (
-        std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name) ==
-        standard_interfaces_.end())
-      {
-        if (
-          std::find(
-            non_standard_interfaces.begin(), non_standard_interfaces.end(), interface.name) ==
-          non_standard_interfaces.end())
-        {
-          non_standard_interfaces.emplace_back(interface.name);
-        }
-      }
-    }
-  };
-
   // check if to create mock command interface for sensor
   auto it = get_hardware_info().hardware_parameters.find("mock_sensor_commands");
   if (it != get_hardware_info().hardware_parameters.end())
@@ -128,21 +107,44 @@ CallbackReturn GenericSystem::on_init(
   }
 
   // search for non-standard joint interfaces
+  std::vector<std::string> other_interfaces;
+  auto populate_non_standard_interfaces =
+    [this](auto interface_list, auto & non_standard_interfaces)
+  {
+    for (const auto & interface : interface_list)
+    {
+      // add to list if non-standard interface
+      if (
+        std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name) ==
+        standard_interfaces_.end())
+      {
+        // and does not exist yet
+        if (
+          std::find(
+            non_standard_interfaces.begin(), non_standard_interfaces.end(), interface.name) ==
+          non_standard_interfaces.end())
+        {
+          non_standard_interfaces.emplace_back(interface.name);
+        }
+      }
+    }
+  };
   for (const auto & joint : get_hardware_info().joints)
   {
-    // populate non-standard command interfaces to other_interfaces_
-    populate_non_standard_interfaces(joint.command_interfaces, other_interfaces_);
+    // populate non-standard command interfaces to other_interfaces
+    populate_non_standard_interfaces(joint.command_interfaces, other_interfaces);
 
-    // populate non-standard state interfaces to other_interfaces_
-    populate_non_standard_interfaces(joint.state_interfaces, other_interfaces_);
+    // populate non-standard state interfaces to other_interfaces
+    populate_non_standard_interfaces(joint.state_interfaces, other_interfaces);
   }
 
   // when following offset is used on custom interface then find its index
   if (!custom_interface_with_following_offset_.empty())
   {
-    auto if_it = std::find(
-      other_interfaces_.begin(), other_interfaces_.end(), custom_interface_with_following_offset_);
-    if (if_it != other_interfaces_.end())
+    if (
+      std::find(
+        other_interfaces.begin(), other_interfaces.end(),
+        custom_interface_with_following_offset_) != other_interfaces.end())
     {
       RCLCPP_INFO(
         get_logger(), "Custom interface with following offset '%s' found.",
@@ -157,19 +159,6 @@ CallbackReturn GenericSystem::on_init(
     }
   }
 
-  for (const auto & sensor : get_hardware_info().sensors)
-  {
-    for (const auto & interface : sensor.state_interfaces)
-    {
-      if (
-        std::find(sensor_interfaces_.begin(), sensor_interfaces_.end(), interface.name) ==
-        sensor_interfaces_.end())
-      {
-        sensor_interfaces_.emplace_back(interface.name);
-      }
-    }
-  }
-
   return CallbackReturn::SUCCESS;
 }
 
@@ -231,26 +220,10 @@ return_type GenericSystem::prepare_command_mode_switch(
       }
       if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY)
       {
-        if (!calculate_dynamics_)
-        {
-          RCLCPP_WARN(
-            get_logger(),
-            "Requested velocity mode for joint '%s' without dynamics calculation enabled - this "
-            "might lead to wrong feedback and unexpected behavior.",
-            info.joints[joint_index].name.c_str());
-        }
         joint_found_in_x_requests_[joint_index] += 1;
       }
       if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION)
       {
-        if (!calculate_dynamics_)
-        {
-          RCLCPP_WARN(
-            get_logger(),
-            "Requested acceleration mode for joint '%s' without dynamics calculation enabled - "
-            "this might lead to wrong feedback and unexpected behavior.",
-            info.joints[joint_index].name.c_str());
-        }
         joint_found_in_x_requests_[joint_index] += 1;
       }
     }
@@ -412,14 +385,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
       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())
+        if (has_command(full_name))
         {
-          joint_command_values_[i] =
-            get_command(joint_command_interfaces_.at(full_name).get_name());
+          joint_command_values_[i] = get_command(full_name);
         }
-        if (joint_state_interfaces_.find(full_name) != joint_state_interfaces_.end())
+        if (has_state(full_name))
         {
-          joint_state_values_[i] = get_state(joint_state_interfaces_.at(full_name).get_name());
+          joint_state_values_[i] = get_state(full_name);
         }
       }
 
@@ -513,7 +485,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
         if (joint_command.get()->get_interface_name() == hardware_interface::HW_IF_POSITION)
         {
           const std::string & name = joint_command.get()->get_name();
-          if (joint_state_interfaces_.find(name) != joint_state_interfaces_.end())
+          if (has_state(name))
           {
             set_state(
               name, get_command(name) + (custom_interface_with_following_offset_.empty()
@@ -536,7 +508,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
       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 (has_command(full_interface_name))
     {
       if (
         mirror_command_to_state(full_interface_name, joint_state.get()->get_data_type()) !=
@@ -561,29 +533,22 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
   {
     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 + "/" + hardware_interface::HW_IF_POSITION) !=
-      joint_state_interfaces_.end())
+    if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION))
     {
       set_state(
         mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION,
         mimic_joint.offset +
           mimic_joint.multiplier *
             get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION));
     }
-    if (
-      joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY) !=
-      joint_state_interfaces_.end())
+    if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY))
     {
       set_state(
         mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY,
         mimic_joint.multiplier *
           get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY));
     }
-    if (
-      joint_state_interfaces_.find(
-        mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) !=
-      joint_state_interfaces_.end())
+    if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION))
     {
       set_state(
         mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION,
@@ -624,7 +589,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
     for (const auto & gpio_command : gpio_commands_)
     {
       const std::string & name = gpio_command.get()->get_name();
-      if (gpio_state_interfaces_.find(name) != gpio_state_interfaces_.end())
+      if (has_state(name))
       {
         if (mirror_command_to_state(name, gpio_command.get()->get_data_type()) != return_type::OK)
         {

hardware_interface/test/mock_components/test_generic_system.cpp

@@ -808,6 +808,8 @@ class TestGenericSystem : public ::testing::Test
       hardware_interface::DEFAULT_REGISTRY_KEY);
   }
 
+  void TearDown() override { node_.reset(); }
+
   std::string hardware_system_2dof_;
   std::string hardware_system_2dof_asymetric_;
   std::string hardware_system_2dof_standard_interfaces_;
@@ -847,19 +849,6 @@ class ResourceStorage;
 class TestableResourceManager : public hardware_interface::ResourceManager
 {
 public:
-  friend TestGenericSystem;
-
-  FRIEND_TEST(TestGenericSystem, generic_system_2dof_symetric_interfaces);
-  FRIEND_TEST(TestGenericSystem, generic_system_2dof_asymetric_interfaces);
-  FRIEND_TEST(TestGenericSystem, generic_system_2dof_other_interfaces);
-  FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor);
-  FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command);
-  FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command_True);
-  FRIEND_TEST(TestGenericSystem, hardware_system_2dof_with_mimic_joint);
-  FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio);
-  FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command);
-  FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True);
-
   explicit TestableResourceManager(rclcpp::Node::SharedPtr node)
   : hardware_interface::ResourceManager(
       node->get_node_clock_interface(), node->get_node_logging_interface())