!Not working! Add registration of DistributedCommandInterfaces

* Its not working, since the storage type of command interfaces needs
to be changed to std::shared_ptr. This is done in follow_up since should
change a lot.

Author: mamueluth

controller_manager/include/controller_manager/controller_manager.hpp

@@ -42,7 +42,6 @@
 #include "controller_manager_msgs/srv/unload_controller.hpp"
 
 #include "diagnostic_updater/diagnostic_updater.hpp"
-#include "hardware_interface/distributed_control_interface/state_publisher.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/resource_manager.hpp"
 
@@ -206,6 +205,9 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   void add_hardware_state_publishers();
 
+  CONTROLLER_MANAGER_PUBLIC
+  void add_hardware_command_forwarders();
+
   CONTROLLER_MANAGER_PUBLIC
   void register_sub_controller_manager();
 

controller_manager/src/controller_manager.cpp

@@ -24,6 +24,8 @@
 #include "controller_interface/controller_interface_base.hpp"
 #include "controller_manager_msgs/msg/hardware_component_state.hpp"
 
+#include "hardware_interface/distributed_control_interface/command_forwarder.hpp"
+#include "hardware_interface/distributed_control_interface/state_publisher.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 
@@ -311,6 +313,7 @@ void ControllerManager::configure_controller_manager()
   if (distributed_sub_controller_manager)
   {
     add_hardware_state_publishers();
+    add_hardware_command_forwarders();
     register_sub_controller_manager();
   }
   // This means we are the central controller manager
@@ -353,13 +356,16 @@ void ControllerManager::register_sub_controller_manager_srv_cb(
 
   auto sub_ctrl_mng_wrapper = std::make_shared<distributed_control::SubControllerManagerWrapper>(
     request->sub_controller_manager_namespace, request->sub_controller_manager_name,
-    request->state_publishers);
+    request->state_publishers, request->command_state_publishers);
 
-  std::vector<std::shared_ptr<hardware_interface::DistributedReadOnlyHandle>> state_interfaces;
-  state_interfaces.reserve(sub_ctrl_mng_wrapper->get_state_publisher_count());
-  state_interfaces = resource_manager_->register_sub_controller_manager(sub_ctrl_mng_wrapper);
+  resource_manager_->register_sub_controller_manager(sub_ctrl_mng_wrapper);
 
-  for (const auto & state_interface : state_interfaces)
+  std::vector<std::shared_ptr<hardware_interface::DistributedReadOnlyHandle>>
+    distributed_state_interfaces;
+  distributed_state_interfaces =
+    resource_manager_->import_state_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper);
+
+  for (const auto & state_interface : distributed_state_interfaces)
   {
     try
     {
@@ -376,6 +382,36 @@ void ControllerManager::register_sub_controller_manager_srv_cb(
     }
   }
 
+  std::vector<std::shared_ptr<hardware_interface::DistributedReadWriteHandle>>
+    distributed_command_interfaces;
+  distributed_command_interfaces =
+    resource_manager_->import_command_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper);
+
+  for (const auto & command_interface : distributed_command_interfaces)
+  {
+    try
+    {
+      executor_->add_node(command_interface->get_node()->get_node_base_interface());
+      auto msg = controller_manager_msgs::msg::PublisherDescription();
+      msg.ns = get_namespace();
+      msg.name.prefix_name = command_interface->get_prefix_name();
+      msg.name.interface_name = command_interface->get_interface_name();
+      // want topic name relative to namespace
+      msg.publisher_topic = std::string(get_namespace()) + std::string("/") +
+                            command_interface->forward_command_topic_name();
+      response->command_state_publishers.push_back(msg);
+    }
+    catch (const std::runtime_error & e)
+    {
+      response->ok = false;
+      RCLCPP_WARN_STREAM(
+        get_logger(),
+        "ControllerManager: Caught exception while trying to register sub controller manager. "
+        "Exception:"
+          << e.what());
+    }
+  }
+
   response->ok = true;
   RCLCPP_INFO_STREAM(
     get_logger(), "ControllerManager: Registered sub controller manager <"
@@ -403,6 +439,27 @@ void ControllerManager::add_hardware_state_publishers()
   }
 }
 
+void ControllerManager::add_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());
+
+  for (auto const & command_forwarder : command_forwarder_vec)
+  {
+    try
+    {
+      executor_->add_node(command_forwarder->get_node()->get_node_base_interface());
+    }
+    catch (const std::runtime_error & e)
+    {
+      RCLCPP_WARN_STREAM(
+        get_logger(), "ControllerManager: Can't create StatePublishers<"
+                        << command_forwarder->command_interface_name() << ">." << e.what());
+    }
+  }
+}
+
 void ControllerManager::register_sub_controller_manager()
 {
   RCLCPP_INFO(get_logger(), "SubControllerManager:Trying to register StatePublishers.");
@@ -421,7 +478,17 @@ void ControllerManager::register_sub_controller_manager()
     // create description of StatePublisher including: prefix_name, interface_name and topic.
     // So that receiver is able to create a DistributedStateInterface which subscribes to the
     // topics provided by this sub controller manager
-    request->state_publishers.push_back(state_publisher->create_description_msg());
+    request->state_publishers.push_back(state_publisher->create_publisher_description_msg());
+  }
+
+  // export the provided CommandForwarders
+  for (auto const & command_forwarders : resource_manager_->get_command_forwarders())
+  {
+    // create description of StatePublisher including: prefix_name, interface_name and topic.
+    // So that receiver is able to create a DistributedStateInterface which subscribes to the
+    // topics provided by this sub controller manager
+    request->command_state_publishers.push_back(
+      command_forwarders->create_publisher_description_msg());
   }
 
   using namespace std::chrono_literals;
@@ -447,6 +514,15 @@ void ControllerManager::register_sub_controller_manager()
      rclcpp::FutureReturnCode::SUCCESS) &&
     result.get()->ok)
   {
+    // TODO(Manuel) we should probably make the keys explicit (add key_generation function to handles)
+    // send keys with request
+    for (auto command_state_publisher : result.get()->command_state_publishers)
+    {
+      std::string key = command_state_publisher.name.prefix_name + "/" +
+                        command_state_publisher.name.interface_name;
+      // auto command_forwarder = resource_manager_->find_command_forwarder(key);
+      // command_forwarder->subscribe_to_command(command_state_publisher);
+    }
     RCLCPP_INFO(get_logger(), "SubControllerManager: Successfully registered StatePublishers.");
   }
   else

hardware_interface/CMakeLists.txt

@@ -29,6 +29,7 @@ add_library(
   SHARED
   src/actuator.cpp
   src/component_parser.cpp
+  src/hardware_interface/distributed_control_interface/command_forwarder.cpp
   src/hardware_interface/distributed_control_interface/state_publisher.cpp
   src/resource_manager.cpp
   src/sensor.cpp

hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp

@@ -0,0 +1,66 @@
+#ifndef DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_
+#define DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "hardware_interface/distributed_control_interface/publisher_description.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+
+#include "controller_manager_msgs/msg/publisher_description.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "std_msgs/msg/float64.hpp"
+
+namespace distributed_control
+{
+
+class CommandForwarder final
+{
+public:
+  explicit CommandForwarder(
+    std::unique_ptr<hardware_interface::LoanedCommandInterface> loaned_command_interface_ptr_,
+    const std::string & ns = "");
+
+  CommandForwarder() = delete;
+
+  ~CommandForwarder() {}
+
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node() const;
+
+  std::string get_namespace() const;
+
+  std::string topic_name() const;
+
+  std::string topic_name_relative_to_namespace() const;
+
+  std::string command_interface_name() const;
+
+  std::string command_interface_prefix_name() const;
+
+  std::string command_interface_interface_name() const;
+
+  controller_manager_msgs::msg::PublisherDescription create_publisher_description_msg() const;
+
+  void subscribe_to_command(const distributed_control::PublisherDescription & description);
+
+private:
+  void publish_value_on_timer();
+  void forward_command(const std_msgs::msg::Float64 & msg);
+
+  std::unique_ptr<hardware_interface::LoanedCommandInterface> loaned_command_interface_ptr_;
+  const std::string namespace_;
+  const std::string topic_name_;
+  std::string subscription_topic_name_;
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr state_value_pub_;
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr command_subscription_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+}  // namespace distributed_control
+
+#endif  // DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_

hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp

@@ -5,8 +5,6 @@
 #include <string>
 #include <vector>
 
-#include "visibility_control.h"
-
 #include "hardware_interface/loaned_state_interface.hpp"
 
 #include "controller_manager_msgs/msg/publisher_description.hpp"
@@ -23,40 +21,34 @@ class StatePublisher final
 {
 public:
   explicit StatePublisher(
-    const std::string & ns = "",
-    std::unique_ptr<hardware_interface::LoanedStateInterface> loaned_state_interface_ptr = nullptr);
+    std::unique_ptr<hardware_interface::LoanedStateInterface> loaned_state_interface_ptr,
+    const std::string & ns = "");
+
+  StatePublisher() = delete;
 
   ~StatePublisher() {}
 
-  STATE_PUBLISHER_PUBLIC
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node() const;
 
-  STATE_PUBLISHER_PUBLIC
   std::string get_namespace() const;
 
-  STATE_PUBLISHER_PUBLIC
   std::string topic_name() const;
 
-  STATE_PUBLISHER_PUBLIC
   std::string topic_name_relative_to_namespace() const;
 
-  STATE_PUBLISHER_PUBLIC
   std::string state_interface_name() const;
 
-  STATE_PUBLISHER_PUBLIC
   std::string state_interface_prefix_name() const;
 
-  STATE_PUBLISHER_PUBLIC
   std::string state_interface_interface_name() const;
 
-  STATE_PUBLISHER_PUBLIC
-  controller_manager_msgs::msg::PublisherDescription create_description_msg() const;
+  controller_manager_msgs::msg::PublisherDescription create_publisher_description_msg() const;
 
 private:
   void publish_value_on_timer();
 
-  const std::string namespace_;
   std::unique_ptr<hardware_interface::LoanedStateInterface> loaned_state_interface_ptr_;
+  const std::string namespace_;
   const std::string topic_name_;
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
   rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr state_value_pub_;

hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp

@@ -17,10 +17,12 @@ class SubControllerManagerWrapper final
 public:
   explicit SubControllerManagerWrapper(
     const std::string & ns, const std::string & name,
-    const std::vector<controller_manager_msgs::msg::PublisherDescription> & state_publishers)
+    const std::vector<controller_manager_msgs::msg::PublisherDescription> & state_publishers,
+    std::vector<controller_manager_msgs::msg::PublisherDescription> & command_forwarders)
   : NAMESPACE_(ns),
     NAME_(name),
-    state_publisher_descriptions_({state_publishers.begin(), state_publishers.end()})
+    state_publisher_descriptions_({state_publishers.begin(), state_publishers.end()}),
+    command_forwarder_descriptions_({command_forwarders.begin(), command_forwarders.end()})
   {
   }
 
@@ -41,12 +43,20 @@ class SubControllerManagerWrapper final
     return state_publisher_descriptions_;
   }
 
+  std::vector<PublisherDescription> get_command_forwarder_descriptions() const
+  {
+    return command_forwarder_descriptions_;
+  }
+
   size_t get_state_publisher_count() const { return state_publisher_descriptions_.size(); }
 
+  size_t get_command_forwarder_count() const { return command_forwarder_descriptions_.size(); }
+
 private:
   const std::string NAMESPACE_;
   const std::string NAME_;
   std::vector<PublisherDescription> state_publisher_descriptions_;
+  std::vector<PublisherDescription> command_forwarder_descriptions_;
 };
 
 }  // namespace distributed_control