From f6c3ba79ea487e6acd85abf7637c341f387734f5 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 14 Sep 2017 19:27:27 -0700 Subject: [PATCH] import controller_manager --- controller_manager/CMakeLists.txt | 106 ++++++++ ...ntroller_manager_register_controller.cmake | 63 +++++ .../controller_manager/controller_manager.hpp | 110 ++++++++ .../controller_manager/visibility_control.h | 56 ++++ controller_manager/package.xml | 32 +++ controller_manager/src/controller_manager.cpp | 246 ++++++++++++++++++ .../test/test_controller/test_controller.cpp | 48 ++++ .../test/test_controller/test_controller.hpp | 50 ++++ .../test/test_controller_manager.cpp | 78 ++++++ .../test/test_load_controller.cpp | 126 +++++++++ 10 files changed, 915 insertions(+) create mode 100644 controller_manager/CMakeLists.txt create mode 100644 controller_manager/cmake/controller_manager_register_controller.cmake create mode 100644 controller_manager/include/controller_manager/controller_manager.hpp create mode 100644 controller_manager/include/controller_manager/visibility_control.h create mode 100644 controller_manager/package.xml create mode 100644 controller_manager/src/controller_manager.cpp create mode 100644 controller_manager/test/test_controller/test_controller.cpp create mode 100644 controller_manager/test/test_controller/test_controller.hpp create mode 100644 controller_manager/test/test_controller_manager.cpp create mode 100644 controller_manager/test/test_load_controller.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt new file mode 100644 index 0000000000..e52864a616 --- /dev/null +++ b/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}_$ + ) + 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 +) diff --git a/controller_manager/cmake/controller_manager_register_controller.cmake b/controller_manager/cmake/controller_manager_register_controller.cmake new file mode 100644 index 0000000000..9183a249e9 --- /dev/null +++ b/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 $) + else() + if(WIN32) + set(_path "bin") + else() + set(_path "lib") + endif() + set(_library_path ${_path}/$) + 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_$" SKIP_INSTALL) + else() + ament_index_register_resource("ros_controllers" CONTENT ${_ROS_CONTROLLERS}) + endif() +endmacro() diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp new file mode 100644 index 0000000000..420bfc6a94 --- /dev/null +++ b/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 +#include +#include +#include + +#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 hw, + std::shared_ptr executor, + const std::string & name = "controller_manager"); + + CONTROLLER_MANAGER_PUBLIC + virtual + ~ControllerManager() = default; + + CONTROLLER_MANAGER_PUBLIC + std::shared_ptr + load_controller( + const std::string & package_name, + const std::string & class_name, + const std::string & controller_name); + + CONTROLLER_MANAGER_PUBLIC + std::vector> + get_loaded_controller() const; + + template< + typename T, + typename std::enable_if::value, T>::type * = nullptr> + std::shared_ptr + add_controller(std::shared_ptr 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 + add_controller_impl( + std::shared_ptr controller, + const std::string & controller_name); + +private: + std::shared_ptr hw_; + std::shared_ptr executor_; + std::vector> loaders_; + std::vector> loaded_controllers_; +}; + +} // namespace controller_manager + +#endif // CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_ diff --git a/controller_manager/include/controller_manager/visibility_control.h b/controller_manager/include/controller_manager/visibility_control.h new file mode 100644 index 0000000000..d3628b9c11 --- /dev/null +++ b/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_ diff --git a/controller_manager/package.xml b/controller_manager/package.xml new file mode 100644 index 0000000000..18b9ad32f0 --- /dev/null +++ b/controller_manager/package.xml @@ -0,0 +1,32 @@ + + + + controller_manager + 0.0.0 + Description of controller_manager + karsten + Apache License 2.0 + + ament_cmake + + ament_index_cpp + class_loader + controller_interface + hardware_interface + rclcpp + + ament_index_cpp + class_loader + controller_interface + hardware_interface + rclcpp + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + test_robot_hardware + + + ament_cmake + + diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp new file mode 100644 index 0000000000..bf04d04df6 --- /dev/null +++ b/controller_manager/src/controller_manager.cpp @@ -0,0 +1,246 @@ +// 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. + +#include "controller_manager/controller_manager.hpp" + +#include +#include +#include + +#include "ament_index_cpp/get_resource.hpp" +#include "ament_index_cpp/get_resources.hpp" + +#include "class_loader/class_loader.h" + +#include "controller_interface/controller_interface.hpp" + +#include "lifecycle_msgs/msg/state.hpp" + +#include "rcutils/format_string.h" +#include "rcutils/logging_macros.h" +#include "rcutils/split.h" +#include "rcutils/types.h" + +#ifdef __clang__ +// TODO(dirk-thomas) custom implementation until we can use libc++ 3.9 +namespace fs +{ +class path +{ +public: + explicit path(const std::string & p) + : path_(p) + {} + bool is_absolute() + { + return path_[0] == '/'; + } + +private: + std::string path_; +}; +} // namespace fs +#else +# include +namespace fs = std::experimental::filesystem; +#endif + +namespace controller_manager +{ + +static constexpr const char * resource_index = "ros_controllers"; + +namespace +{ + +std::string +parse_library_path( + const std::string & package_name, + const std::string & class_name) +{ + std::string library_path = ""; + + auto allocator = rcutils_get_default_allocator(); + // get node plugin resource from package + std::string content; + std::string base_path; + if (!ament_index_cpp::get_resource(resource_index, package_name, content, &base_path)) { + auto error_msg = rcutils_format_string( + allocator, "unable to load resource for package %s", package_name.c_str()); + RCUTILS_LOG_ERROR(error_msg) + throw std::runtime_error(error_msg); + } + + rcutils_string_array_t controller_array = rcutils_get_zero_initialized_string_array(); + rcutils_split(content.c_str(), '\n', allocator, &controller_array); + if (controller_array.size == 0) { + auto error_msg = rcutils_format_string( + allocator, "no ros controllers found in package %s", package_name.c_str()); + RCUTILS_LOG_ERROR(error_msg) + throw std::runtime_error(error_msg); + } + + bool controller_is_available = false; + for (auto i = 0u; i < controller_array.size; ++i) { + rcutils_string_array_t controller_details = rcutils_get_zero_initialized_string_array(); + rcutils_split(controller_array.data[i], ';', allocator, &controller_details); + if (controller_details.size != 2) { + auto error_msg = rcutils_format_string( + allocator, + "package resource content has wrong format. should be ;", + package_name.c_str()); + RCUTILS_LOG_ERROR(error_msg) + throw std::runtime_error(error_msg); + } + + if (strcmp(controller_details.data[0], class_name.c_str()) == 0) { + library_path = controller_details.data[1]; + if (!fs::path(library_path).is_absolute()) { + library_path = base_path + "/" + controller_details.data[1]; + } + controller_is_available = true; + break; + } + } + + if (!controller_is_available) { + auto error_msg = rcutils_format_string( + allocator, "couldn't find controller class %s", class_name.c_str()); + RCUTILS_LOG_ERROR(error_msg) + throw std::runtime_error(error_msg); + } + + return library_path; +} + +} // namespace + +ControllerManager::ControllerManager( + std::shared_ptr hw, + std::shared_ptr executor, + const std::string & manager_node_name) +: rclcpp::node::Node(manager_node_name), + hw_(hw), + executor_(executor) +{} + +std::shared_ptr +ControllerManager::load_controller( + const std::string & package_name, + const std::string & class_name, + const std::string & controller_name) +{ + auto library_path = parse_library_path(package_name, class_name); + + RCUTILS_LOG_INFO("going to load controller %s from library %s\n", + class_name.c_str(), library_path.c_str()) + + // let possible exceptions escalate + auto loader = std::make_shared(library_path); + std::shared_ptr controller = + loader->createInstance(class_name); + loaders_.push_back(loader); + + return add_controller_impl(controller, controller_name); +} + +std::vector> +ControllerManager::get_loaded_controller() const +{ + return loaded_controllers_; +} + +std::shared_ptr +ControllerManager::add_controller_impl( + std::shared_ptr controller, + const std::string & controller_name) +{ + controller->init(hw_, controller_name); + executor_->add_node(controller->get_lifecycle_node()->get_node_base_interface()); + + loaded_controllers_.emplace_back(controller); + return loaded_controllers_.back(); +} + +controller_interface::controller_interface_ret_t +ControllerManager::update() +{ + auto ret = controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS; + for (auto loaded_controller : loaded_controllers_) { + auto controller_ret = loaded_controller->update(); + if (controller_ret != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) { + ret = controller_ret; + } + } + + return ret; +} + +controller_interface::controller_interface_ret_t +ControllerManager::configure() const +{ + auto ret = controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS; + for (auto loaded_controller : loaded_controllers_) { + auto controller_state = loaded_controller->get_lifecycle_node()->configure(); + if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + ret = controller_interface::CONTROLLER_INTERFACE_RET_ERROR; + } + } + + return ret; +} + +controller_interface::controller_interface_ret_t +ControllerManager::activate() const +{ + auto ret = controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS; + for (auto loaded_controller : loaded_controllers_) { + auto controller_state = loaded_controller->get_lifecycle_node()->activate(); + if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + ret = controller_interface::CONTROLLER_INTERFACE_RET_ERROR; + } + } + + return ret; +} + +controller_interface::controller_interface_ret_t +ControllerManager::deactivate() const +{ + auto ret = controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS; + for (auto loaded_controller : loaded_controllers_) { + auto controller_state = loaded_controller->get_lifecycle_node()->deactivate(); + if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + ret = controller_interface::CONTROLLER_INTERFACE_RET_ERROR; + } + } + + return ret; +} + +controller_interface::controller_interface_ret_t +ControllerManager::cleanup() const +{ + auto ret = controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS; + for (auto loaded_controller : loaded_controllers_) { + auto controller_state = loaded_controller->get_lifecycle_node()->cleanup(); + if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { + ret = controller_interface::CONTROLLER_INTERFACE_RET_ERROR; + } + } + + return ret; +} + +} // namespace controller_manager diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp new file mode 100644 index 0000000000..59fc35fe5f --- /dev/null +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -0,0 +1,48 @@ +// 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. + +#include "./test_controller.hpp" + +#include +#include + +#include "lifecycle_msgs/msg/transition.hpp" + +namespace test_controller +{ + +TestController::TestController() +: controller_interface::ControllerInterface() +{} + +controller_interface::controller_interface_ret_t +TestController::update() +{ + ++internal_counter; + return controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS; +} + +rcl_lifecycle_transition_key_t +TestController::on_configure(const rclcpp_lifecycle::State & previous_state) +{ + (void) previous_state; + return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; +} + +} // namespace test_controller + +#include "class_loader/class_loader_register_macro.h" + +CLASS_LOADER_REGISTER_CLASS( + test_controller::TestController, controller_interface::ControllerInterface) diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp new file mode 100644 index 0000000000..46cb9668c1 --- /dev/null +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -0,0 +1,50 @@ +// 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 TEST_CONTROLLER__TEST_CONTROLLER_HPP_ +#define TEST_CONTROLLER__TEST_CONTROLLER_HPP_ + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "controller_interface/visibility_control.h" + +namespace test_controller +{ + +class TestController : public controller_interface::ControllerInterface +{ +public: + CONTROLLER_MANAGER_PUBLIC + TestController(); + + CONTROLLER_MANAGER_PUBLIC + virtual + ~TestController() = default; + + CONTROLLER_MANAGER_PUBLIC + controller_interface::controller_interface_ret_t + update() override; + + CONTROLLER_MANAGER_PUBLIC + rcl_lifecycle_transition_key_t + on_configure(const rclcpp_lifecycle::State & previous_state) override; + + size_t internal_counter = 0; +}; + +} // namespace test_controller + +#endif // TEST_CONTROLLER__TEST_CONTROLLER_HPP_ diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp new file mode 100644 index 0000000000..b4bd0c0bf0 --- /dev/null +++ b/controller_manager/test/test_controller_manager.cpp @@ -0,0 +1,78 @@ +// 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. + +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" + +#include "lifecycle_msgs/msg/state.hpp" + +#include "rclcpp/utilities.hpp" + +#include "test_robot_hardware/test_robot_hardware.hpp" + +#include "./test_controller/test_controller.hpp" + +class TestControllerManager : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::utilities::init(0, nullptr); + } + + void SetUp() + { + robot = std::make_shared(); + robot->init(); + + executor = std::make_shared(); + } + + std::shared_ptr robot; + std::shared_ptr executor; +}; + +TEST_F(TestControllerManager, controller_lifecycle) { + controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); + + auto test_controller = std::make_shared(); + auto abstract_test_controller = cm.add_controller(test_controller, "test_controller"); + EXPECT_EQ(1u, cm.get_loaded_controller().size()); + + EXPECT_EQ(controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS, cm.update()); + EXPECT_EQ(1u, test_controller->internal_counter); + + EXPECT_EQ(controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS, cm.configure()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_node()->get_current_state().id()); + + EXPECT_EQ(controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS, cm.activate()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller->get_lifecycle_node()->get_current_state().id()); + + EXPECT_EQ(controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS, cm.deactivate()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_node()->get_current_state().id()); + + EXPECT_EQ(controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS, cm.cleanup()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_node()->get_current_state().id()); +} diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp new file mode 100644 index 0000000000..b57f0a657b --- /dev/null +++ b/controller_manager/test/test_load_controller.cpp @@ -0,0 +1,126 @@ +// 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. + +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" + +#include "lifecycle_msgs/msg/state.hpp" + +#include "rclcpp/utilities.hpp" + +#include "test_robot_hardware/test_robot_hardware.hpp" + +#include "./test_controller/test_controller.hpp" + +class TestControllerManager : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::utilities::init(0, nullptr); + } + + void SetUp() + { + robot = std::make_shared(); + robot->init(); + + executor = std::make_shared(); + } + + std::shared_ptr robot; + std::shared_ptr executor; +}; + +TEST_F(TestControllerManager, load_unknown_controller) { + controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); + ASSERT_THROW( + cm.load_controller("unknown_package", "unknown_controller", "unknown_node"), + std::runtime_error); + ASSERT_THROW( + cm.load_controller("controller_manager", "unknown_controller", "unknown_node"), + std::runtime_error); +} + +TEST_F(TestControllerManager, load_known_controller) { + controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); + std::string class_name = "test_controller::TestController"; + std::string package_name = "controller_manager"; + std::string controller_name = "test_controller"; + ASSERT_NO_THROW(cm.load_controller(package_name, class_name, controller_name)); + EXPECT_EQ(1u, cm.get_loaded_controller().size()); + + std::shared_ptr abstract_test_controller = + cm.get_loaded_controller()[0]; + + auto lifecycle_node = abstract_test_controller->get_lifecycle_node(); + lifecycle_node->configure(); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller->get_lifecycle_node()->get_current_state().id()); +} + +TEST_F(TestControllerManager, load2_known_controller) { + controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); + std::string class_name = "test_controller::TestController"; + std::string package_name = "controller_manager"; + + // load the controller with name1 + std::string controller_name1 = "test_controller1"; + ASSERT_NO_THROW(cm.load_controller(package_name, class_name, controller_name1)); + EXPECT_EQ(1u, cm.get_loaded_controller().size()); + std::shared_ptr abstract_test_controller1 = + cm.get_loaded_controller()[0]; + EXPECT_STREQ( + controller_name1.c_str(), + abstract_test_controller1->get_lifecycle_node()->get_name()); + abstract_test_controller1->get_lifecycle_node()->configure(); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + + // load the same controller again with a different name + std::string controller_name2 = "test_controller2"; + ASSERT_NO_THROW(cm.load_controller(package_name, class_name, controller_name2)); + EXPECT_EQ(2u, cm.get_loaded_controller().size()); + std::shared_ptr abstract_test_controller2 = + cm.get_loaded_controller()[1]; + EXPECT_STREQ( + controller_name2.c_str(), + abstract_test_controller2->get_lifecycle_node()->get_name()); + abstract_test_controller2->get_lifecycle_node()->configure(); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2->get_lifecycle_node()->get_current_state().id()); +} + +TEST_F(TestControllerManager, update) { + controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); + std::string class_name = "test_controller::TestController"; + std::string package_name = "controller_manager"; + std::string controller_name = "test_controller"; + ASSERT_NO_THROW(cm.load_controller(package_name, class_name, controller_name)); + + std::shared_ptr abstract_test_controller = + cm.get_loaded_controller()[0]; + + auto lifecycle_node = abstract_test_controller->get_lifecycle_node(); + lifecycle_node->configure(); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller->get_lifecycle_node()->get_current_state().id()); +}