diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 683f23c202..d6a0d625b1 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/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt new file mode 100644 index 0000000000..b96b6f96e2 --- /dev/null +++ b/gpio_controllers/CMakeLists.txt @@ -0,0 +1,113 @@ +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) +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(std_msgs REQUIRED) +find_package(generate_parameter_library 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 + std_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 + std_msgs + ) +endif() + +ament_export_dependencies( + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs +) +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..442132b977 --- /dev/null +++ b/gpio_controllers/doc/userdoc.rst @@ -0,0 +1,41 @@ +.. _gpio_controllers_userdoc: + +gpio_controllers +===================== + +This is a collection of controllers for gpios that work with multiple interfaces. + +Hardware interface type +----------------------- + +These controllers work with gpios using user defined command interfaces. + +Using GPIO Command Controller +----------------------------- +The controller expects at least one gpio interface abd the corresponding command interface names. +A yaml file for using it could be: +.. code-block:: yaml + + controller_manager: + ros__parameters: + update_rate: 100 # Hz + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + gpio_command_controller: + type: gpio_controllers/GpioCommandController + + gpio_command_controller: + ros__parameters: + gpios: + - Gpio1 + - Gpio2 + command_interfaces: + Gpio1: + - ports: + - dig.1 + - dig.2 + - dig.3 + Gpio2: + -ports: + - ana.1 + - ana.2 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..975680a663 --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -0,0 +1,99 @@ +// 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_joint_state.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::DynamicJointState; +using StateType = control_msgs::msg::DynamicJointState; +using CallbackReturn = controller_interface::CallbackReturn; +using InterfacesNames = std::vector; +using MapOfReferencesToCommandInterfaces = std::unordered_map< + std::string, std::reference_wrapper>; +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_interface_types(); + void initialize_gpio_state_msg(); + void update_gpios_states(); + controller_interface::return_type update_gpios_commands(); + MapOfReferencesToCommandInterfaces create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params); + template + bool check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, const T & interfaces_map); + +protected: + InterfacesNames interface_types_; + MapOfReferencesToCommandInterfaces command_interfaces_map_; + StateInterfaces ordered_state_interfaces_; + + 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..4746c98c46 --- /dev/null +++ b/gpio_controllers/package.xml @@ -0,0 +1,29 @@ + + + + gpio_controllers + 4.12.1 + Controllers to interact with gpios. + Maciej Bednarczyk + Wiktor Bajor + Apache License 2.0 + + ament_cmake + + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs + + 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..0bda77e84d --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -0,0 +1,281 @@ +// 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 "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" + +namespace +{ +void print_interface( + const rclcpp::Logger & logger, const gpio_controllers::StateInterfaces & state_interfaces) +{ + for (const auto & interface : state_interfaces) + { + RCLCPP_ERROR(logger, "Got %s", interface.get().get_name().c_str()); + } +} +void print_interface( + const rclcpp::Logger & logger, + const gpio_controllers::MapOfReferencesToCommandInterfaces & command_interfaces) +{ + for (const auto & [interface_name, value] : command_interfaces) + { + RCLCPP_ERROR(logger, "Got %s", interface_name.c_str()); + } +} +} // 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 +{ + store_interface_types(); + 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 = 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 = interface_types_; + + return state_interfaces_config; +} + +CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State &) +{ + command_interfaces_map_ = create_map_of_references_to_interfaces(interface_types_); + controller_interface::get_ordered_interfaces( + state_interfaces_, interface_types_, "", ordered_state_interfaces_); + + if ( + !check_if_configured_interfaces_matches_received(interface_types_, command_interfaces_map_) || + !check_if_configured_interfaces_matches_received(interface_types_, ordered_state_interfaces_)) + { + 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(); +} + +void GpioCommandController::store_interface_types() +{ + for (const auto & [gpio_name, ports] : params_.command_interfaces.gpios_map) + { + std::transform( + ports.ports.begin(), ports.ports.end(), std::back_inserter(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.joint_names.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.joint_names[gpio_index] = gpio_name; + std::copy( + params_.command_interfaces.gpios_map[gpio_name].ports.begin(), + params_.command_interfaces.gpios_map[gpio_name].ports.end(), + std::back_insert_iterator(gpio_state_msg.interface_values[gpio_index].interface_names)); + gpio_state_msg.interface_values[gpio_index].values = std::vector( + params_.command_interfaces.gpios_map[gpio_name].ports.size(), + std::numeric_limits::quiet_NaN()); + } +} + +MapOfReferencesToCommandInterfaces GpioCommandController::create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params) +{ + MapOfReferencesToCommandInterfaces map; + for (const auto & interface_name : interfaces_from_params) + { + auto interface = std::find_if( + command_interfaces_.begin(), command_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 != command_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.joint_names.size(); ++gpio_index) + { + const auto & gpio_name = gpio_commands.joint_names[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) + { + const auto & full_command_interface_name = + gpio_name + '/' + + 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 with message: %s \n", e.what()); + } + } + } + return controller_interface::return_type::OK; +} + +void GpioCommandController::update_gpios_states() +{ + if (realtime_gpio_state_publisher_ && realtime_gpio_state_publisher_->trylock()) + { + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + gpio_state_msg.header.stamp = get_node()->now(); + + std::size_t state_index{}; + for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.joint_names.size(); ++gpio_index) + { + for (std::size_t interface_index = 0; + interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); + ++interface_index) + { + gpio_state_msg.interface_values[gpio_index].values[interface_index] = + ordered_state_interfaces_[state_index].get().get_value(); + ++state_index; + } + } + realtime_gpio_state_publisher_->unlockAndPublish(); + } +} + +} // 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..e6fe0c4ebd --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller_parameters.yaml @@ -0,0 +1,20 @@ +gpio_command_controller_parameters: + gpios: { + type: string_array, + description: "List of gpios", + validation: { + size_gt<>: [0], + unique<>: null + } + } + + command_interfaces: + __map_gpios: + ports: { + type: string_array, + description: "List of ports: for each gpio", + validation: { + size_gt<>: [0], + 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..04b360c3fb --- /dev/null +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -0,0 +1,514 @@ +// 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 "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" + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using CmdType = control_msgs::msg::DynamicJointState; +using StateType = control_msgs::msg::DynamicJointState; +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, + WhenGivenCommandPortsInDifferentOrderThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandContainsWrongGpioPortsOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated); +}; + +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); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + } + + void move_to_activate_state() + { + ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller.get_node()->activate().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + + 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.joint_names = 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", "", 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, WhenPortsParameterForGpioIsEmptyInitShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.ports", 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, WhenPortsParameterForGpioIsNotSetInitShouldFail) +{ + 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::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, WhenGpiosAreSetAndPortsAreSetForAllGpiosThenInitShouldSuccess) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", 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, ConfigureAndActivateParamsSuccess) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + setup_command_and_state_interfaces(); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + 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.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + 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(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedSateInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + 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(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenThereWasNoCommandForGpiosThenCommandInterfacesShouldHaveDefaultValues) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + + setup_command_and_state_interfaces(); + 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.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + + setup_command_and_state_interfaces(); + 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.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + + setup_command_and_state_interfaces(); + 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.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + + setup_command_and_state_interfaces(); + 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, + WhenGivenCommandPortsInDifferentOrderThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + + setup_command_and_state_interfaces(); + 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.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + setup_command_and_state_interfaces(); + move_to_activate_state(result); + + 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, + WhenGivenCommandContainsWrongGpioPortsOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + setup_command_and_state_interfaces(); + move_to_activate_state(result); + + 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.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + setup_command_and_state_interfaces(); + 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.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + setup_command_and_state_interfaces(); + 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.joint_names.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.joint_names.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.joint_names.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); +} 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(); +}