Add ros2 control managers

controller_interface/CMakeLists.txt

@@ -27,6 +27,8 @@ ament_target_dependencies(
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(controller_interface PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL")
+# prevent pluginlib from using boost
+target_compile_definitions(controller_interface PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
 install(DIRECTORY include/
   DESTINATION include

controller_interface/include/controller_interface/controller_interface.hpp

@@ -20,6 +20,7 @@
 
 #include "controller_interface/visibility_control.h"
 
+#include "hardware_interface/resource_manager.hpp"
 #include "hardware_interface/robot_hardware.hpp"
 
 #include "rclcpp/rclcpp.hpp"
@@ -65,7 +66,41 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> lifecycle_node_;
 };
 
+class ControllerInterfaceNewComponents
+  : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
+{
+public:
+  CONTROLLER_INTERFACE_PUBLIC
+  ControllerInterfaceNewComponents() = default;
+
+  CONTROLLER_INTERFACE_PUBLIC
+  virtual
+  ~ControllerInterfaceNewComponents() = default;
+
+  CONTROLLER_INTERFACE_PUBLIC
+  virtual
+  return_type
+  init(
+    std::weak_ptr<resource_manager::ResourceManager> resource_manager,
+    const std::string & controller_name);
+
+  CONTROLLER_INTERFACE_PUBLIC
+  virtual
+  return_type
+  update() = 0;
+
+  CONTROLLER_INTERFACE_PUBLIC
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
+  get_lifecycle_node();
+
+protected:
+  std::weak_ptr<resource_manager::ResourceManager> resource_manager_;
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> lifecycle_node_;
+};
+
 using ControllerInterfaceSharedPtr = std::shared_ptr<ControllerInterface>;
+using ControllerInterfaceNewComponentsSharedPtr =
+  std::shared_ptr<ControllerInterfaceNewComponents>;
 
 }  // namespace controller_interface
 

controller_interface/src/controller_interface.cpp

@@ -20,6 +20,15 @@
 namespace controller_interface
 {
 
+rclcpp::NodeOptions get_cm_node_options()
+{
+  rclcpp::NodeOptions node_options;
+  // Required for getting controllers paramaters without declaration
+  node_options.allow_undeclared_parameters(true);
+  node_options.automatically_declare_parameters_from_overrides(true);
+  return node_options;
+}
+
 return_type
 ControllerInterface::init(
   std::weak_ptr<hardware_interface::RobotHardware> robot_hardware,
@@ -55,4 +64,41 @@ ControllerInterface::get_lifecycle_node()
   return lifecycle_node_;
 }
 
+return_type
+ControllerInterfaceNewComponents::init(
+  std::weak_ptr<resource_manager::ResourceManager> resource_manager,
+  const std::string & controller_name)
+{
+  resource_manager_ = resource_manager;
+  lifecycle_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
+    controller_name,
+    get_cm_node_options());
+
+  lifecycle_node_->register_on_configure(
+    std::bind(&ControllerInterface::on_configure, this, std::placeholders::_1));
+
+  lifecycle_node_->register_on_cleanup(
+    std::bind(&ControllerInterface::on_cleanup, this, std::placeholders::_1));
+
+  lifecycle_node_->register_on_activate(
+    std::bind(&ControllerInterface::on_activate, this, std::placeholders::_1));
+
+  lifecycle_node_->register_on_deactivate(
+    std::bind(&ControllerInterface::on_deactivate, this, std::placeholders::_1));
+
+  lifecycle_node_->register_on_shutdown(
+    std::bind(&ControllerInterface::on_shutdown, this, std::placeholders::_1));
+
+  lifecycle_node_->register_on_error(
+    std::bind(&ControllerInterface::on_error, this, std::placeholders::_1));
+
+  return return_type::SUCCESS;
+}
+
+std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
+ControllerInterfaceNewComponents::get_lifecycle_node()
+{
+  return lifecycle_node_;
+}
+
 }  // namespace controller_interface

controller_manager/CMakeLists.txt

@@ -35,11 +35,33 @@ target_compile_definitions(controller_manager PRIVATE "CONTROLLER_MANAGER_BUILDI
 # prevent pluginlib from using boost
 target_compile_definitions(controller_manager PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
-install(TARGETS controller_manager
+add_library(ros2_control_manager SHARED src/ros2_control_manager.cpp)
+target_include_directories(ros2_control_manager PRIVATE include)
+target_link_libraries(ros2_control_manager controller_manager)
+ament_target_dependencies(ros2_control_manager
+  controller_interface
+  hardware_interface
+  rclcpp
+)
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(ros2_control_manager PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
+
+add_executable(ros2_control_node src/ros2_control_node.cpp)
+target_include_directories(ros2_control_node PRIVATE include)
+target_link_libraries(ros2_control_node ros2_control_manager)
+ament_target_dependencies(ros2_control_node
+  controller_interface
+  hardware_interface
+  rclcpp
+)
+
+install(TARGETS controller_manager ros2_control_manager ros2_control_node
   RUNTIME DESTINATION bin
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
 )
+
 install(DIRECTORY include/
   DESTINATION include
 )
@@ -108,13 +130,16 @@ endif()
 
 ament_export_libraries(
   controller_manager
+  ros2_control_manager
 )
 ament_export_include_directories(
   include
 )
 ament_export_dependencies(
   controller_interface
   controller_manager_msgs
+  hardware_interface
   pluginlib
+  rclcpp
 )
 ament_package()

controller_manager/include/controller_manager/controller_loader_interface.hpp

@@ -40,6 +40,10 @@ class ControllerLoaderInterface
   virtual controller_interface::ControllerInterfaceSharedPtr create(
     const std::string & controller_type) = 0;
 
+  CONTROLLER_MANAGER_PUBLIC
+  virtual controller_interface::ControllerInterfaceNewComponentsSharedPtr create_new_components(
+    const std::string & controller_type) = 0;
+
   CONTROLLER_MANAGER_PUBLIC
   virtual bool is_available(const std::string & controller_type) const = 0;
 

controller_manager/include/controller_manager/controller_loader_pluginlib.hpp

@@ -36,22 +36,60 @@ class ControllerLoaderPluginlib : public ControllerLoaderInterface
   virtual ~ControllerLoaderPluginlib() = default;
 
   CONTROLLER_MANAGER_PUBLIC
-  controller_interface::ControllerInterfaceSharedPtr create(const std::string & controller_type)
-  override;
+  controller_interface::ControllerInterfaceSharedPtr
+  create(const std::string & controller_type) override;
 
+  // Only for the interface compatibility
   CONTROLLER_MANAGER_PUBLIC
-  bool is_available(const std::string & controller_type) const override;
+  controller_interface::ControllerInterfaceNewComponentsSharedPtr
+  create_new_components(const std::string & controller_type) override;
 
   CONTROLLER_MANAGER_PUBLIC
   std::vector<std::string> get_declared_classes() const override;
 
+  CONTROLLER_MANAGER_PUBLIC
+  bool is_available(const std::string & controller_type) const override;
+
   CONTROLLER_MANAGER_PUBLIC
   void reload() override;
 
 private:
   std::shared_ptr<pluginlib::ClassLoader<controller_interface::ControllerInterface>> loader_;
 };
 
+class ControllerLoaderPluginlibNewComponents : public ControllerLoaderInterface
+{
+public:
+  CONTROLLER_MANAGER_PUBLIC
+  ControllerLoaderPluginlibNewComponents();
+
+  CONTROLLER_MANAGER_PUBLIC
+  virtual ~ControllerLoaderPluginlibNewComponents() = default;
+
+  // Only for the interface compatibility
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::ControllerInterfaceSharedPtr
+  create(const std::string & controller_type) override;
+
+  // TODO(anyone) new loader with components - rename to create
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::ControllerInterfaceNewComponentsSharedPtr
+  create_new_components(const std::string & controller_type) override;
+
+  CONTROLLER_MANAGER_PUBLIC
+  std::vector<std::string> get_declared_classes() const override;
+
+  CONTROLLER_MANAGER_PUBLIC
+  bool is_available(const std::string & controller_type) const override;
+
+  CONTROLLER_MANAGER_PUBLIC
+  void reload() override;
+
+private:
+  std::shared_ptr<pluginlib::ClassLoader<controller_interface::ControllerInterfaceNewComponents>>
+  loader_;
+};
+
 }  // namespace controller_manager
 
 #endif  // CONTROLLER_MANAGER__CONTROLLER_LOADER_PLUGINLIB_HPP_

controller_manager/include/controller_manager/controller_manager.hpp

@@ -32,6 +32,7 @@
 #include "controller_manager_msgs/srv/switch_controller.hpp"
 #include "controller_manager_msgs/srv/unload_controller.hpp"
 
+#include "hardware_interface/resource_manager.hpp"
 #include "hardware_interface/robot_hardware.hpp"
 
 #include "rclcpp/executor.hpp"
@@ -276,6 +277,61 @@ class ControllerManager : public rclcpp::Node
   SwitchParams switch_params_;
 };
 
+class ControllerManagerNewWithManager
+{
+public:
+  CONTROLLER_MANAGER_PUBLIC
+  ControllerManagerNewWithManager(
+    std::shared_ptr<resource_manager::ResourceManager> resource_manager,
+    std::shared_ptr<rclcpp::Executor> executor);
+
+  CONTROLLER_MANAGER_PUBLIC
+  virtual
+  ~ControllerManagerNewWithManager() = default;
+
+  CONTROLLER_MANAGER_PUBLIC
+  std::shared_ptr<controller_interface::ControllerInterfaceNewComponents>
+  load_controller(
+    const std::string & controller_name,
+    const std::string & controller_type);
+
+  template<
+    typename T,
+    typename std::enable_if<std::is_convertible<
+      T *, controller_interface::ControllerInterfaceNewComponents *>::value, T>::type * = nullptr>
+  std::shared_ptr<controller_interface::ControllerInterfaceNewComponents>
+  add_controller(std::shared_ptr<T> controller, std::string controller_name)
+  {
+    return add_controller_impl(controller, controller_name);
+  }
+
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::return_type
+  update();
+
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::return_type
+  configure() const;
+
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::return_type
+  activate() const;
+
+protected:
+  CONTROLLER_MANAGER_PUBLIC
+  std::shared_ptr<controller_interface::ControllerInterfaceNewComponents>
+  add_controller_impl(
+    std::shared_ptr<controller_interface::ControllerInterfaceNewComponents> controller,
+    const std::string & controller_name);
+
+private:
+  std::shared_ptr<resource_manager::ResourceManager> resource_manager_;
+  std::shared_ptr<rclcpp::Executor> executor_;
+  std::vector<ControllerLoaderInterfaceSharedPtr> loaders_;
+  std::vector<controller_interface::ControllerInterfaceNewComponentsSharedPtr>
+  loaded_controllers_;
+};
+
 }  // namespace controller_manager
 
 #endif  // CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_

controller_manager/include/controller_manager/ros2_control_manager.hpp

@@ -0,0 +1,54 @@
+// Copyright 2020 ROS2-Control Development Team
+//
+// 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__ROS2_CONTROL_MANAGER_HPP_
+#define CONTROLLER_MANAGER__ROS2_CONTROL_MANAGER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "controller_manager/controller_manager.hpp"
+#include "hardware_interface/resource_manager.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace control_manager
+{
+
+class ROS2ControlManager : public rclcpp::Node
+{
+public:
+  CONTROLLER_MANAGER_PUBLIC
+  explicit ROS2ControlManager(
+    std::shared_ptr<rclcpp::Executor> executor,
+    const std::string & manager_node_name = "control_manager",
+    rclcpp::NodeOptions options = rclcpp::NodeOptions()
+  );
+
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::return_type configure();
+
+private:
+  void loop();
+
+  std::shared_ptr<rclcpp::Executor> executor_;
+
+  std::unique_ptr<controller_manager::ControllerManagerNewWithManager> controller_manager_;
+  std::shared_ptr<resource_manager::ResourceManager> resource_manager_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+}  // namespace control_manager
+
+#endif  // CONTROLLER_MANAGER__ROS2_CONTROL_MANAGER_HPP_