create publishers in cm and only add in rm

Author: mamueluth

controller_manager/include/controller_manager/controller_manager.hpp

@@ -236,10 +236,10 @@ class ControllerManager : public rclcpp::Node
     std::shared_ptr<controller_manager_msgs::srv::RegisterSubControllerManager::Response> response);
 
   CONTROLLER_MANAGER_PUBLIC
-  void add_hardware_state_publishers();
+  void create_hardware_state_publishers();
 
   CONTROLLER_MANAGER_PUBLIC
-  void add_hardware_command_forwarders();
+  void create_hardware_command_forwarders();
 
   CONTROLLER_MANAGER_PUBLIC
   void register_sub_controller_manager();

controller_manager/src/controller_manager.cpp

@@ -453,8 +453,8 @@ void ControllerManager::init_distributed_sub_controller_manager()
     distributed_pub_sub_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
       std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false);
   }
-  add_hardware_state_publishers();
-  add_hardware_command_forwarders();
+  create_hardware_state_publishers();
+  create_hardware_command_forwarders();
   register_sub_controller_manager();
 }
 
@@ -557,15 +557,19 @@ void ControllerManager::register_sub_controller_manager_srv_cb(
                     << sub_ctrl_mng_wrapper->get_name() << ">.");
 }
 
-void ControllerManager::add_hardware_state_publishers()
+void ControllerManager::create_hardware_state_publishers()
 {
-  std::vector<std::shared_ptr<distributed_control::StatePublisher>> state_publishers_vec;
-  state_publishers_vec.reserve(resource_manager_->available_state_interfaces().size());
-  state_publishers_vec = resource_manager_->create_hardware_state_publishers(
-    get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_);
+  auto available_state_interfaces = resource_manager_->available_state_interfaces();
 
-  for (auto const & state_publisher : state_publishers_vec)
+  for (const auto & state_interface : available_state_interfaces)
   {
+    auto state_publisher = std::make_shared<distributed_control::StatePublisher>(
+      std::move(std::make_unique<hardware_interface::LoanedStateInterface>(
+        resource_manager_->claim_state_interface(state_interface))),
+      get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_);
+
+    resource_manager_->add_hardware_state_publishers(state_publisher);
+
     try
     {
       executor_->add_node(state_publisher->get_node()->get_node_base_interface());
@@ -579,15 +583,19 @@ void ControllerManager::add_hardware_state_publishers()
   }
 }
 
-void ControllerManager::add_hardware_command_forwarders()
+void ControllerManager::create_hardware_command_forwarders()
 {
-  std::vector<std::shared_ptr<distributed_control::CommandForwarder>> command_forwarder_vec;
-  command_forwarder_vec.reserve(resource_manager_->available_command_interfaces().size());
-  command_forwarder_vec = resource_manager_->create_hardware_command_forwarders(
-    get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_);
+  auto available_command_interfaces = resource_manager_->available_command_interfaces();
 
-  for (auto const & command_forwarder : command_forwarder_vec)
+  for (auto const & command_interface : available_command_interfaces)
   {
+    auto command_forwarder = std::make_shared<distributed_control::CommandForwarder>(
+      std::move(std::make_unique<hardware_interface::LoanedCommandInterface>(
+        resource_manager_->claim_command_interface(command_interface))),
+      get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_);
+
+    resource_manager_->add_hardware_command_forwarders(command_forwarder);
+
     try
     {
       executor_->add_node(command_forwarder->get_node()->get_node_base_interface());

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -142,15 +142,11 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
     std::shared_ptr<distributed_control::SubControllerManagerWrapper> sub_controller_manager,
     const std::string & ns, std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node);
 
-  std::vector<std::shared_ptr<distributed_control::StatePublisher>>
-  create_hardware_state_publishers(
-    const std::string & ns, std::chrono::milliseconds update_period,
-    std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node);
-
-  std::vector<std::shared_ptr<distributed_control::CommandForwarder>>
-  create_hardware_command_forwarders(
-    const std::string & ns, std::chrono::milliseconds update_period,
-    std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node);
+  void add_hardware_state_publishers(
+    std::shared_ptr<distributed_control::StatePublisher> state_publisher);
+
+  void add_hardware_command_forwarders(
+    std::shared_ptr<distributed_control::CommandForwarder> command_forwarder);
 
   std::pair<bool, std::shared_ptr<distributed_control::CommandForwarder>> find_command_forwarder(
     const std::string & key);

hardware_interface/src/resource_manager.cpp

@@ -957,50 +957,18 @@ ResourceManager::import_command_interfaces_of_sub_controller_manager(
   return resource_storage_->import_distributed_command_interfaces(sub_controller_manager, ns, node);
 }
 
-std::vector<std::shared_ptr<distributed_control::StatePublisher>>
-ResourceManager::create_hardware_state_publishers(
-  const std::string & ns, std::chrono::milliseconds update_period,
-  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node)
+void ResourceManager::add_hardware_state_publishers(
+  std::shared_ptr<distributed_control::StatePublisher> state_publisher)
 {
   std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
-  std::vector<std::shared_ptr<distributed_control::StatePublisher>> state_publishers_vec;
-  state_publishers_vec.reserve(available_state_interfaces().size());
-
-  for (const auto & state_interface : available_state_interfaces())
-  {
-    auto state_publisher = std::make_shared<distributed_control::StatePublisher>(
-      std::move(std::make_unique<hardware_interface::LoanedStateInterface>(
-        claim_state_interface(state_interface))),
-      ns, update_period, node);
-
-    resource_storage_->add_state_publisher(state_publisher);
-    state_publishers_vec.push_back(state_publisher);
-  }
-
-  return state_publishers_vec;
+  resource_storage_->add_state_publisher(state_publisher);
 }
 
-std::vector<std::shared_ptr<distributed_control::CommandForwarder>>
-ResourceManager::create_hardware_command_forwarders(
-  const std::string & ns, std::chrono::milliseconds update_period,
-  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node)
+void ResourceManager::add_hardware_command_forwarders(
+  std::shared_ptr<distributed_control::CommandForwarder> command_forwarder)
 {
   std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
-  std::vector<std::shared_ptr<distributed_control::CommandForwarder>> command_forwarders_vec;
-  command_forwarders_vec.reserve(available_command_interfaces().size());
-
-  for (const auto & command_interface : available_command_interfaces())
-  {
-    auto command_forwarder = std::make_shared<distributed_control::CommandForwarder>(
-      std::move(std::make_unique<hardware_interface::LoanedCommandInterface>(
-        claim_command_interface(command_interface))),
-      ns, update_period, node);
-
-    resource_storage_->add_command_forwarder(command_forwarder);
-    command_forwarders_vec.push_back(command_forwarder);
-  }
-
-  return command_forwarders_vec;
+  resource_storage_->add_command_forwarder(command_forwarder);
 }
 
 std::vector<std::shared_ptr<distributed_control::StatePublisher>>