Hardware resource folder

controller_manager/CMakeLists.txt

@@ -19,7 +19,11 @@ find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 
-add_library(controller_manager SHARED src/controller_manager.cpp src/controller_loader_pluginlib.cpp)
+add_library(controller_manager SHARED
+  src/controller_manager.cpp
+  src/controller_loader_pluginlib.cpp
+  src/resource_manager.cpp
+)
 target_include_directories(controller_manager PRIVATE include)
 ament_target_dependencies(controller_manager
   ament_index_cpp
@@ -63,6 +67,10 @@ 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")
+  install(TARGETS test_controller
+    DESTINATION lib
+  )
+  pluginlib_export_plugin_description_file(controller_interface test/test_controller.xml)
 
   ament_add_gmock(
     test_controller_manager
@@ -99,11 +107,27 @@ if(BUILD_TESTING)
     test_robot_hardware
   )
 
-  pluginlib_export_plugin_description_file(controller_interface test/test_controller.xml)
-
-  install(TARGETS test_controller
+  add_library(test_hardware_resources SHARED
+    test/test_hardware_resources/test_actuator_hardware.cpp
+    test/test_hardware_resources/test_joint_component.cpp
+    test/test_hardware_resources/test_sensor_component.cpp
+    test/test_hardware_resources/test_sensor_hardware.cpp
+    test/test_hardware_resources/test_system_hardware.cpp)
+  ament_target_dependencies(test_hardware_resources
+    hardware_interface
+    pluginlib)
+  install(TARGETS test_hardware_resources
     DESTINATION lib
   )
+  pluginlib_export_plugin_description_file(
+    hardware_interface test/test_hardware_resources.xml)
+
+  ament_add_gtest(
+    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/include/controller_manager/controller_loader_pluginlib.hpp

@@ -36,10 +36,11 @@ class ControllerLoaderPluginlib : public ControllerLoaderInterface
   virtual ~ControllerLoaderPluginlib() = default;
 
   CONTROLLER_MANAGER_PUBLIC
-  controller_interface::ControllerInterfaceSharedPtr create(const std::string & controller_type);
+  controller_interface::ControllerInterfaceSharedPtr create(const std::string & controller_type)
+  override;
 
   CONTROLLER_MANAGER_PUBLIC
-  bool is_available(const std::string & controller_type) const;
+  bool is_available(const std::string & controller_type) const override;
 
   CONTROLLER_MANAGER_PUBLIC
   std::vector<std::string> get_declared_classes() const override;

controller_manager/src/resource_manager.cpp

@@ -0,0 +1,197 @@
+// 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 <utility>
+
+#include "hardware_interface/components/component_info.hpp"
+#include "hardware_interface/component_parser.hpp"
+#include "hardware_interface/hardware_resources/actuator_hardware.hpp"
+#include "hardware_interface/hardware_resources/hardware_info.hpp"
+#include "hardware_interface/hardware_resources/sensor_hardware.hpp"
+#include "hardware_interface/hardware_resources/system_hardware.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 * joint_component_interface_name =
+    "hardware_interface::components::Joint";
+  static constexpr const char * sensor_component_interface_name =
+    "hardware_interface::components::Sensor";
+
+  static constexpr const char * actuator_interface_name =
+    "hardware_interface::hardware_resources::ActuatorHardwareInterface";
+  static constexpr const char * sensor_interface_name =
+    "hardware_interface::hardware_resources::SensorHardwareInterface";
+  static constexpr const char * system_interface_name =
+    "hardware_interface::hardware_resources::SystemHardwareInterface";
+
+public:
+  ResourceStorage()
+  : joint_component_loader_(pkg_name, joint_component_interface_name),
+    sensor_component_loader_(pkg_name, sensor_component_interface_name),
+    actuator_loader_(pkg_name, actuator_interface_name),
+    sensor_loader_(pkg_name, sensor_interface_name),
+    system_loader_(pkg_name, system_interface_name)
+  {}
+
+  ~ResourceStorage() = default;
+
+  void initialize_joint_component(
+    const hardware_interface::components::ComponentInfo & component_info)
+  {
+    joint_components_.emplace_back(
+      std::unique_ptr<hardware_interface::components::Joint>(
+        joint_component_loader_.createUnmanagedInstance(component_info.class_type)));
+    joint_components_.back()->configure(component_info);
+  }
+
+  void initialize_sensor_component(
+    const hardware_interface::components::ComponentInfo & component_info)
+  {
+    sensor_components_.emplace_back(
+      std::unique_ptr<hardware_interface::components::Sensor>(
+        sensor_component_loader_.createUnmanagedInstance(component_info.class_type)));
+    sensor_components_.back()->configure(component_info);
+  }
+
+  template<class HardwareT, class HardwareInterfaceT>
+  void initialize_hardware(
+    const hardware_interface::hardware_resources::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));
+    container.back().configure(hardware_info);
+  }
+
+  void initialize_actuator(
+    const hardware_interface::hardware_resources::HardwareInfo & hardware_info)
+  {
+    initialize_hardware<hardware_interface::hardware_resources::ActuatorHardware,
+      hardware_interface::hardware_resources::ActuatorHardwareInterface>(
+      hardware_info, actuator_loader_, actuators_);
+  }
+
+  void initialize_sensor(const hardware_interface::hardware_resources::HardwareInfo & hardware_info)
+  {
+    initialize_hardware<hardware_interface::hardware_resources::SensorHardware,
+      hardware_interface::hardware_resources::SensorHardwareInterface>(
+      hardware_info, sensor_loader_, sensors_);
+  }
+
+  void initialize_system(const hardware_interface::hardware_resources::HardwareInfo & hardware_info)
+  {
+    initialize_hardware<hardware_interface::hardware_resources::SystemHardware,
+      hardware_interface::hardware_resources::SystemHardwareInterface>(
+      hardware_info, system_loader_, systems_);
+  }
+
+  // components plugins
+  pluginlib::ClassLoader<hardware_interface::components::Joint> joint_component_loader_;
+  pluginlib::ClassLoader<hardware_interface::components::Sensor> sensor_component_loader_;
+
+  std::vector<std::unique_ptr<hardware_interface::components::Joint>> joint_components_;
+  std::vector<std::unique_ptr<hardware_interface::components::Sensor>> sensor_components_;
+
+  // hardware plugins
+  pluginlib::ClassLoader<hardware_interface::hardware_resources::ActuatorHardwareInterface>
+  actuator_loader_;
+  pluginlib::ClassLoader<hardware_interface::hardware_resources::SensorHardwareInterface>
+  sensor_loader_;
+  pluginlib::ClassLoader<hardware_interface::hardware_resources::SystemHardwareInterface>
+  system_loader_;
+
+  std::vector<hardware_interface::hardware_resources::ActuatorHardware> actuators_;
+  std::vector<hardware_interface::hardware_resources::SensorHardware> sensors_;
+  std::vector<hardware_interface::hardware_resources::SystemHardware> systems_;
+};
+
+ResourceManager::ResourceManager()
+: resource_storage_(std::make_unique<ResourceStorage>())
+{}
+
+ResourceManager::~ResourceManager() = default;
+
+ResourceManager::ResourceManager(const std::string & urdf)
+: 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);
+    }
+
+    for (const auto & joint : hardware.joints) {
+      resource_storage_->initialize_joint_component(joint);
+    }
+
+    for (const auto & sensor : hardware.sensors) {
+      resource_storage_->initialize_sensor_component(sensor);
+    }
+  }
+}
+
+size_t ResourceManager::joint_components_size() const
+{
+  return resource_storage_->joint_components_.size();
+}
+
+size_t ResourceManager::sensor_components_size() const
+{
+  return resource_storage_->sensor_components_.size();
+}
+
+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

controller_manager/src/resource_manager.hpp

@@ -0,0 +1,55 @@
+// 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 RESOURCE_MANAGER_HPP_
+#define RESOURCE_MANAGER_HPP_
+
+#include <memory>
+#include <string>
+
+namespace controller_manager
+{
+
+class ResourceStorage;
+
+class ResourceManager
+{
+public:
+  ResourceManager();
+
+  explicit ResourceManager(const std::string & urdf);
+
+  ResourceManager(const ResourceManager &) = delete;
+
+  ~ResourceManager();
+
+  size_t joint_components_size() const;
+
+  size_t sensor_components_size() const;
+
+  size_t actuator_interfaces_size() const;
+
+  size_t sensor_interfaces_size() const;
+
+  size_t system_interfaces_size() const;
+
+  // loan_joint(const std::string & name);
+  // loan_sensor(const std::string & name);
+
+private:
+  std::unique_ptr<ResourceStorage> resource_storage_;
+};
+
+}  // namespace controller_manager
+#endif  // RESOURCE_MANAGER_HPP_

controller_manager/test/controller_manager_test_common.hpp

@@ -71,13 +71,15 @@ class ControllerLoaderMock : public controller_manager::ControllerLoaderInterfac
   ControllerLoaderMock()
   : controller_manager::ControllerLoaderInterface("controller_interface::MockControllerInterface")
   {}
-  MOCK_METHOD1(create, controller_interface::ControllerInterfaceSharedPtr(const std::string &));
-  MOCK_CONST_METHOD1(is_available, bool(const std::string &));
+  MOCK_METHOD(
+    controller_interface::ControllerInterfaceSharedPtr, create, (const std::string &),
+    (override));
+  MOCK_METHOD(bool, is_available, (const std::string &), (const, override));
   std::vector<std::string> get_declared_classes() const override
   {
     return {MOCK_TEST_CONTROLLER_NAME};
   }
-  MOCK_METHOD0(reload, void());
+  MOCK_METHOD(void, reload, (), (override));
 };
 
 #endif  // CONTROLLER_MANAGER_TEST_COMMON_HPP_