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();
+}