Allocate memory for components and handles (#207)

* introducing handles

Signed-off-by: Karsten Knese <[email protected]>

* component interfaces & tests

Signed-off-by: Karsten Knese <[email protected]>

* linters

Signed-off-by: Karsten Knese <[email protected]>

* import resource manager

Signed-off-by: Karsten Knese <[email protected]>

* correct year

Signed-off-by: Karsten Knese <[email protected]>

* import handles from loaded components

Signed-off-by: Karsten Knese <[email protected]>

* wip / debug

Signed-off-by: Karsten Knese <[email protected]>

* parse components

Signed-off-by: Karsten Knese <[email protected]>

* changes after rebase

Signed-off-by: Karsten Knese <[email protected]>

* component parser as shared library

Signed-off-by: Karsten Knese <[email protected]>

* validate urdf configuratin

Signed-off-by: Karsten Knese <[email protected]>

* documentation

Signed-off-by: Karsten Knese <[email protected]>

* remove default constructor

Signed-off-by: Karsten Knese <[email protected]>

Author: Karsten1987

controller_manager/CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(rclcpp REQUIRED)
 
 add_library(controller_manager SHARED
   src/controller_manager.cpp
+  src/resource_manager.cpp
 )
 target_include_directories(controller_manager PRIVATE include)
 ament_target_dependencies(controller_manager
@@ -65,6 +66,11 @@ if(BUILD_TESTING)
   target_include_directories(test_controller PRIVATE include)
   target_link_libraries(test_controller controller_manager)
   target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
+  pluginlib_export_plugin_description_file(
+    controller_interface test/test_controller/test_controller.xml)
+  install(TARGETS test_controller
+    DESTINATION lib
+  )
 
   ament_add_gmock(
     test_controller_manager
@@ -101,11 +107,25 @@ if(BUILD_TESTING)
     test_robot_hardware
   )
 
-  pluginlib_export_plugin_description_file(controller_interface test/test_controller.xml)
-
-  install(TARGETS test_controller
+  add_library(test_components SHARED
+    test/test_components/test_actuator.cpp
+    test/test_components/test_sensor.cpp
+    test/test_components/test_system.cpp)
+  ament_target_dependencies(test_components
+    hardware_interface
+    pluginlib)
+  install(TARGETS test_components
     DESTINATION lib
   )
+  pluginlib_export_plugin_description_file(
+    hardware_interface test/test_components/test_components.xml)
+
+  ament_add_gmock(
+    test_resource_manager
+    test/test_resource_manager.cpp
+  )
+  target_include_directories(test_resource_manager PRIVATE include src)
+  target_link_libraries(test_resource_manager controller_manager)
 endif()
 
 ament_export_libraries(

controller_manager/src/resource_manager.cpp

@@ -0,0 +1,251 @@
+// 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.
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <unordered_map>
+#include <utility>
+
+#include "hardware_interface/components/actuator.hpp"
+#include "hardware_interface/components/actuator_interface.hpp"
+#include "hardware_interface/components/sensor.hpp"
+#include "hardware_interface/components/sensor_interface.hpp"
+#include "hardware_interface/components/system.hpp"
+#include "hardware_interface/components/system_interface.hpp"
+#include "hardware_interface/component_parser.hpp"
+#include "hardware_interface/hardware_info.hpp"
+
+#include "pluginlib/class_loader.hpp"
+
+#include "./resource_manager.hpp"
+
+namespace controller_manager
+{
+
+class ResourceStorage
+{
+  static constexpr const char * pkg_name = "hardware_interface";
+
+  static constexpr const char * actuator_interface_name =
+    "hardware_interface::components::ActuatorInterface";
+  static constexpr const char * sensor_interface_name =
+    "hardware_interface::components::SensorInterface";
+  static constexpr const char * system_interface_name =
+    "hardware_interface::components::SystemInterface";
+
+public:
+  ResourceStorage()
+  : actuator_loader_(pkg_name, actuator_interface_name),
+    sensor_loader_(pkg_name, sensor_interface_name),
+    system_loader_(pkg_name, system_interface_name)
+  {}
+
+  template<class HardwareT, class HardwareInterfaceT>
+  void initialize_hardware(
+    const hardware_interface::HardwareInfo & hardware_info,
+    pluginlib::ClassLoader<HardwareInterfaceT> & loader,
+    std::vector<HardwareT> & container)
+  {
+    // hardware_class_type has to match class name in plugin xml description
+    // TODO(karsten1987) extract package from hardware_class_type
+    // e.g.: <package_vendor>/<system_type>
+    auto interface = std::unique_ptr<HardwareInterfaceT>(
+      loader.createUnmanagedInstance(hardware_info.hardware_class_type));
+    HardwareT actuator(std::move(interface));
+    container.emplace_back(std::move(actuator));
+  }
+
+  template<class HardwareT>
+  void import_state_interfaces(HardwareT & hardware)
+  {
+    auto interfaces = hardware.export_state_interfaces();
+    for (auto i = 0u; i < interfaces.size(); ++i) {
+      auto key = interfaces[i].get_name() + "/" + interfaces[i].get_interface_name();
+      state_interface_map_.emplace(
+        std::make_pair(key, std::move(interfaces[i])));
+    }
+  }
+
+  template<class HardwareT>
+  void import_command_interfaces(HardwareT & hardware)
+  {
+    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])));
+    }
+  }
+
+  void initialize_actuator(const hardware_interface::HardwareInfo & hardware_info)
+  {
+    initialize_hardware<hardware_interface::components::Actuator,
+      hardware_interface::components::ActuatorInterface>(
+      hardware_info, actuator_loader_, actuators_);
+    if (hardware_interface::return_type::OK != actuators_.back().configure(hardware_info)) {
+      throw std::runtime_error(std::string("failed to configure ") + hardware_info.name);
+    }
+    import_state_interfaces(actuators_.back());
+    import_command_interfaces(actuators_.back());
+  }
+
+  void initialize_sensor(const hardware_interface::HardwareInfo & hardware_info)
+  {
+    initialize_hardware<hardware_interface::components::Sensor,
+      hardware_interface::components::SensorInterface>(
+      hardware_info, sensor_loader_, sensors_);
+    sensors_.back().configure(hardware_info);
+    import_state_interfaces(sensors_.back());
+  }
+
+  void initialize_system(const hardware_interface::HardwareInfo & hardware_info)
+  {
+    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());
+  }
+
+  // hardware plugins
+  pluginlib::ClassLoader<hardware_interface::components::ActuatorInterface> actuator_loader_;
+  pluginlib::ClassLoader<hardware_interface::components::SensorInterface> sensor_loader_;
+  pluginlib::ClassLoader<hardware_interface::components::SystemInterface> system_loader_;
+
+  std::vector<hardware_interface::components::Actuator> actuators_;
+  std::vector<hardware_interface::components::Sensor> sensors_;
+  std::vector<hardware_interface::components::System> systems_;
+
+  std::unordered_map<std::string, hardware_interface::StateInterface> state_interface_map_;
+  std::unordered_map<std::string, hardware_interface::CommandInterface> command_interface_map_;
+};
+
+ResourceManager::~ResourceManager() = default;
+
+ResourceManager::ResourceManager(const std::string & urdf, bool validate_interfaces)
+: resource_storage_(std::make_unique<ResourceStorage>())
+{
+  const std::string system_type = "system";
+  const std::string sensor_type = "sensor";
+  const std::string actuator_type = "actuator";
+
+  auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
+
+  for (const auto & hardware : hardware_info) {
+    if (hardware.type == actuator_type) {
+      resource_storage_->initialize_actuator(hardware);
+    }
+    if (hardware.type == sensor_type) {
+      resource_storage_->initialize_sensor(hardware);
+    }
+    if (hardware.type == system_type) {
+      resource_storage_->initialize_system(hardware);
+    }
+  }
+
+  auto validate_storage = [this, &hardware_info]() -> void
+    {
+      std::vector<std::string> missing_state_keys = {};
+      std::vector<std::string> missing_command_keys = {};
+
+      for (const auto & hardware : hardware_info) {
+        for (const auto & joint : hardware.joints) {
+          for (const auto & state_interface : joint.state_interfaces) {
+            if (!state_interface_exists(joint.name + "/" + state_interface.name)) {
+              missing_state_keys.emplace_back(joint.name + "/" + state_interface.name);
+            }
+          }
+          for (const auto & command_interface : joint.command_interfaces) {
+            if (!command_interface_exists(joint.name + "/" + command_interface.name)) {
+              missing_state_keys.emplace_back(joint.name + "/" + command_interface.name);
+            }
+          }
+        }
+        for (const auto & sensor : hardware.sensors) {
+          for (const auto & state_interface : sensor.state_interfaces) {
+            if (!state_interface_exists(sensor.name + "/" + state_interface.name)) {
+              missing_state_keys.emplace_back(sensor.name + "/" + state_interface.name);
+            }
+          }
+        }
+      }
+
+      if (!missing_state_keys.empty() || !missing_command_keys.empty()) {
+        std::string err_msg = "wrong state or command interface configuration.\n";
+        err_msg += "missing state interfaces:\n";
+        for (const auto & missing_key : missing_state_keys) {
+          err_msg += missing_key + "\t";
+        }
+        err_msg += "\nmissing command interfaces:\n";
+        for (const auto & missing_key : missing_command_keys) {
+          err_msg += missing_key + "\t";
+        }
+
+        throw std::runtime_error(err_msg);
+      }
+    };
+
+  // throw on missing state and command interfaces, not specified keys are being ignored
+  if (validate_interfaces) {
+    validate_storage();
+  }
+}
+
+std::vector<std::string> ResourceManager::state_interface_keys() const
+{
+  std::vector<std::string> keys;
+  for (const auto & item : resource_storage_->state_interface_map_) {
+    keys.push_back(std::get<0>(item));
+  }
+  return keys;
+}
+
+bool ResourceManager::state_interface_exists(const std::string & key) const
+{
+  return resource_storage_->state_interface_map_.find(key) !=
+         resource_storage_->state_interface_map_.end();
+}
+
+std::vector<std::string> ResourceManager::command_interface_keys() const
+{
+  std::vector<std::string> keys;
+  for (const auto & item : resource_storage_->command_interface_map_) {
+    keys.push_back(std::get<0>(item));
+  }
+  return keys;
+}
+
+bool ResourceManager::command_interface_exists(const std::string & key) const
+{
+  return resource_storage_->command_interface_map_.find(key) !=
+         resource_storage_->command_interface_map_.end();
+}
+
+size_t ResourceManager::actuator_interfaces_size() const
+{
+  return resource_storage_->actuators_.size();
+}
+
+size_t ResourceManager::sensor_interfaces_size() const
+{
+  return resource_storage_->sensors_.size();
+}
+
+size_t ResourceManager::system_interfaces_size() const
+{
+  return resource_storage_->systems_.size();
+}
+}  // namespace controller_manager