make the clock and logger interfaces mandatory arguments and fix tests

controller_manager/include/controller_manager/controller_manager.hpp

@@ -82,6 +82,13 @@ class ControllerManager : public rclcpp::Node
     const std::string & node_namespace = "",
     const rclcpp::NodeOptions & options = get_cm_node_options());
 
+  CONTROLLER_MANAGER_PUBLIC
+  ControllerManager(
+    std::shared_ptr<rclcpp::Executor> executor, const std::string & urdf,
+    bool activate_all_hw_components, const std::string & manager_node_name = "controller_manager",
+    const std::string & node_namespace = "",
+    const rclcpp::NodeOptions & options = get_cm_node_options());
+
   CONTROLLER_MANAGER_PUBLIC
   virtual ~ControllerManager() = default;
 

controller_manager/src/controller_manager.cpp

@@ -233,6 +233,39 @@ ControllerManager::ControllerManager(
     "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
 }
 
+ControllerManager::ControllerManager(
+  std::shared_ptr<rclcpp::Executor> executor, const std::string & urdf,
+  bool activate_all_hw_components, const std::string & manager_node_name,
+  const std::string & node_namespace, const rclcpp::NodeOptions & options)
+: rclcpp::Node(manager_node_name, node_namespace, options),
+  update_rate_(get_parameter_or<int>("update_rate", 100)),
+  resource_manager_(std::make_unique<hardware_interface::ResourceManager>(
+    urdf, this->get_node_clock_interface(), this->get_node_logging_interface(),
+    activate_all_hw_components, update_rate_)),
+  diagnostics_updater_(this),
+  executor_(executor),
+  loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
+    kControllerInterfaceNamespace, kControllerInterfaceClassName)),
+  chainable_loader_(
+    std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
+      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
+{
+  if (!get_parameter("update_rate", update_rate_))
+  {
+    RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
+  }
+
+  if (is_resource_manager_initialized())
+  {
+    init_services();
+  }
+  subscribe_to_robot_description_topic();
+
+  diagnostics_updater_.setHardwareID("ros2_control");
+  diagnostics_updater_.add(
+    "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
+}
+
 ControllerManager::ControllerManager(
   std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,

controller_manager/test/controller_manager_test_common.hpp

@@ -74,7 +74,9 @@ class ControllerManagerFixture : public ::testing::Test
     if (robot_description_.empty())
     {
       cm_ = std::make_shared<CtrlMgr>(
-        std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
+        std::make_unique<hardware_interface::ResourceManager>(
+          rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
+        executor_, TEST_CM_NAME);
     }
     else
     {
@@ -83,7 +85,9 @@ class ControllerManagerFixture : public ::testing::Test
       if (pass_urdf_as_parameter_)
       {
         cm_ = std::make_shared<CtrlMgr>(
-          std::make_unique<hardware_interface::ResourceManager>(robot_description_, true, true),
+          std::make_unique<hardware_interface::ResourceManager>(
+            robot_description_, rm_node_->get_node_clock_interface(),
+            rm_node_->get_node_logging_interface(), true, 100),
           executor_, TEST_CM_NAME);
       }
       else
@@ -93,7 +97,9 @@ class ControllerManagerFixture : public ::testing::Test
 
         // this is just a workaround to skip passing
         cm_ = std::make_shared<CtrlMgr>(
-          std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
+          std::make_unique<hardware_interface::ResourceManager>(
+            rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
+          executor_, TEST_CM_NAME);
         // mimic topic call
         auto msg = std_msgs::msg::String();
         msg.data = robot_description_;
@@ -158,6 +164,9 @@ class ControllerManagerFixture : public ::testing::Test
   const std::string robot_description_;
   const bool pass_urdf_as_parameter_;
   rclcpp::Time time_;
+
+protected:
+  rclcpp::Node::SharedPtr rm_node_ = std::make_shared<rclcpp::Node>("ResourceManager");
 };
 
 class TestControllerManagerSrvs

controller_manager/test/test_controller_manager_with_namespace.cpp

@@ -39,7 +39,8 @@ class TestControllerManagerWithNamespace
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
     cm_ = std::make_shared<controller_manager::ControllerManager>(
       std::make_unique<hardware_interface::ResourceManager>(
-        ros2_control_test_assets::minimal_robot_urdf, true, true),
+        ros2_control_test_assets::minimal_robot_urdf, rm_node_->get_node_clock_interface(),
+        rm_node_->get_node_logging_interface(), true),
       executor_, TEST_CM_NAME, TEST_NAMESPACE);
     run_updater_ = false;
   }

controller_manager/test/test_controllers_chaining_with_controller_manager.cpp

@@ -111,7 +111,8 @@ class TestControllerChainingWithControllerManager
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
     cm_ = std::make_shared<TestableControllerManager>(
       std::make_unique<hardware_interface::ResourceManager>(
-        ros2_control_test_assets::diffbot_urdf, true, true),
+        ros2_control_test_assets::diffbot_urdf, rm_node_->get_node_clock_interface(),
+        rm_node_->get_node_logging_interface(), true),
       executor_, TEST_CM_NAME);
     run_updater_ = false;
   }

controller_manager/test/test_hardware_management_srvs.cpp

@@ -63,8 +63,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
   void SetUp() override
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-    cm_ = std::make_shared<controller_manager::ControllerManager>(
-      std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
+    cm_ = std::make_shared<controller_manager::ControllerManager>(executor_, TEST_CM_NAME);
     run_updater_ = false;
 
     cm_->set_parameter(
@@ -369,8 +368,7 @@ class TestControllerManagerHWManagementSrvsWithoutParams
   void SetUp() override
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-    cm_ = std::make_shared<controller_manager::ControllerManager>(
-      std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
+    cm_ = std::make_shared<controller_manager::ControllerManager>(executor_, TEST_CM_NAME);
     run_updater_ = false;
 
     // TODO(destogl): separate this to init_tests method where parameter can be set for each test

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -50,8 +50,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
 public:
   /// Default constructor for the Resource Manager.
   explicit ResourceManager(
-    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr,
-    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr);
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface);
 
   /// Constructor for the Resource Manager.
   /**
@@ -68,9 +68,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * used for triggering async components.
    */
   explicit ResourceManager(
-    const std::string & urdf, bool activate_all = false, const unsigned int update_rate = 100,
-    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr,
-    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr);
+    const std::string & urdf,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
+    bool activate_all = false, const unsigned int update_rate = 100);
 
   ResourceManager(const ResourceManager &) = delete;
 

hardware_interface/src/resource_manager.cpp

@@ -99,13 +99,18 @@ class ResourceStorage
   // TODO(VX792): Change this when HW ifs get their own update rate,
   // because the ResourceStorage really shouldn't know about the cm's parameters
   explicit ResourceStorage(
-    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr,
-    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr)
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface)
   : actuator_loader_(pkg_name, actuator_interface_name),
     sensor_loader_(pkg_name, sensor_interface_name),
     system_loader_(pkg_name, system_interface_name),
     clock_interface_(clock_interface)
   {
+    if (!clock_interface_)
+    {
+      throw std::invalid_argument(
+        "Clock interface is nullptr. ResourceManager needs a valid clock interface.");
+    }
     if (logger_interface)
     {
       rm_logger_ = logger_interface->get_logger().get_child("resource_manager");
@@ -994,9 +999,9 @@ ResourceManager::ResourceManager(
 ResourceManager::~ResourceManager() = default;
 
 ResourceManager::ResourceManager(
-  const std::string & urdf, bool activate_all, const unsigned int update_rate,
-  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
-  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface)
+  const std::string & urdf, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, bool activate_all,
+  const unsigned int update_rate)
 : resource_storage_(std::make_unique<ResourceStorage>(clock_interface, logger_interface))
 {
   load_and_initialize_components(urdf, update_rate);

hardware_interface/test/mock_components/test_generic_system.cpp

@@ -26,6 +26,7 @@
 #include "hardware_interface/resource_manager.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "rclcpp/node.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "ros2_control_test_assets/components_urdfs.hpp"
 #include "ros2_control_test_assets/descriptions.hpp"
@@ -670,6 +671,7 @@ class TestGenericSystem : public ::testing::Test
   std::string disabled_commands_;
   std::string hardware_system_2dof_standard_interfaces_with_same_hardware_group_;
   std::string hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_;
+  rclcpp::Node node_ = rclcpp::Node("TestGenericSystem");
 };
 
 // Forward declaration
@@ -694,10 +696,16 @@ class TestableResourceManager : public hardware_interface::ResourceManager
   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);
 
-  TestableResourceManager() : hardware_interface::ResourceManager() {}
+  explicit TestableResourceManager(rclcpp::Node & node)
+  : hardware_interface::ResourceManager(
+      node.get_node_clock_interface(), node.get_node_logging_interface())
+  {
+  }
 
-  explicit TestableResourceManager(const std::string & urdf, bool activate_all = false)
-  : hardware_interface::ResourceManager(urdf, activate_all)
+  explicit TestableResourceManager(
+    rclcpp::Node & node, const std::string & urdf, bool activate_all = false)
+  : hardware_interface::ResourceManager(
+      urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, 100)
   {
   }
 };
@@ -744,15 +752,15 @@ TEST_F(TestGenericSystem, load_generic_system_2dof)
 {
   auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ +
               ros2_control_test_assets::urdf_tail;
-  ASSERT_NO_THROW(TestableResourceManager rm(urdf));
+  ASSERT_NO_THROW(TestableResourceManager rm(node_, urdf));
 }
 
 // Test inspired by hardware_interface/test_resource_manager.cpp
 TEST_F(TestGenericSystem, generic_system_2dof_symetric_interfaces)
 {
   auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ +
               ros2_control_test_assets::urdf_tail;
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
   // Activate components to get all interfaces available
   activate_components(rm, {"MockHardwareSystem"});
 
@@ -783,7 +791,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces)
 {
   auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_asymetric_ +
               ros2_control_test_assets::urdf_tail;
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
   // Activate components to get all interfaces available
   activate_components(rm, {"MockHardwareSystem"});
 
@@ -831,7 +839,8 @@ void generic_system_functional_test(
   const std::string & urdf, const std::string component_name = "GenericSystem2dof",
   const double offset = 0)
 {
-  TestableResourceManager rm(urdf);
+  rclcpp::Node node("test_generic_system");
+  TestableResourceManager rm(node, urdf);
   // check is hardware is configured
   auto status_map = rm.get_components_status();
   EXPECT_EQ(
@@ -929,7 +938,8 @@ void generic_system_functional_test(
 void generic_system_error_group_test(
   const std::string & urdf, const std::string component_prefix, bool validate_same_group)
 {
-  TestableResourceManager rm(urdf);
+  rclcpp::Node node("test_generic_system");
+  TestableResourceManager rm(node, urdf);
   const std::string component1 = component_prefix + "1";
   const std::string component2 = component_prefix + "2";
   // check is hardware is configured
@@ -1108,7 +1118,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces)
 {
   auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_other_interface_ +
               ros2_control_test_assets::urdf_tail;
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
   // Activate components to get all interfaces available
   activate_components(rm, {"MockHardwareSystem"});
 
@@ -1191,7 +1201,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor)
 {
   auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_ +
               ros2_control_test_assets::urdf_tail;
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
   // Activate components to get all interfaces available
   activate_components(rm, {"MockHardwareSystem"});
 
@@ -1290,7 +1300,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor)
 void TestGenericSystem::test_generic_system_with_mock_sensor_commands(
   std::string & urdf, const std::string & component_name)
 {
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
   // Activate components to get all interfaces available
   activate_components(rm, {component_name});
 
@@ -1430,7 +1440,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True)
 void TestGenericSystem::test_generic_system_with_mimic_joint(
   std::string & urdf, const std::string & component_name)
 {
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
   // Activate components to get all interfaces available
   activate_components(rm, {component_name});
 
@@ -1527,7 +1537,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i
 
   const double offset = -3;
 
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
 
   const std::string hardware_name = "MockHardwareSystem";
 
@@ -1640,7 +1650,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio)
 {
   auto urdf = ros2_control_test_assets::urdf_head +
               valid_urdf_ros2_control_system_robot_with_gpio_ + ros2_control_test_assets::urdf_tail;
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
 
   const std::string hardware_name = "MockHardwareSystem";
 
@@ -1737,7 +1747,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)
 {
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
 
   // check is hardware is started
   auto status_map = rm.get_components_status();
@@ -1863,7 +1873,7 @@ TEST_F(TestGenericSystem, sensor_with_initial_value)
 {
   auto urdf = ros2_control_test_assets::urdf_head + sensor_with_initial_value_ +
               ros2_control_test_assets::urdf_tail;
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
   // Activate components to get all interfaces available
   activate_components(rm, {"MockHardwareSystem"});
 
@@ -1891,7 +1901,7 @@ TEST_F(TestGenericSystem, gpio_with_initial_value)
 {
   auto urdf = ros2_control_test_assets::urdf_head + gpio_with_initial_value_ +
               ros2_control_test_assets::urdf_tail;
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
   // Activate components to get all interfaces available
   activate_components(rm, {"MockHardwareSystem"});
 
@@ -1912,7 +1922,7 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces)
               hardware_system_2dof_standard_interfaces_with_different_control_modes_ +
               ros2_control_test_assets::urdf_tail;
 
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
   // Activate components to get all interfaces available
   activate_components(rm, {"MockHardwareSystem"});
 
@@ -2106,7 +2116,7 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active)
 {
   auto urdf =
     ros2_control_test_assets::urdf_head + disabled_commands_ + ros2_control_test_assets::urdf_tail;
-  TestableResourceManager rm(urdf);
+  TestableResourceManager rm(node_, urdf);
   // Activate components to get all interfaces available
   activate_components(rm, {"MockHardwareSystem"});
 
@@ -2155,7 +2165,7 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag
     [&](
       const std::string & urdf, const std::string & urdf_head = ros2_control_test_assets::urdf_head)
   {
-    TestableResourceManager rm(urdf_head + urdf + ros2_control_test_assets::urdf_tail);
+    TestableResourceManager rm(node_, urdf_head + urdf + ros2_control_test_assets::urdf_tail);
     rclcpp_lifecycle::State state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active");
     rm.set_component_state("MockHardwareSystem", state);
     auto start_interfaces = rm.command_interface_keys();
@@ -2192,3 +2202,10 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag
     valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_));
   ASSERT_TRUE(check_prepare_command_mode_switch(disabled_commands_));
 }
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}