change commandinterface from obj to shared_ptr

Author: mamueluth

controller_manager/src/controller_manager.cpp

@@ -516,12 +516,21 @@ void ControllerManager::register_sub_controller_manager()
   {
     // 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)
+    for (const 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);
+      auto [found, command_forwarder] = resource_manager_->find_command_forwarder(key);
+      if (found)
+      {
+        command_forwarder->subscribe_to_command_publisher(command_state_publisher.publisher_topic);
+      }
+      else
+      {
+        RCLCPP_WARN_STREAM(
+          get_logger(), "SubControllerManager: Could not find a CommandForwarder for key["
+                          << key << "]. No subscription to command state possible.");
+      }
     }
     RCLCPP_INFO(get_logger(), "SubControllerManager: Successfully registered StatePublishers.");
   }

hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp

@@ -45,7 +45,7 @@ class CommandForwarder final
 
   controller_manager_msgs::msg::PublisherDescription create_publisher_description_msg() const;
 
-  void subscribe_to_command(const distributed_control::PublisherDescription & description);
+  void subscribe_to_command_publisher(const std::string & topic_name);
 
 private:
   void publish_value_on_timer();

hardware_interface/include/hardware_interface/loaned_command_interface.hpp

@@ -28,12 +28,12 @@ class LoanedCommandInterface
 public:
   using Deleter = std::function<void(void)>;
 
-  explicit LoanedCommandInterface(CommandInterface & command_interface)
+  explicit LoanedCommandInterface(std::shared_ptr<ReadWriteHandle> command_interface)
   : LoanedCommandInterface(command_interface, nullptr)
   {
   }
 
-  LoanedCommandInterface(CommandInterface & command_interface, Deleter && deleter)
+  LoanedCommandInterface(std::shared_ptr<ReadWriteHandle> command_interface, Deleter && deleter)
   : command_interface_(command_interface), deleter_(std::forward<Deleter>(deleter))
   {
   }
@@ -50,30 +50,30 @@ class LoanedCommandInterface
     }
   }
 
-  std::string get_name() const { return command_interface_.get_name(); }
+  std::string get_name() const { return command_interface_->get_name(); }
 
-  std::string get_interface_name() const { return command_interface_.get_interface_name(); }
+  std::string get_interface_name() const { return command_interface_->get_interface_name(); }
 
   [[deprecated(
     "Replaced by get_name method, which is semantically more correct")]] const std::string
   get_full_name() const
   {
-    return command_interface_.get_name();
+    return command_interface_->get_name();
   }
 
-  std::string get_prefix_name() const { return command_interface_.get_prefix_name(); }
+  std::string get_prefix_name() const { return command_interface_->get_prefix_name(); }
 
-  void set_value(double val) { command_interface_.set_value(val); }
+  void set_value(double val) { command_interface_->set_value(val); }
 
-  double get_value() const { return command_interface_.get_value(); }
+  double get_value() const { return command_interface_->get_value(); }
 
   std::string get_underscore_separated_name() const
   {
-    return command_interface_.get_underscore_separated_name();
+    return command_interface_->get_underscore_separated_name();
   }
 
 protected:
-  CommandInterface & command_interface_;
+  std::shared_ptr<ReadWriteHandle> command_interface_;
   Deleter deleter_;
 };
 

hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp

@@ -78,10 +78,9 @@ CommandForwarder::create_publisher_description_msg() const
   return msg;
 }
 
-void CommandForwarder::subscribe_to_command(
-  const distributed_control::PublisherDescription & description)
+void CommandForwarder::subscribe_to_command_publisher(const std::string & topic_name)
 {
-  subscription_topic_name_ = description.topic_name();
+  subscription_topic_name_ = topic_name;
   command_subscription_ = node_->create_subscription<std_msgs::msg::Float64>(
     subscription_topic_name_, 10,
     std::bind(&CommandForwarder::forward_command, this, std::placeholders::_1));

hardware_interface/src/resource_manager.cpp

@@ -460,6 +460,33 @@ class ResourceStorage
     hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces);
   }
 
+  void add_command_interface(std::shared_ptr<ReadWriteHandle> command_interface)
+  {
+    const auto [it, success] = command_interface_map_.insert(
+      std::make_pair(command_interface->get_name(), command_interface));
+    if (!success)
+    {
+      std::string msg(
+        "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" +
+        command_interface->get_name() + "]");
+      throw std::runtime_error(msg);
+    }
+  }
+
+  void add_command_interface(CommandInterface && command_interface)
+  {
+    const auto [it, success] = command_interface_map_.emplace(std::make_pair(
+      command_interface.get_name(),
+      std::make_shared<CommandInterface>(std::move(command_interface))));
+    if (!success)
+    {
+      std::string msg(
+        "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" +
+        command_interface.get_name() + "]");
+      throw std::runtime_error(msg);
+    }
+  }
+
   /// Adds exported command interfaces into internal storage.
   /**
    * Add command interfaces to the internal storage. Command interfaces exported from hardware or
@@ -478,7 +505,7 @@ class ResourceStorage
     for (auto & interface : interfaces)
     {
       auto key = interface.get_name();
-      command_interface_map_.emplace(std::make_pair(key, std::move(interface)));
+      add_command_interface(std::move(interface));
       claimed_command_interface_map_.emplace(std::make_pair(key, false));
       interface_names.push_back(key);
     }
@@ -563,13 +590,13 @@ class ResourceStorage
   std::pair<bool, std::shared_ptr<distributed_control::CommandForwarder>> find_command_forwarder(
     const std::string & key)
   {
-    // auto command_forwarder = state_interface_state_publisher_map_.find(key);
-    // // we could not find a command forwarder for the provided key
-    // if (command_forwarder == state_interface_state_publisher_map_.end())
-    // {
-    //   return std::make_pair(false, nullptr);
-    // }
-    // return std::make_pair(true, command_forwarder->second);
+    auto command_forwarder = command_interface_command_forwarder_map_.find(key);
+    // we could not find a command forwarder for the provided key
+    if (command_forwarder == command_interface_command_forwarder_map_.end())
+    {
+      return std::make_pair(false, nullptr);
+    }
+    return std::make_pair(true, command_forwarder->second);
   }
 
   void check_for_duplicates(const HardwareInfo & hardware_info)
@@ -708,7 +735,7 @@ class ResourceStorage
   /// Storage of all available state interfaces
   std::map<std::string, std::shared_ptr<ReadOnlyHandle>> state_interface_map_;
   /// Storage of all available command interfaces
-  std::map<std::string, CommandInterface> command_interface_map_;
+  std::map<std::string, std::shared_ptr<ReadWriteHandle>> command_interface_map_;
 
   /// Vectors with interfaces available to controllers (depending on hardware component state)
   std::vector<std::string> available_state_interfaces_;
@@ -919,7 +946,7 @@ ResourceManager::get_command_forwarders() const
 std::pair<bool, std::shared_ptr<distributed_control::CommandForwarder>>
 ResourceManager::find_command_forwarder(const std::string & key)
 {
-  // return resource_storage_->find_command_forwarder(key);
+  return resource_storage_->find_command_forwarder(key);
 }
 
 // CM API: Called in "callback/slow"-thread