resource loaning

controller_manager/include/controller_manager/loaned_command_interface.hpp

@@ -0,0 +1,68 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONTROLLER_MANAGER__LOANED_COMMAND_INTERFACE_HPP_
+#define CONTROLLER_MANAGER__LOANED_COMMAND_INTERFACE_HPP_
+
+#include <utility>
+
+#include "hardware_interface/handle.hpp"
+
+namespace controller_manager
+{
+
+class LoanedCommandInterface
+{
+public:
+  using Deleter = std::function<void (void)>;
+
+  explicit LoanedCommandInterface(hardware_interface::CommandInterface & command_interface)
+  : LoanedCommandInterface(command_interface, nullptr)
+  {}
+
+  LoanedCommandInterface(
+    hardware_interface::CommandInterface & command_interface,
+    Deleter && deleter)
+  : command_interface_(command_interface),
+    deleter_(std::forward<Deleter>(deleter))
+  {}
+
+  LoanedCommandInterface(const LoanedCommandInterface & other) = delete;
+
+  LoanedCommandInterface(LoanedCommandInterface && other) = default;
+
+  virtual ~LoanedCommandInterface()
+  {
+    if (deleter_) {
+      deleter_();
+    }
+  }
+
+  void set_value(double val)
+  {
+    command_interface_.set_value(val);
+  }
+
+  double get_value()
+  {
+    return command_interface_.get_value();
+  }
+
+private:
+  hardware_interface::CommandInterface & command_interface_;
+  Deleter deleter_;
+};
+
+}  // namespace controller_manager
+#endif  // CONTROLLER_MANAGER__LOANED_COMMAND_INTERFACE_HPP_

controller_manager/include/controller_manager/loaned_state_interface.hpp

@@ -0,0 +1,63 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONTROLLER_MANAGER__LOANED_STATE_INTERFACE_HPP_
+#define CONTROLLER_MANAGER__LOANED_STATE_INTERFACE_HPP_
+
+#include <utility>
+
+#include "hardware_interface/handle.hpp"
+
+namespace controller_manager
+{
+
+class LoanedStateInterface
+{
+public:
+  using Deleter = std::function<void (void)>;
+
+  explicit LoanedStateInterface(hardware_interface::StateInterface & state_interface)
+  : LoanedStateInterface(state_interface, nullptr)
+  {}
+
+  LoanedStateInterface(
+    hardware_interface::StateInterface & state_interface,
+    Deleter && deleter)
+  : state_interface_(state_interface),
+    deleter_(std::forward<Deleter>(deleter))
+  {}
+
+  LoanedStateInterface(const LoanedStateInterface & other) = delete;
+
+  LoanedStateInterface(LoanedStateInterface && other) = default;
+
+  virtual ~LoanedStateInterface()
+  {
+    if (deleter_) {
+      deleter_();
+    }
+  }
+
+  double get_value()
+  {
+    return state_interface_.get_value();
+  }
+
+private:
+  hardware_interface::StateInterface & state_interface_;
+  Deleter deleter_;
+};
+
+}  // namespace controller_manager
+#endif  // CONTROLLER_MANAGER__LOANED_STATE_INTERFACE_HPP_

controller_manager/src/resource_manager.cpp

@@ -79,17 +79,23 @@ class ResourceStorage
   }
 
   template<class HardwareT>
-  void import_command_interfaces(HardwareT & hardware)
+  void import_command_interfaces(
+    HardwareT & hardware,
+    std::unordered_map<std::string, bool> & claimed_command_interface_map)
   {
     auto interfaces = hardware.export_command_interfaces();
     for (auto i = 0u; i < interfaces.size(); ++i) {
       auto key = interfaces[i].get_name() + "/" + interfaces[i].get_interface_name();
       command_interface_map_.emplace(
         std::make_pair(key, std::move(interfaces[i])));
+      claimed_command_interface_map.emplace(
+        std::make_pair(key, false));
     }
   }
 
-  void initialize_actuator(const hardware_interface::HardwareInfo & hardware_info)
+  void initialize_actuator(
+    const hardware_interface::HardwareInfo & hardware_info,
+    std::unordered_map<std::string, bool> & claimed_command_interface_map)
   {
     initialize_hardware<hardware_interface::components::Actuator,
       hardware_interface::components::ActuatorInterface>(
@@ -98,7 +104,7 @@ class ResourceStorage
       throw std::runtime_error(std::string("failed to configure ") + hardware_info.name);
     }
     import_state_interfaces(actuators_.back());
-    import_command_interfaces(actuators_.back());
+    import_command_interfaces(actuators_.back(), claimed_command_interface_map);
   }
 
   void initialize_sensor(const hardware_interface::HardwareInfo & hardware_info)
@@ -110,14 +116,16 @@ class ResourceStorage
     import_state_interfaces(sensors_.back());
   }
 
-  void initialize_system(const hardware_interface::HardwareInfo & hardware_info)
+  void initialize_system(
+    const hardware_interface::HardwareInfo & hardware_info,
+    std::unordered_map<std::string, bool> & claimed_command_interface_map)
   {
     initialize_hardware<hardware_interface::components::System,
       hardware_interface::components::SystemInterface>(
       hardware_info, system_loader_, systems_);
     systems_.back().configure(hardware_info);
     import_state_interfaces(systems_.back());
-    import_command_interfaces(systems_.back());
+    import_command_interfaces(systems_.back(), claimed_command_interface_map);
   }
 
   // hardware plugins
@@ -146,13 +154,13 @@ ResourceManager::ResourceManager(const std::string & urdf, bool validate_interfa
 
   for (const auto & hardware : hardware_info) {
     if (hardware.type == actuator_type) {
-      resource_storage_->initialize_actuator(hardware);
+      resource_storage_->initialize_actuator(hardware, claimed_command_interface_map_);
     }
     if (hardware.type == sensor_type) {
       resource_storage_->initialize_sensor(hardware);
     }
     if (hardware.type == system_type) {
-      resource_storage_->initialize_system(hardware);
+      resource_storage_->initialize_system(hardware, claimed_command_interface_map_);
     }
   }
 
@@ -204,6 +212,23 @@ ResourceManager::ResourceManager(const std::string & urdf, bool validate_interfa
   }
 }
 
+void ResourceManager::release_command_interface(const std::string & key)
+{
+  std::lock_guard<decltype(resource_lock_)> lg(resource_lock_);
+  claimed_command_interface_map_[key] = false;
+}
+
+LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key)
+{
+  if (!state_interface_exists(key)) {
+    throw std::runtime_error(
+            std::string("state interface with key") + key + " does not exist");
+  }
+
+  std::lock_guard<decltype(resource_lock_)> lg(resource_lock_);
+  return LoanedStateInterface(resource_storage_->state_interface_map_.at(key));
+}
+
 std::vector<std::string> ResourceManager::state_interface_keys() const
 {
   std::vector<std::string> keys;
@@ -219,6 +244,35 @@ bool ResourceManager::state_interface_exists(const std::string & key) const
          resource_storage_->state_interface_map_.end();
 }
 
+bool ResourceManager::command_interface_is_claimed(const std::string & key) const
+{
+  if (!command_interface_exists(key)) {
+    return false;
+  }
+
+  std::lock_guard<decltype(resource_lock_)> lg(resource_lock_);
+  return claimed_command_interface_map_.at(key);
+}
+
+LoanedCommandInterface ResourceManager::claim_command_interface(const std::string & key)
+{
+  if (!command_interface_exists(key)) {
+    throw std::runtime_error(
+            std::string("command interface with key") + key + " does not exist");
+  }
+
+  if (command_interface_is_claimed(key)) {
+    throw std::runtime_error(
+            std::string("command interface with key") + key + " is already claimed");
+  }
+
+  std::lock_guard<decltype(resource_lock_)> lg(resource_lock_);
+  claimed_command_interface_map_[key] = true;
+  return LoanedCommandInterface(
+    resource_storage_->command_interface_map_.at(key),
+    std::bind(&ResourceManager::release_command_interface, this, key));
+}
+
 std::vector<std::string> ResourceManager::command_interface_keys() const
 {
   std::vector<std::string> keys;
@@ -234,17 +288,47 @@ bool ResourceManager::command_interface_exists(const std::string & key) const
          resource_storage_->command_interface_map_.end();
 }
 
-size_t ResourceManager::actuator_interfaces_size() const
+void ResourceManager::import_component(
+  std::unique_ptr<hardware_interface::components::ActuatorInterface> actuator)
+{
+  resource_storage_->actuators_.emplace_back(
+    hardware_interface::components::Actuator(
+      std::move(
+        actuator)));
+  resource_storage_->import_state_interfaces(resource_storage_->actuators_.back());
+  resource_storage_->import_command_interfaces(
+    resource_storage_->actuators_.back(), claimed_command_interface_map_);
+}
+
+size_t ResourceManager::actuator_components_size() const
 {
   return resource_storage_->actuators_.size();
 }
 
-size_t ResourceManager::sensor_interfaces_size() const
+void ResourceManager::import_component(
+  std::unique_ptr<hardware_interface::components::SensorInterface> sensor)
+{
+  resource_storage_->sensors_.emplace_back(
+    hardware_interface::components::Sensor(std::move(sensor)));
+  resource_storage_->import_state_interfaces(resource_storage_->sensors_.back());
+}
+
+size_t ResourceManager::sensor_components_size() const
 {
   return resource_storage_->sensors_.size();
 }
 
-size_t ResourceManager::system_interfaces_size() const
+void ResourceManager::import_component(
+  std::unique_ptr<hardware_interface::components::SystemInterface> system)
+{
+  resource_storage_->systems_.emplace_back(
+    hardware_interface::components::System(std::move(system)));
+  resource_storage_->import_state_interfaces(resource_storage_->systems_.back());
+  resource_storage_->import_command_interfaces(
+    resource_storage_->systems_.back(), claimed_command_interface_map_);
+}
+
+size_t ResourceManager::system_components_size() const
 {
   return resource_storage_->systems_.size();
 }