Gpio command controller

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

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

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

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,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 <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#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<std::string>;
+using MapOfReferencesToCommandInterfaces = std::unordered_map<
+  std::string, std::reference_wrapper<hardware_interface::LoanedCommandInterface>>;
+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_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 <typename T>
+  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<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,29 @@
+<?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.12.1</version>
+  <description>Controllers to interact with gpios.</description>
+  <maintainer email="macbednarczyk@gmail.com">Maciej Bednarczyk</maintainer>
+  <maintainer email="wiktorbajor1@gmail.com">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>realtime_tools</depend>
+  <depend>std_msgs</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>