import controller_manager

controller_manager/CMakeLists.txt

@@ -0,0 +1,106 @@
+cmake_minimum_required(VERSION 3.5)
+project(controller_manager)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+find_package(ament_cmake)
+find_package(ament_cmake_core REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(class_loader REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(rclcpp REQUIRED)
+
+add_library(controller_manager SHARED src/controller_manager.cpp)
+target_include_directories(controller_manager PRIVATE include)
+ament_target_dependencies(controller_manager
+  "ament_index_cpp"
+  "class_loader"
+  "controller_interface"
+  "hardware_interface"
+  "rclcpp"
+)
+if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+  target_link_libraries(controller_manager "stdc++fs")
+endif()
+# 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_manager PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
+
+install(TARGETS controller_manager
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+)
+install(DIRECTORY include/
+  DESTINATION include
+)
+install(DIRECTORY cmake/
+  DESTINATION share/${PROJECT_NAME}/cmake
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  ament_index_get_prefix_path(ament_index_build_path SKIP_AMENT_PREFIX_PATH)
+  # Get the first item (it will be the build space version of the build path).
+  list(GET ament_index_build_path 0 ament_index_build_path)
+  if(WIN32)
+    # On Windows prevent CMake errors and prevent it being evaluated as a list.
+    string(REPLACE "\\" "/" ament_index_build_path "${ament_index_build_path}")
+  endif()
+
+  add_library(test_controller SHARED test/test_controller/test_controller.cpp)
+  target_include_directories(test_controller PRIVATE include)
+  target_link_libraries(test_controller controller_manager)
+  target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
+
+  include(cmake/controller_manager_register_controller.cmake)
+  controller_manager_register_controller(
+    test_controller "test_controller::TestController"
+    SKIP_INSTALL)
+
+  ament_add_gtest(
+    test_controller_manager
+    test/test_controller_manager.cpp
+  )
+  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_gtest(
+    test_load_controller
+    test/test_load_controller.cpp
+    APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>
+  )
+  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
+  )
+endif()
+
+ament_export_libraries(
+  controller_manager
+)
+ament_export_include_directories(
+  include
+)
+ament_export_dependencies(
+  class_loader
+)
+ament_package(
+  CONFIG_EXTRAS cmake/controller_manager_register_controller.cmake
+)

controller_manager/cmake/controller_manager_register_controller.cmake

@@ -0,0 +1,63 @@
+# Copyright 2017 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.
+
+find_package(ament_cmake_core REQUIRED)
+
+macro(controller_manager_register_controller target)
+  if(NOT TARGET ${target})
+    message(
+      FATAL_ERROR
+      "controller_manager_register_controller() first argument "
+      "'${target}' is not a target")
+  endif()
+
+  cmake_parse_arguments(_ARG "SKIP_INSTALL" "" "" ${ARGN})
+
+  get_target_property(_target_type ${target} TYPE)
+  if(NOT _target_type STREQUAL "SHARED_LIBRARY")
+    message(
+      FATAL_ERROR
+      "controller_manager_register_controller() first argument "
+      "'${target}' is not a shared library target")
+  endif()
+  set(_unique_names)
+  foreach(_arg ${_ARG_UNPARSED_ARGUMENTS})
+    if(_arg IN_LIST _unique_names)
+      message(
+        FATAL_ERROR
+        "controller_manager_register_controller() the plugin names "
+        "must be unique (multiple '${_arg}')")
+    endif()
+    list(APPEND _unique_names "${_arg}")
+
+    if(_ARG_SKIP_INSTALL)
+      set(_library_path $<TARGET_FILE:${target}>)
+    else()
+      if(WIN32)
+        set(_path "bin")
+      else()
+        set(_path "lib")
+      endif()
+      set(_library_path ${_path}/$<TARGET_FILE_NAME:${target}>)
+    endif()
+    set(_ROS_CONTROLLERS
+      "${_ROS_CONTROLLERS}${_arg};${_library_path}\n")
+  endforeach()
+
+  if(_ARG_SKIP_INSTALL)
+    ament_index_register_resource("ros_controllers" CONTENT ${_ROS_CONTROLLERS} AMENT_INDEX_BINARY_DIR "${CMAKE_BINARY_DIR}/ament_cmake_index_$<CONFIG>" SKIP_INSTALL)
+  else()
+    ament_index_register_resource("ros_controllers" CONTENT ${_ROS_CONTROLLERS})
+  endif()
+endmacro()

controller_manager/include/controller_manager/controller_manager.hpp

@@ -0,0 +1,110 @@
+// Copyright 2017 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__CONTROLLER_MANAGER_HPP_
+#define CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_
+
+#include <memory>
+#include <string>
+#include <tuple>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+
+#include "controller_manager/visibility_control.h"
+
+#include "hardware_interface/robot_hardware.hpp"
+
+#include "rclcpp/executor.hpp"
+#include "rclcpp/node.hpp"
+
+namespace class_loader
+{
+class ClassLoader;
+}  // namespace class_loader
+
+namespace controller_manager
+{
+
+class ControllerManager : public rclcpp::node::Node
+{
+public:
+  CONTROLLER_MANAGER_PUBLIC
+  ControllerManager(
+    std::shared_ptr<hardware_interface::RobotHardware> hw,
+    std::shared_ptr<rclcpp::executor::Executor> executor,
+    const std::string & name = "controller_manager");
+
+  CONTROLLER_MANAGER_PUBLIC
+  virtual
+  ~ControllerManager() = default;
+
+  CONTROLLER_MANAGER_PUBLIC
+  std::shared_ptr<controller_interface::ControllerInterface>
+  load_controller(
+    const std::string & package_name,
+    const std::string & class_name,
+    const std::string & controller_name);
+
+  CONTROLLER_MANAGER_PUBLIC
+  std::vector<std::shared_ptr<controller_interface::ControllerInterface>>
+  get_loaded_controller() const;
+
+  template<
+    typename T,
+    typename std::enable_if<std::is_convertible<
+      T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr>
+  std::shared_ptr<controller_interface::ControllerInterface>
+  add_controller(std::shared_ptr<T> controller, std::string controller_name)
+  {
+    return add_controller_impl(controller, controller_name);
+  }
+
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::controller_interface_ret_t
+  update();
+
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::controller_interface_ret_t
+  configure() const;
+
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::controller_interface_ret_t
+  activate() const;
+
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::controller_interface_ret_t
+  deactivate() const;
+
+  CONTROLLER_MANAGER_PUBLIC
+  controller_interface::controller_interface_ret_t
+  cleanup() const;
+
+protected:
+  CONTROLLER_MANAGER_PUBLIC
+  std::shared_ptr<controller_interface::ControllerInterface>
+  add_controller_impl(
+    std::shared_ptr<controller_interface::ControllerInterface> controller,
+    const std::string & controller_name);
+
+private:
+  std::shared_ptr<hardware_interface::RobotHardware> hw_;
+  std::shared_ptr<rclcpp::executor::Executor> executor_;
+  std::vector<std::shared_ptr<class_loader::ClassLoader>> loaders_;
+  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> loaded_controllers_;
+};
+
+}  // namespace controller_manager
+
+#endif  // CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_

controller_manager/include/controller_manager/visibility_control.h

@@ -0,0 +1,56 @@
+// Copyright 2017 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.
+
+/* This header must be included by all rclcpp headers which declare symbols
+ * which are defined in the rclcpp library. When not building the rclcpp
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the rclcpp
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+
+#ifndef CONTROLLER_MANAGER__VISIBILITY_CONTROL_H_
+#define CONTROLLER_MANAGER__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define CONTROLLER_MANAGER_EXPORT __attribute__ ((dllexport))
+    #define CONTROLLER_MANAGER_IMPORT __attribute__ ((dllimport))
+  #else
+    #define CONTROLLER_MANAGER_EXPORT __declspec(dllexport)
+    #define CONTROLLER_MANAGER_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef CONTROLLER_MANAGER_BUILDING_DLL
+    #define CONTROLLER_MANAGER_PUBLIC CONTROLLER_MANAGER_EXPORT
+  #else
+    #define CONTROLLER_MANAGER_PUBLIC CONTROLLER_MANAGER_IMPORT
+  #endif
+  #define CONTROLLER_MANAGER_PUBLIC_TYPE CONTROLLER_MANAGER_PUBLIC
+  #define CONTROLLER_MANAGER_LOCAL
+#else
+  #define CONTROLLER_MANAGER_EXPORT __attribute__ ((visibility("default")))
+  #define CONTROLLER_MANAGER_IMPORT
+  #if __GNUC__ >= 4
+    #define CONTROLLER_MANAGER_PUBLIC __attribute__ ((visibility("default")))
+    #define CONTROLLER_MANAGER_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define CONTROLLER_MANAGER_PUBLIC
+    #define CONTROLLER_MANAGER_LOCAL
+  #endif
+  #define CONTROLLER_MANAGER_PUBLIC_TYPE
+#endif
+
+#endif  // CONTROLLER_MANAGER__VISIBILITY_CONTROL_H_

controller_manager/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>controller_manager</name>
+  <version>0.0.0</version>
+  <description>Description of controller_manager</description>
+  <maintainer email="karsten@osrfoundation.org">karsten</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>ament_index_cpp</build_depend>
+  <build_depend>class_loader</build_depend>
+  <build_depend>controller_interface</build_depend>
+  <build_depend>hardware_interface</build_depend>
+  <build_depend>rclcpp</build_depend>
+
+  <exec_depend>ament_index_cpp</exec_depend>
+  <exec_depend>class_loader</exec_depend>
+  <exec_depend>controller_interface</exec_depend>
+  <exec_depend>hardware_interface</exec_depend>
+  <exec_depend>rclcpp</exec_depend>
+
+  <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>
+  </export>
+</package>