Use resource manager

.github/workflows/ci.yml

@@ -41,7 +41,6 @@ jobs:
             controller_manager
             controller_manager_msgs
             hardware_interface
-            joint_limits_interface
             ros2_control
             test_robot_hardware
             transmission_interface

.github/workflows/lint.yml

@@ -12,7 +12,14 @@ jobs:
     - uses: ros-tooling/action-ros-lint@0.0.6
       with:
         linter: copyright
-        package-name: controller_interface controller_manager controller_manager_msgs hardware_interface joint_limits_interface test_robot_hardware transmission_interface
+        package-name:
+          controller_interface
+          controller_manager
+          controller_manager_msgs
+          hardware_interface
+          ros2_control
+          test_robot_hardware
+          transmission_interface
 
   ament_xmllint:
     name: ament_xmllint
@@ -23,7 +30,14 @@ jobs:
     - uses: ros-tooling/action-ros-lint@0.0.6
       with:
         linter: xmllint
-        package-name: controller_interface controller_manager controller_manager_msgs hardware_interface joint_limits_interface test_robot_hardware transmission_interface
+        package-name:
+          controller_interface
+          controller_manager
+          controller_manager_msgs
+          hardware_interface
+          ros2_control
+          test_robot_hardware
+          transmission_interface
 
   ament_lint_cpp:
     name: ament_${{ matrix.linter }}
@@ -38,4 +52,11 @@ jobs:
     - uses: ros-tooling/action-ros-lint@0.0.6
       with:
         linter: ${{ matrix.linter }}
-        package-name: controller_interface controller_manager controller_manager_msgs hardware_interface joint_limits_interface test_robot_hardware transmission_interface
+        package-name:
+          controller_interface
+          controller_manager
+          controller_manager_msgs
+          hardware_interface
+          ros2_control
+          test_robot_hardware
+          transmission_interface

controller_interface/include/controller_interface/controller_interface.hpp

@@ -17,10 +17,12 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "controller_interface/visibility_control.h"
 
-#include "hardware_interface/robot_hardware.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include "hardware_interface/loaned_state_interface.hpp"
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
@@ -34,6 +36,26 @@ enum class return_type : std::uint8_t
   ERROR = 1,
 };
 
+/// Indicating which interfaces are to be claimed.
+/**
+ * One might either claim all available command/state interfaces,
+ * specifying a set of individual interfaces,
+ * or none at all.
+ */
+enum class interface_configuration_type : std::uint8_t
+{
+  ALL = 0,
+  INDIVIDUAL = 1,
+  NONE = 2,
+};
+
+/// Configuring what command/state interfaces to claim.
+struct InterfaceConfiguration
+{
+  interface_configuration_type type;
+  std::vector<std::string> names = {};
+};
+
 class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
 {
 public:
@@ -44,12 +66,26 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
   virtual
   ~ControllerInterface() = default;
 
+  CONTROLLER_INTERFACE_PUBLIC
+  virtual
+  InterfaceConfiguration command_interface_configuration() const = 0;
+
+  CONTROLLER_INTERFACE_PUBLIC
+  virtual
+  InterfaceConfiguration state_interface_configuration() const = 0;
+
+  CONTROLLER_INTERFACE_PUBLIC
+  void assign_interfaces(
+    std::vector<hardware_interface::LoanedCommandInterface> && command_interfaces,
+    std::vector<hardware_interface::LoanedStateInterface> && state_interfaces);
+
+  CONTROLLER_INTERFACE_PUBLIC
+  void release_interfaces();
+
   CONTROLLER_INTERFACE_PUBLIC
   virtual
   return_type
-  init(
-    std::weak_ptr<hardware_interface::RobotHardware> robot_hardware,
-    const std::string & controller_name);
+  init(const std::string & controller_name);
 
   CONTROLLER_INTERFACE_PUBLIC
   virtual
@@ -61,7 +97,8 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
   get_lifecycle_node();
 
 protected:
-  std::weak_ptr<hardware_interface::RobotHardware> robot_hardware_;
+  std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
+  std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> lifecycle_node_;
 };
 

controller_interface/src/controller_interface.cpp

@@ -16,17 +16,19 @@
 
 #include <memory>
 #include <string>
+#include <utility>
+#include <vector>
 
 namespace controller_interface
 {
 
 return_type
-ControllerInterface::init(
-  std::weak_ptr<hardware_interface::RobotHardware> robot_hardware,
-  const std::string & controller_name)
+ControllerInterface::init(const std::string & controller_name)
 {
-  robot_hardware_ = robot_hardware;
-  lifecycle_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(controller_name);
+  lifecycle_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
+    controller_name,
+    rclcpp::NodeOptions().allow_undeclared_parameters(true).
+    automatically_declare_parameters_from_overrides(true));
 
   lifecycle_node_->register_on_configure(
     std::bind(&ControllerInterface::on_configure, this, std::placeholders::_1));
@@ -49,6 +51,20 @@ ControllerInterface::init(
   return return_type::SUCCESS;
 }
 
+void ControllerInterface::assign_interfaces(
+  std::vector<hardware_interface::LoanedCommandInterface> && command_interfaces,
+  std::vector<hardware_interface::LoanedStateInterface> && state_interfaces)
+{
+  command_interfaces_ = std::forward<decltype(command_interfaces)>(command_interfaces);
+  state_interfaces_ = std::forward<decltype(state_interfaces)>(state_interfaces);
+}
+
+void ControllerInterface::release_interfaces()
+{
+  command_interfaces_.clear();
+  state_interfaces_.clear();
+}
+
 std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
 ControllerInterface::get_lifecycle_node()
 {

controller_manager/CMakeLists.txt

@@ -37,11 +37,25 @@ target_compile_definitions(controller_manager PRIVATE "CONTROLLER_MANAGER_BUILDI
 # prevent pluginlib from using boost
 target_compile_definitions(controller_manager PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
+add_executable(ros2_control_node src/ros2_control_node.cpp)
+target_include_directories(ros2_control_node PRIVATE include)
+target_link_libraries(ros2_control_node controller_manager)
+ament_target_dependencies(ros2_control_node
+  controller_interface
+  hardware_interface
+  rclcpp
+)
+
 install(TARGETS controller_manager
   RUNTIME DESTINATION bin
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
 )
+
+install(TARGETS ros2_control_node
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
 install(DIRECTORY include/
   DESTINATION include
 )
@@ -77,10 +91,6 @@ if(BUILD_TESTING)
   )
   target_include_directories(test_controller_manager PRIVATE include)
   target_link_libraries(test_controller_manager controller_manager test_controller)
-  ament_target_dependencies(
-    test_controller_manager
-    test_robot_hardware
-  )
 
   ament_add_gmock(
     test_load_controller
@@ -89,10 +99,6 @@ if(BUILD_TESTING)
   )
   target_include_directories(test_load_controller PRIVATE include)
   target_link_libraries(test_load_controller controller_manager)
-  ament_target_dependencies(
-    test_load_controller
-    test_robot_hardware
-  )
 
   ament_add_gmock(
     test_controller_manager_srvs
@@ -101,10 +107,6 @@ if(BUILD_TESTING)
   )
   target_include_directories(test_controller_manager_srvs PRIVATE include)
   target_link_libraries(test_controller_manager_srvs controller_manager test_controller)
-  ament_target_dependencies(
-    test_controller_manager_srvs
-    test_robot_hardware
-  )
 endif()
 
 ament_export_libraries(
@@ -116,6 +118,8 @@ ament_export_include_directories(
 ament_export_dependencies(
   controller_interface
   controller_manager_msgs
+  hardware_interface
   pluginlib
+  rclcpp
 )
 ament_package()

controller_manager/include/controller_manager/controller_manager.hpp

@@ -26,12 +26,13 @@
 #include "controller_manager/visibility_control.h"
 #include "controller_manager_msgs/srv/list_controllers.hpp"
 #include "controller_manager_msgs/srv/list_controller_types.hpp"
+#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp"
 #include "controller_manager_msgs/srv/load_controller.hpp"
 #include "controller_manager_msgs/srv/reload_controller_libraries.hpp"
 #include "controller_manager_msgs/srv/switch_controller.hpp"
 #include "controller_manager_msgs/srv/unload_controller.hpp"
 
-#include "hardware_interface/robot_hardware.hpp"
+#include "hardware_interface/resource_manager.hpp"
 
 #include "pluginlib/class_loader.hpp"
 
@@ -44,14 +45,19 @@ namespace controller_manager
 class ControllerManager : public rclcpp::Node
 {
 public:
-  static constexpr bool WAIT_FOR_ALL_RESOURCES = false;
-  static constexpr double INFINITE_TIMEOUT = 0.0;
+  static constexpr bool kWaitForAllResources = false;
+  static constexpr double kInfiniteTimeout = 0.0;
 
   CONTROLLER_MANAGER_PUBLIC
   ControllerManager(
-    std::shared_ptr<hardware_interface::RobotHardware> hw,
+    std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
     std::shared_ptr<rclcpp::Executor> executor,
-    const std::string & name = "controller_manager");
+    const std::string & manager_node_name = "controller_manager");
+
+  CONTROLLER_MANAGER_PUBLIC
+  ControllerManager(
+    std::shared_ptr<rclcpp::Executor> executor,
+    const std::string & manager_node_name = "controller_manager");
 
   CONTROLLER_MANAGER_PUBLIC
   virtual
@@ -68,12 +74,10 @@ class ControllerManager : public rclcpp::Node
    */
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::ControllerInterfaceSharedPtr
-  load_controller(
-    const std::string & controller_name);
+  load_controller(const std::string & controller_name);
 
   CONTROLLER_MANAGER_PUBLIC
-  controller_interface::return_type unload_controller(
-    const std::string & controller_name);
+  controller_interface::return_type unload_controller(const std::string & controller_name);
 
   CONTROLLER_MANAGER_PUBLIC
   std::vector<ControllerSpec> get_loaded_controllers() const;
@@ -104,14 +108,30 @@ class ControllerManager : public rclcpp::Node
     const std::vector<std::string> & start_controllers,
     const std::vector<std::string> & stop_controllers,
     int strictness,
-    bool start_asap = WAIT_FOR_ALL_RESOURCES,
-    const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT));
+    bool start_asap = kWaitForAllResources,
+    const rclcpp::Duration & timeout = rclcpp::Duration(kInfiniteTimeout));
+
 
   CONTROLLER_MANAGER_PUBLIC
-  controller_interface::return_type
-  update();
+  void read();
+
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::return_type update();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void write();
+
+  /// Deterministic (real-time safe) callback group, e.g., update function.
+  /**
+   * Deterministic (real-time safe) callback group for the update function. Default behavior
+   * is read hardware, update controller and finally write new values to the hardware.
+   */
+  rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_;
 
 protected:
+  CONTROLLER_MANAGER_PUBLIC
+  void init_services();
+
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::ControllerInterfaceSharedPtr
   add_controller_impl(const ControllerSpec & controller);
@@ -138,6 +158,11 @@ class ControllerManager : public rclcpp::Node
     const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,
     std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);
 
+  CONTROLLER_MANAGER_PUBLIC
+  void list_hardware_interfaces_srv_cb(
+    const std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Request> request,
+    std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Response> response);
+
   CONTROLLER_MANAGER_PUBLIC
   void load_controller_service_cb(
     const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,
@@ -161,10 +186,19 @@ class ControllerManager : public rclcpp::Node
 private:
   std::vector<std::string> get_controller_names();
 
-  std::shared_ptr<hardware_interface::RobotHardware> hw_;
+  std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
+
   std::shared_ptr<rclcpp::Executor> executor_;
+
   std::shared_ptr<pluginlib::ClassLoader<controller_interface::ControllerInterface>> loader_;
 
+  /// Best effort (non real-time safe) callback group, e.g., service callbacks.
+  /**
+   * Best effort (non real-time safe) callback group for callbacks that can possibly break
+   * real-time requirements, for example, service callbacks.
+   */
+  rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_;
+
   /**
    * @brief The RTControllerListWrapper class wraps a double-buffered list of controllers
    * to avoid needing to lock the real-time thread when switching controllers in
@@ -245,6 +279,8 @@ class ControllerManager : public rclcpp::Node
     list_controllers_service_;
   rclcpp::Service<controller_manager_msgs::srv::ListControllerTypes>::SharedPtr
     list_controller_types_service_;
+  rclcpp::Service<controller_manager_msgs::srv::ListHardwareInterfaces>::SharedPtr
+    list_hardware_interfaces_service_;
   rclcpp::Service<controller_manager_msgs::srv::LoadController>::SharedPtr
     load_controller_service_;
   rclcpp::Service<controller_manager_msgs::srv::ReloadControllerLibraries>::SharedPtr

controller_manager/package.xml

@@ -21,7 +21,6 @@
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
-  <test_depend>test_robot_hardware</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>