From 0590c6ac2f916b13ce5842913b8811a32fd4ffab Mon Sep 17 00:00:00 2001 From: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Date: Mon, 18 Nov 2024 15:07:19 +0100 Subject: [PATCH] Gpio command controller (#1251) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Co-authored-by: m.bednarczyk Co-authored-by: Maciej Bednarczyk Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich Co-authored-by: Christoph Froehlich Co-authored-by: Sai Kishor Kothakota --- doc/controllers_index.rst | 1 + doc/release_notes.rst | 4 + gpio_controllers/CMakeLists.txt | 115 +++ gpio_controllers/doc/userdoc.rst | 57 ++ gpio_controllers/gpio_controllers_plugin.xml | 7 + .../gpio_command_controller.hpp | 114 +++ .../gpio_controllers/visibility_control.h | 49 ++ gpio_controllers/package.xml | 30 + .../src/gpio_command_controller.cpp | 416 +++++++++++ .../gpio_command_controller_parameters.yaml | 34 + .../test/test_gpio_command_controller.cpp | 693 ++++++++++++++++++ .../test_load_gpio_command_controller.cpp | 37 + ros2_controllers/package.xml | 1 + 13 files changed, 1558 insertions(+) create mode 100644 gpio_controllers/CMakeLists.txt create mode 100644 gpio_controllers/doc/userdoc.rst create mode 100644 gpio_controllers/gpio_controllers_plugin.xml create mode 100644 gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp create mode 100644 gpio_controllers/include/gpio_controllers/visibility_control.h create mode 100644 gpio_controllers/package.xml create mode 100644 gpio_controllers/src/gpio_command_controller.cpp create mode 100644 gpio_controllers/src/gpio_command_controller_parameters.yaml create mode 100644 gpio_controllers/test/test_gpio_command_controller.cpp create mode 100644 gpio_controllers/test/test_load_gpio_command_controller.cpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index d5b956aa8a..0a9c6ec485 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -56,6 +56,7 @@ The controllers are using `common hardware interface definitions`_, and may use PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> + Gpio Command Controller <../gpio_controllers/doc/userdoc.rst> Broadcasters diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 688c35724d..b1db026df2 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -63,3 +63,7 @@ steering_controllers_library tricycle_controller ************************ * tricycle_controller now uses generate_parameter_library (`#957 `_). + +gpio_controllers +************************ +* The GPIO command controller was added 🎉 (`#1251 `_). diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt new file mode 100644 index 0000000000..6ea97e04ca --- /dev/null +++ b/gpio_controllers/CMakeLists.txt @@ -0,0 +1,115 @@ +cmake_minimum_required(VERSION 3.8) +project(gpio_controllers) + +# 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 -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(control_msgs REQUIRED) + + +generate_parameter_library(gpio_command_controller_parameters + src/gpio_command_controller_parameters.yaml +) + +add_library(gpio_controllers + SHARED + src/gpio_command_controller.cpp +) +target_include_directories(gpio_controllers PRIVATE include) +target_link_libraries(gpio_controllers PUBLIC gpio_command_controller_parameters) +ament_target_dependencies(gpio_controllers PUBLIC + builtin_interfaces + controller_interface + hardware_interface + pluginlib + rclcpp_lifecycle + rcutils + realtime_tools + control_msgs +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(gpio_controllers PRIVATE "GPIO_COMMAND_CONTROLLER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface gpio_controllers_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + gpio_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock( + test_load_gpio_command_controller + test/test_load_gpio_command_controller.cpp + ) + + target_include_directories(test_load_gpio_command_controller PRIVATE include) + ament_target_dependencies(test_load_gpio_command_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock( + test_gpio_command_controller + test/test_gpio_command_controller.cpp + ) + target_include_directories(test_gpio_command_controller PRIVATE include) + target_link_libraries(test_gpio_command_controller + gpio_controllers + ) + ament_target_dependencies(test_gpio_command_controller + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + ros2_control_test_assets + control_msgs + ) +endif() + +ament_export_dependencies( + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools +) +ament_export_include_directories( + include +) +ament_export_libraries( + gpio_controllers +) +ament_package() diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst new file mode 100644 index 0000000000..199a4bd208 --- /dev/null +++ b/gpio_controllers/doc/userdoc.rst @@ -0,0 +1,57 @@ +.. _gpio_controllers_userdoc: + +gpio_controllers +===================== + +This is a collection of controllers for hardware interfaces of type GPIO (```` tag in the URDF). + +gpio_command_controller +----------------------------- +gpio_command_controller let the user expose command interfaces of given GPIO interfaces and publishes state interfaces of the configured command interfaces. + +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +- ``//gpio_states`` [``control_msgs/msg/DynamicJointState``]: Publishes all state interfaces of the given GPIO interfaces. +- ``//commands`` [``control_msgs/msg/DynamicJointState``]: A subscriber for configured command interfaces. + + +Parameters +^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +.. generate_parameter_library_details:: + ../src/gpio_command_controller_parameters.yaml + +The controller expects at least one GPIO interface and the corresponding command interface names or state interface. However, these Command and State interfaces are optional. The controller behaves as a broadcaster when no Command Interface is present, thereby publishing the configured GPIO state interfaces if set, else the one present in the URDF. + +.. note:: + + When no state interface is provided in the param file, the controller will try to use state_interfaces from ros2_control's config placed in the URDF for configured gpio interfaces. + However, command interfaces will not be configured based on the available URDF setup. + +.. code-block:: yaml + + gpio_command_controller: + ros__parameters: + type: gpio_controllers/GpioCommandController + gpios: + - Gpio1 + - Gpio2 + command_interfaces: + Gpio1: + - interfaces: + - dig.1 + - dig.2 + - dig.3 + Gpio2: + - interfaces: + - ana.1 + - ana.2 + state_interfaces: + Gpio2: + - interfaces: + - ana.1 + - ana.2 + +With the above-defined controller configuration, the controller will accept commands for all gpios' interfaces and will only publish the state of Gpio2. diff --git a/gpio_controllers/gpio_controllers_plugin.xml b/gpio_controllers/gpio_controllers_plugin.xml new file mode 100644 index 0000000000..03fd3e1ee0 --- /dev/null +++ b/gpio_controllers/gpio_controllers_plugin.xml @@ -0,0 +1,7 @@ + + + + The gpio command controller commands a group of gpios using multiple interfaces. + + + diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp new file mode 100644 index 0000000000..5149c74a1c --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -0,0 +1,114 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ +#define GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "control_msgs/msg/dynamic_interface_group_values.hpp" +#include "controller_interface/controller_interface.hpp" +#include "gpio_command_controller_parameters.hpp" +#include "gpio_controllers/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +namespace gpio_controllers +{ +using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; +using StateType = control_msgs::msg::DynamicInterfaceGroupValues; +using CallbackReturn = controller_interface::CallbackReturn; +using InterfacesNames = std::vector; +using MapOfReferencesToCommandInterfaces = std::unordered_map< + std::string, std::reference_wrapper>; +using MapOfReferencesToStateInterfaces = + std::unordered_map>; +using StateInterfaces = + std::vector>; + +class GpioCommandController : public controller_interface::ControllerInterface +{ +public: + GPIO_COMMAND_CONTROLLER_PUBLIC + GpioCommandController(); + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + void store_command_interface_types(); + void store_state_interface_types(); + void initialize_gpio_state_msg(); + void update_gpios_states(); + controller_interface::return_type update_gpios_commands(); + template + std::unordered_map> create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, std::vector & configured_interfaces); + template + bool check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, const T & configured_interfaces); + void apply_state_value( + StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const; + void apply_command( + const CmdType & gpio_commands, std::size_t gpio_index, + std::size_t command_interface_index) const; + bool should_broadcast_all_interfaces_of_configured_gpios() const; + void set_all_state_interfaces_of_configured_gpios(); + InterfacesNames get_gpios_state_interfaces_names(const std::string & gpio_name) const; + bool update_dynamic_map_parameters(); + std::vector get_gpios_from_urdf() const; + +protected: + InterfacesNames command_interface_types_; + InterfacesNames state_interface_types_; + MapOfReferencesToCommandInterfaces command_interfaces_map_; + MapOfReferencesToStateInterfaces state_interfaces_map_; + + realtime_tools::RealtimeBuffer> rt_command_ptr_{}; + rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; + + std::shared_ptr> gpio_state_publisher_{}; + std::shared_ptr> realtime_gpio_state_publisher_{}; + + std::shared_ptr param_listener_{}; + gpio_command_controller_parameters::Params params_; +}; + +} // namespace gpio_controllers + +#endif // GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ diff --git a/gpio_controllers/include/gpio_controllers/visibility_control.h b/gpio_controllers/include/gpio_controllers/visibility_control.h new file mode 100644 index 0000000000..a735a1621c --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ +#define GPIO_CONTROLLERS__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 GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GPIO_COMMAND_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define GPIO_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) +#define GPIO_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GPIO_COMMAND_CONTROLLER_BUILDING_LIBRARY +#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_EXPORT +#else +#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_IMPORT +#endif +#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE GPIO_COMMAND_CONTROLLER_PUBLIC +#define GPIO_COMMAND_CONTROLLER_LOCAL +#else +#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GPIO_COMMAND_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GPIO_COMMAND_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GPIO_COMMAND_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GPIO_COMMAND_CONTROLLER_PUBLIC +#define GPIO_COMMAND_CONTROLLER_LOCAL +#endif +#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml new file mode 100644 index 0000000000..9b98746545 --- /dev/null +++ b/gpio_controllers/package.xml @@ -0,0 +1,30 @@ + + + + gpio_controllers + 4.15.0 + Controllers to interact with gpios. + Maciej Bednarczyk + Wiktor Bajor + Apache License 2.0 + + ament_cmake + + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + control_msgs + realtime_tools + generate_parameter_library + + pluginlib + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp new file mode 100644 index 0000000000..a509eae774 --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -0,0 +1,416 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 "gpio_controllers/gpio_command_controller.hpp" + +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/component_parser.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" + +namespace +{ +template +void print_interface(const rclcpp::Logger & logger, const T & command_interfaces) +{ + for (const auto & [interface_name, value] : command_interfaces) + { + RCLCPP_ERROR(logger, "Got %s", interface_name.c_str()); + } +} + +std::vector extract_gpios_from_hardware_info( + const std::vector & hardware_infos) +{ + std::vector result; + for (const auto & hardware_info : hardware_infos) + { + std::copy( + hardware_info.gpios.begin(), hardware_info.gpios.end(), std::back_insert_iterator(result)); + } + return result; +} +} // namespace + +namespace gpio_controllers +{ + +GpioCommandController::GpioCommandController() : controller_interface::ControllerInterface() {} + +CallbackReturn GpioCommandController::on_init() +try +{ + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + return CallbackReturn::SUCCESS; +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} + +CallbackReturn GpioCommandController::on_configure(const rclcpp_lifecycle::State &) +try +{ + if (!update_dynamic_map_parameters()) + { + return controller_interface::CallbackReturn::ERROR; + } + + store_command_interface_types(); + store_state_interface_types(); + + if (command_interface_types_.empty() && state_interface_types_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "No command or state interfaces are configured"); + return CallbackReturn::ERROR; + } + + if (!command_interface_types_.empty()) + { + gpios_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + } + + gpio_state_publisher_ = + get_node()->create_publisher("~/gpio_states", rclcpp::SystemDefaultsQoS()); + + realtime_gpio_state_publisher_ = + std::make_shared>(gpio_state_publisher_); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} + +controller_interface::InterfaceConfiguration +GpioCommandController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = command_interface_types_; + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration GpioCommandController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = state_interface_types_; + + return state_interfaces_config; +} + +CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State &) +{ + command_interfaces_map_ = + create_map_of_references_to_interfaces(command_interface_types_, command_interfaces_); + state_interfaces_map_ = + create_map_of_references_to_interfaces(state_interface_types_, state_interfaces_); + if ( + !check_if_configured_interfaces_matches_received( + command_interface_types_, command_interfaces_map_) || + !check_if_configured_interfaces_matches_received(state_interface_types_, state_interfaces_map_)) + { + return CallbackReturn::ERROR; + } + + initialize_gpio_state_msg(); + rt_command_ptr_.reset(); + RCLCPP_INFO(get_node()->get_logger(), "activate successful"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) +{ + rt_command_ptr_.reset(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type GpioCommandController::update( + const rclcpp::Time &, const rclcpp::Duration &) +{ + update_gpios_states(); + return update_gpios_commands(); +} + +bool GpioCommandController::update_dynamic_map_parameters() +{ + auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(logger, "Error encountered during init"); + return false; + } + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + return true; +} + +void GpioCommandController::store_command_interface_types() +{ + for (const auto & [gpio_name, interfaces] : params_.command_interfaces.gpios_map) + { + std::transform( + interfaces.interfaces.cbegin(), interfaces.interfaces.cend(), + std::back_inserter(command_interface_types_), + [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} + +bool GpioCommandController::should_broadcast_all_interfaces_of_configured_gpios() const +{ + auto are_interfaces_empty = [](const auto & interfaces) + { return interfaces.second.interfaces.empty(); }; + return std::all_of( + params_.state_interfaces.gpios_map.cbegin(), params_.state_interfaces.gpios_map.cend(), + are_interfaces_empty); +} + +std::vector GpioCommandController::get_gpios_from_urdf() const +try +{ + return extract_gpios_from_hardware_info( + hardware_interface::parse_control_resources_from_urdf(get_robot_description())); +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during extracting gpios info from urdf %s \n", e.what()); + return {}; +} + +void GpioCommandController::set_all_state_interfaces_of_configured_gpios() +{ + const auto gpios{get_gpios_from_urdf()}; + for (const auto & gpio_name : params_.gpios) + { + for (const auto & gpio : gpios) + { + if (gpio_name == gpio.name) + { + std::transform( + gpio.state_interfaces.begin(), gpio.state_interfaces.end(), + std::back_insert_iterator(state_interface_types_), + [&gpio_name](const auto & interface_name) + { return gpio_name + '/' + interface_name.name; }); + } + } + } +} + +void GpioCommandController::store_state_interface_types() +{ + if (should_broadcast_all_interfaces_of_configured_gpios()) + { + RCLCPP_INFO( + get_node()->get_logger(), + "State interfaces are not configured. All available interfaces of configured GPIOs will be " + "broadcasted."); + set_all_state_interfaces_of_configured_gpios(); + return; + } + + for (const auto & [gpio_name, interfaces] : params_.state_interfaces.gpios_map) + { + std::transform( + interfaces.interfaces.cbegin(), interfaces.interfaces.cend(), + std::back_inserter(state_interface_types_), + [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} + +void GpioCommandController::initialize_gpio_state_msg() +{ + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + gpio_state_msg.header.stamp = get_node()->now(); + gpio_state_msg.interface_groups.resize(params_.gpios.size()); + gpio_state_msg.interface_values.resize(params_.gpios.size()); + + for (std::size_t gpio_index = 0; gpio_index < params_.gpios.size(); ++gpio_index) + { + const auto gpio_name = params_.gpios[gpio_index]; + gpio_state_msg.interface_groups[gpio_index] = gpio_name; + gpio_state_msg.interface_values[gpio_index].interface_names = + get_gpios_state_interfaces_names(gpio_name); + gpio_state_msg.interface_values[gpio_index].values = std::vector( + gpio_state_msg.interface_values[gpio_index].interface_names.size(), + std::numeric_limits::quiet_NaN()); + } +} + +InterfacesNames GpioCommandController::get_gpios_state_interfaces_names( + const std::string & gpio_name) const +{ + InterfacesNames result; + for (const auto & interface_name : state_interface_types_) + { + const auto it = state_interfaces_map_.find(interface_name); + if (it != state_interfaces_map_.cend() && it->second.get().get_prefix_name() == gpio_name) + { + result.emplace_back(it->second.get().get_interface_name()); + } + } + return result; +} + +template +std::unordered_map> +GpioCommandController::create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, std::vector & configured_interfaces) +{ + std::unordered_map> map; + for (const auto & interface_name : interfaces_from_params) + { + auto interface = std::find_if( + configured_interfaces.begin(), configured_interfaces.end(), + [&](const auto & configured_interface) + { + const auto full_name_interface_name = configured_interface.get_name(); + return full_name_interface_name == interface_name; + }); + if (interface != configured_interfaces.end()) + { + map.emplace(interface_name, std::ref(*interface)); + } + } + return map; +} + +template +bool GpioCommandController::check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, const T & configured_interfaces) +{ + if (!(configured_interfaces.size() == interfaces_from_params.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %ld interfaces, got %ld", interfaces_from_params.size(), + configured_interfaces.size()); + for (const auto & interface : interfaces_from_params) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected %s", interface.c_str()); + } + print_interface(get_node()->get_logger(), configured_interfaces); + return false; + } + return true; +} + +controller_interface::return_type GpioCommandController::update_gpios_commands() +{ + auto gpio_commands_ptr = rt_command_ptr_.readFromRT(); + if (!gpio_commands_ptr || !(*gpio_commands_ptr)) + { + return controller_interface::return_type::OK; + } + + const auto gpio_commands = *(*gpio_commands_ptr); + for (std::size_t gpio_index = 0; gpio_index < gpio_commands.interface_groups.size(); ++gpio_index) + { + const auto & gpio_name = gpio_commands.interface_groups[gpio_index]; + if ( + gpio_commands.interface_values[gpio_index].values.size() != + gpio_commands.interface_values[gpio_index].interface_names.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "For gpio %s interfaces_names do not match values", + gpio_name.c_str()); + return controller_interface::return_type::ERROR; + } + for (std::size_t command_interface_index = 0; + command_interface_index < gpio_commands.interface_values[gpio_index].values.size(); + ++command_interface_index) + { + apply_command(gpio_commands, gpio_index, command_interface_index); + } + } + return controller_interface::return_type::OK; +} + +void GpioCommandController::apply_command( + const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index) const +{ + const auto full_command_interface_name = + gpio_commands.interface_groups[gpio_index] + '/' + + gpio_commands.interface_values[gpio_index].interface_names[command_interface_index]; + try + { + command_interfaces_map_.at(full_command_interface_name) + .get() + .set_value(gpio_commands.interface_values[gpio_index].values[command_interface_index]); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during applying command stage of %s with message: %s \n", + full_command_interface_name.c_str(), e.what()); + } +} + +void GpioCommandController::update_gpios_states() +{ + if (!realtime_gpio_state_publisher_ || !realtime_gpio_state_publisher_->trylock()) + { + return; + } + + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + gpio_state_msg.header.stamp = get_node()->now(); + for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.interface_groups.size(); + ++gpio_index) + { + for (std::size_t interface_index = 0; + interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); + ++interface_index) + { + apply_state_value(gpio_state_msg, gpio_index, interface_index); + } + } + realtime_gpio_state_publisher_->unlockAndPublish(); +} + +void GpioCommandController::apply_state_value( + StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const +{ + const auto interface_name = + state_msg.interface_groups[gpio_index] + '/' + + state_msg.interface_values[gpio_index].interface_names[interface_index]; + try + { + state_msg.interface_values[gpio_index].values[interface_index] = + state_interfaces_map_.at(interface_name).get().get_value(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during reading state of: %s \n", interface_name.c_str()); + } +} + +} // namespace gpio_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + gpio_controllers::GpioCommandController, controller_interface::ControllerInterface) diff --git a/gpio_controllers/src/gpio_command_controller_parameters.yaml b/gpio_controllers/src/gpio_command_controller_parameters.yaml new file mode 100644 index 0000000000..0c6aade35f --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller_parameters.yaml @@ -0,0 +1,34 @@ +gpio_command_controller_parameters: + gpios: { + type: string_array, + description: "List of gpios", + read_only: true, + validation: { + size_gt<>: [0], + unique<>: null + } + } + + command_interfaces: + __map_gpios: + interfaces: { + type: string_array, + description: "List of command interfaces for each gpio.", + read_only: true, + default_value: [], + validation: { + unique<>: null + } + } + + state_interfaces: + __map_gpios: + interfaces: { + type: string_array, + description: "List of state interfaces for each gpio. If empty all available gpios' states are used.", + read_only: true, + default_value: [], + validation: { + unique<>: null + } + } diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp new file mode 100644 index 0000000000..ec0e0f1228 --- /dev/null +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -0,0 +1,693 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 "control_msgs/msg/dynamic_interface_group_values.hpp" +#include "gmock/gmock.h" +#include "gpio_controllers/gpio_command_controller.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/wait_result.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +namespace +{ +const auto hardware_resources_with_gpio = + R"( + + + + + +)"; + +const auto minimal_robot_urdf_with_gpio = std::string(ros2_control_test_assets::urdf_head) + + std::string(hardware_resources_with_gpio) + + std::string(ros2_control_test_assets::urdf_tail); +} // namespace + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; +using StateType = control_msgs::msg::DynamicInterfaceGroupValues; +using hardware_interface::CommandInterface; +using hardware_interface::StateInterface; + +namespace +{ +rclcpp::NodeOptions create_node_options_with_overriden_parameters( + std::vector parameters) +{ + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + return node_options; +} +} // namespace + +class FriendGpioCommandController : public gpio_controllers::GpioCommandController +{ + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandWithOnlyOneGpioThenInterfacesValuesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandInterfacesInDifferentOrderThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated); +}; + +class GpioCommandControllerTestSuite : public ::testing::Test +{ +public: + GpioCommandControllerTestSuite() + { + rclcpp::init(0, nullptr); + node = std::make_unique("node"); + } + ~GpioCommandControllerTestSuite() { rclcpp::shutdown(); } + void setup_command_and_state_interfaces() + { + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + } + + void move_to_activate_state(controller_interface::return_type result_of_initialization) + { + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + setup_command_and_state_interfaces(); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + } + + void stop_test_when_message_cannot_be_published(int max_sub_check_loop_count) + { + ASSERT_GE(max_sub_check_loop_count, 0) + << "Test was unable to publish a message through controller/broadcaster update loop"; + } + + void assert_default_command_and_state_values() + { + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); + ASSERT_EQ(gpio_1_1_dig_state.get_value(), gpio_states.at(0)); + ASSERT_EQ(gpio_1_2_dig_state.get_value(), gpio_states.at(1)); + ASSERT_EQ(gpio_2_ana_state.get_value(), gpio_states.at(2)); + } + + void update_controller_loop() + { + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + } + + CmdType createGpioCommand( + const std::vector & gpios_names, + const std::vector & interface_values) + { + CmdType command; + command.interface_groups = gpios_names; + command.interface_values = interface_values; + return command; + } + + control_msgs::msg::InterfaceValue createInterfaceValue( + const std::vector & interfaces_names, + const std::vector & interfaces_values) + { + control_msgs::msg::InterfaceValue output; + output.interface_names = interfaces_names; + output.values = interfaces_values; + return output; + } + + void wait_one_miliseconds_for_timeout() + { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller.get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = controller.get_node()->get_clock()->now() + timeout; + while (controller.get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + int wait_for_subscription( + std::shared_ptr subscription, int max_sub_check_loop_count = 5) + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + return max_sub_check_loop_count; + } + + FriendGpioCommandController controller; + + const std::vector gpio_names{"gpio1", "gpio2"}; + std::vector gpio_commands{1.0, 0.0, 3.1}; + std::vector gpio_states{1.0, 0.0, 3.1}; + + CommandInterface gpio_1_1_dig_cmd{gpio_names.at(0), "dig.1", &gpio_commands.at(0)}; + CommandInterface gpio_1_2_dig_cmd{gpio_names.at(0), "dig.2", &gpio_commands.at(1)}; + CommandInterface gpio_2_ana_cmd{gpio_names.at(1), "ana.1", &gpio_commands.at(2)}; + + StateInterface gpio_1_1_dig_state{gpio_names.at(0), "dig.1", &gpio_states.at(0)}; + StateInterface gpio_1_2_dig_state{gpio_names.at(0), "dig.2", &gpio_states.at(1)}; + StateInterface gpio_2_ana_state{gpio_names.at(1), "ana.1", &gpio_states.at(2)}; + std::unique_ptr node; +}; + +TEST_F(GpioCommandControllerTestSuite, WhenNoParametersAreSetInitShouldFail) +{ + const auto result = controller.init( + "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", + controller.define_custom_node_options()); + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, WhenGpiosParameterIsEmptyInitShouldFail) +{ + const auto node_options = + create_node_options_with_overriden_parameters({{"gpios", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsEmptyInitShouldNotFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsNotSetInitShouldNotFail) +{ + const auto node_options = + create_node_options_with_overriden_parameters({{"gpios", std::vector{"gpio1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGpiosAreSetAndInterfacesAreSetForAllGpiosThenInitShouldSuccess) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandAndStateInterfacesAreEmptyAndNoInterfacesAreSetInUrdfConfigurationShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init( + "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", + node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredButAreSetInUrdfForConfiguredGpiosThenConfigureShouldSucceed) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init( + "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredAndProvidedUrdfIsEmptyThenConfigureShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + setup_command_and_state_interfaces(); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedCommandInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedStateInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandInterfacesDontMatchStatesButBothMatchAssignedOnesThenOnActivationShouldSucceed) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenThereWasNoCommandForGpiosThenCommandInterfacesShouldHaveDefaultValues) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + assert_default_command_and_state_values(); + update_controller_loop(); + assert_default_command_and_state_values(); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, + {createInterfaceValue({"dig.1", "dig.2"}, {0.0}), createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandInterfacesInDifferentOrderThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio2", "gpio1"}, {createInterfaceValue({"ana.1"}, {30.0}), + createInterfaceValue({"dig.2", "dig.1"}, {1.0, 0.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithOnlyOneGpioThenInterfacesValuesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = + createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); + + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands[2]); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio3"}, {createInterfaceValue({"dig.3", "dig.4"}, {20.0, 25.0}), + createInterfaceValue({"ana.1"}, {21.0})}); + + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesViaTopicThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + auto command_pub = node->create_publisher( + std::string(controller.get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + command_pub->publish(command); + wait_one_miliseconds_for_timeout(); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurrentValues) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.at(1), "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(1), "dig.2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).interface_names.at(0), "ana.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(1), 0.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredButSetInUrdfForConfiguredGpiosThenThatStatesShouldBePublished) +{ + const std::vector single_gpio{"gpio1"}; + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", single_gpio}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1"}}}); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + const auto result_of_initialization = controller.init( + "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), single_gpio.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + ControllerShouldPublishGpioStatesWithCurrentValuesWhenOnlyStateInterfacesAreSet) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + const auto result_of_initialization = + controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller.assign_interfaces({}, std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.at(1), "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).interface_names.at(0), "ana.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); +} diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp new file mode 100644 index 0000000000..379a53b048 --- /dev/null +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -0,0 +1,37 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadGpioCommandController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller("test_gpio_command_controller", "gpio_controllers/GpioCommandController")); + + rclcpp::shutdown(); +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ccde6cf812..4969658788 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -24,6 +24,7 @@ effort_controllers force_torque_sensor_broadcaster forward_command_controller + gpio_controllers imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller