Gpio command controller (#1251)

---------

Co-authored-by: m.bednarczyk <[email protected]>
Co-authored-by: Maciej Bednarczyk <[email protected]>
Co-authored-by: Bence Magyar <[email protected]>
Co-authored-by: Christoph Fröhlich <[email protected]>
Co-authored-by: Christoph Froehlich <[email protected]>
Co-authored-by: Sai Kishor Kothakota <[email protected]>

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

doc/release_notes.rst

@@ -63,3 +63,7 @@ steering_controllers_library
 tricycle_controller
 ************************
 * tricycle_controller now uses generate_parameter_library (`#957 <https://github.com/ros-controls/ros2_controllers/pull/957>`_).
+
+gpio_controllers
+************************
+* The GPIO command controller was added 🎉 (`#1251 <https://github.com/ros-controls/ros2_controllers/pull/1251>`_).

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()

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 (``<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
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+- ``/<controller_name>/gpio_states`` [``control_msgs/msg/DynamicJointState``]: Publishes all state interfaces of the given GPIO interfaces.
+- ``/<controller_name>/commands`` [``control_msgs/msg/DynamicJointState``]:  A subscriber for configured command interfaces.
+
+
+Parameters
+^^^^^^^^^^^^^^^^^^^^^^^^
+
+This controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters. The parameter `definition file located in the src folder <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/gpio_controllers/src/gpio_command_controller_parameters.yaml>`_ 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.

gpio_controllers/gpio_controllers_plugin.xml

@@ -0,0 +1,7 @@
+<library path="gpio_controllers">
+  <class name="gpio_controllers/GpioCommandController" type="gpio_controllers::GpioCommandController" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    The gpio command controller commands a group of gpios using multiple interfaces.
+  </description>
+  </class>
+</library>

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 <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#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<std::string>;
+using MapOfReferencesToCommandInterfaces = std::unordered_map<
+  std::string, std::reference_wrapper<hardware_interface::LoanedCommandInterface>>;
+using MapOfReferencesToStateInterfaces =
+  std::unordered_map<std::string, std::reference_wrapper<hardware_interface::LoanedStateInterface>>;
+using StateInterfaces =
+  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>;
+
+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 <typename T>
+  std::unordered_map<std::string, std::reference_wrapper<T>> create_map_of_references_to_interfaces(
+    const InterfacesNames & interfaces_from_params, std::vector<T> & configured_interfaces);
+  template <typename T>
+  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<hardware_interface::ComponentInfo> 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<std::shared_ptr<CmdType>> rt_command_ptr_{};
+  rclcpp::Subscription<CmdType>::SharedPtr gpios_command_subscriber_{};
+
+  std::shared_ptr<rclcpp::Publisher<StateType>> gpio_state_publisher_{};
+  std::shared_ptr<realtime_tools::RealtimePublisher<StateType>> realtime_gpio_state_publisher_{};
+
+  std::shared_ptr<gpio_command_controller_parameters::ParamListener> param_listener_{};
+  gpio_command_controller_parameters::Params params_;
+};
+
+}  // namespace gpio_controllers
+
+#endif  // GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_

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_

gpio_controllers/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gpio_controllers</name>
+  <version>4.15.0</version>
+  <description>Controllers to interact with gpios.</description>
+  <maintainer email="[email protected]">Maciej Bednarczyk</maintainer>
+  <maintainer email="[email protected]">Wiktor Bajor</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>control_msgs</depend>
+  <depend>realtime_tools</depend>
+  <depend>generate_parameter_library</depend>
+
+  <build_depend>pluginlib</build_depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>