diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt new file mode 100644 index 0000000000..969971eb05 --- /dev/null +++ b/controller_interface/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.5) +project(controller_interface) + +# 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 REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +add_library(controller_interface SHARED src/controller_interface.cpp) +target_include_directories( + controller_interface + PRIVATE + include) +ament_target_dependencies( + controller_interface + "hardware_interface" + "rclcpp_lifecycle" +) +# 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") + +install(DIRECTORY include/ + DESTINATION include +) +install(TARGETS controller_interface + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_dependencies( + hardware_interface + rclcpp_lifecycle +) +ament_export_include_directories( + include +) +ament_export_libraries( + controller_interface +) +ament_package() diff --git a/controller_interface/cmake/controller_interface_configure_controller_library.cmake b/controller_interface/cmake/controller_interface_configure_controller_library.cmake new file mode 100644 index 0000000000..b154202329 --- /dev/null +++ b/controller_interface/cmake/controller_interface_configure_controller_library.cmake @@ -0,0 +1,46 @@ +# 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. + +# +# Configures a controller library which implements the controller_interface. +# +# This should be called on any library which is built to implement the rmw API. +# The custom settings are all related to library symbol visibility, see: +# +# https://gcc.gnu.org/wiki/Visibility +# http://www.ibm.com/developerworks/aix/library/au-aix-symbol-visibility/ +# +# Thought about using: +# +# http://www.cmake.org/cmake/help/v2.8.8/cmake.html#module:GenerateExportHeader +# +# But it doesn't seem to set the compiler flags correctly on clang and +# also doesn't work very well when the headers and library are in +# different projects like this. +# +# Effectively, using this macro results in the +# CONTROLLER_INTERFACE_BUILDING_DLL definition +# being set on Windows, and the -fvisibility* flags being passed to gcc and +# clang. +# +# @public +# +macro(controller_interface_configure_controller_library library_target) + if(WIN32) + # Causes the visibility macros to use dllexport rather than dllimport + # which is appropriate when building the dll but not consuming it. + target_compile_definitions(${library_target} + PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") + endif() +endmacro() diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp new file mode 100644 index 0000000000..93c342173f --- /dev/null +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -0,0 +1,72 @@ +// 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_INTERFACE__CONTROLLER_INTERFACE_HPP_ +#define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_HPP_ + +#include +#include + +#include "controller_interface/visibility_control.h" + +#include "hardware_interface/robot_hardware.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace controller_interface +{ + +using controller_interface_ret_t = std::uint8_t; +static constexpr controller_interface_ret_t CONTROLLER_INTERFACE_RET_SUCCESS = 1; +static constexpr controller_interface_ret_t CONTROLLER_INTERFACE_RET_ERROR = 0; + +class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +{ +public: + CONTROLLER_INTERFACE_PUBLIC + ControllerInterface() = default; + + CONTROLLER_INTERFACE_PUBLIC + virtual + ~ControllerInterface() = default; + + CONTROLLER_INTERFACE_PUBLIC + controller_interface_ret_t + virtual + init( + std::weak_ptr robot_hardware, + const std::string & controller_name); + + CONTROLLER_INTERFACE_PUBLIC + std::shared_ptr + get_lifecycle_node(); + + CONTROLLER_INTERFACE_PUBLIC + virtual + controller_interface_ret_t + update() = 0; + +protected: + std::weak_ptr robot_hardware_; + std::shared_ptr lifecycle_node_; + std::shared_ptr parameters_client_; + +private: + std::shared_ptr parameter_client_node_; +}; + +} // namespace controller_interface + +#endif // CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_HPP_ diff --git a/controller_interface/include/controller_interface/visibility_control.h b/controller_interface/include/controller_interface/visibility_control.h new file mode 100644 index 0000000000..244c164799 --- /dev/null +++ b/controller_interface/include/controller_interface/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_INTERFACE__VISIBILITY_CONTROL_H_ +#define CONTROLLER_INTERFACE__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_INTERFACE_EXPORT __attribute__ ((dllexport)) + #define CONTROLLER_INTERFACE_IMPORT __attribute__ ((dllimport)) + #else + #define CONTROLLER_INTERFACE_EXPORT __declspec(dllexport) + #define CONTROLLER_INTERFACE_IMPORT __declspec(dllimport) + #endif + #ifdef CONTROLLER_INTERFACE_BUILDING_DLL + #define CONTROLLER_INTERFACE_PUBLIC CONTROLLER_INTERFACE_EXPORT + #else + #define CONTROLLER_INTERFACE_PUBLIC CONTROLLER_INTERFACE_IMPORT + #endif + #define CONTROLLER_INTERFACE_PUBLIC_TYPE CONTROLLER_INTERFACE_PUBLIC + #define CONTROLLER_INTERFACE_LOCAL +#else + #define CONTROLLER_INTERFACE_EXPORT __attribute__ ((visibility("default"))) + #define CONTROLLER_INTERFACE_IMPORT + #if __GNUC__ >= 4 + #define CONTROLLER_INTERFACE_PUBLIC __attribute__ ((visibility("default"))) + #define CONTROLLER_INTERFACE_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define CONTROLLER_INTERFACE_PUBLIC + #define CONTROLLER_INTERFACE_LOCAL + #endif + #define CONTROLLER_INTERFACE_PUBLIC_TYPE +#endif + +#endif // CONTROLLER_INTERFACE__VISIBILITY_CONTROL_H_ diff --git a/controller_interface/package.xml b/controller_interface/package.xml new file mode 100644 index 0000000000..afcd096374 --- /dev/null +++ b/controller_interface/package.xml @@ -0,0 +1,25 @@ + + + + controller_interface + 0.0.0 + Description of controller_interface + Karsten Knese + Apache License 2.0 + + ament_cmake + + hardware_interface + rclcpp_lifecycle + + hardware_interface + rclcpp_lifecycle + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp new file mode 100644 index 0000000000..ab59c37bd6 --- /dev/null +++ b/controller_interface/src/controller_interface.cpp @@ -0,0 +1,66 @@ +// 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_interface/controller_interface.hpp" + +#include +#include + +namespace controller_interface +{ + +controller_interface_ret_t +ControllerInterface::init( + std::weak_ptr robot_hardware, + const std::string & controller_name) +{ + robot_hardware_ = robot_hardware; + lifecycle_node_ = std::make_shared(controller_name); + + 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)); + + std::string remote_controller_parameter_server = "controller_parameter_server"; + parameters_client_ = std::make_shared( + lifecycle_node_->get_node_base_interface(), + lifecycle_node_->get_node_topics_interface(), + lifecycle_node_->get_node_graph_interface(), + lifecycle_node_->get_node_services_interface(), + remote_controller_parameter_server); + + return CONTROLLER_INTERFACE_RET_SUCCESS; +} + +std::shared_ptr +ControllerInterface::get_lifecycle_node() +{ + return lifecycle_node_; +} + +} // namespace controller_interface