|
| 1 | +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 |
| 2 | +From: Esteve Fernandez <esteve@apache.org> |
| 3 | +Date: Tue, 12 May 2026 14:02:58 +0100 |
| 4 | +Subject: [PATCH] fix(turtlebot4-base): support both libgpiod 1 and 2 |
| 5 | + |
| 6 | +Signed-off-by: Esteve Fernandez <esteve@apache.org> |
| 7 | + |
| 8 | +--- |
| 9 | + CMakeLists.txt | 7 ++++ |
| 10 | + include/turtlebot4_base/gpio_interface.hpp | 5 ++- |
| 11 | + src/gpio_interface.cpp | 66 ++++++++++++++++++++++++++++++ |
| 12 | + 3 files changed, 77 insertions(+), 1 deletion(-) |
| 13 | + |
| 14 | +diff --git a/CMakeLists.txt b/CMakeLists.txt |
| 15 | +index 6aeeea9..aeb64b4 100644 |
| 16 | +--- a/CMakeLists.txt |
| 17 | ++++ b/CMakeLists.txt |
| 18 | +@@ -25,6 +25,12 @@ find_package(std_msgs REQUIRED) |
| 19 | + find_package(sensor_msgs REQUIRED) |
| 20 | + find_package(turtlebot4_msgs REQUIRED) |
| 21 | + find_package(turtlebot4_node REQUIRED) |
| 22 | ++find_package(PkgConfig REQUIRED) |
| 23 | ++pkg_check_modules(GPIOD REQUIRED libgpiod) |
| 24 | ++ |
| 25 | ++if(NOT GPIOD_VERSION VERSION_LESS 2.0) |
| 26 | ++ add_definitions(-DTURTLEBOT4_BASE_GPIOD_V2) |
| 27 | ++endif() |
| 28 | + |
| 29 | + find_library(gpiod_library NAMES libgpiod.so) |
| 30 | + |
| 31 | +@@ -32,6 +38,7 @@ find_library(gpiod_library NAMES libgpiod.so) |
| 32 | + |
| 33 | + include_directories( |
| 34 | + include |
| 35 | ++ ${GPIOD_INCLUDE_DIRS} |
| 36 | + ) |
| 37 | + |
| 38 | + add_library(${PROJECT_NAME}_lib |
| 39 | +diff --git a/include/turtlebot4_base/gpio_interface.hpp b/include/turtlebot4_base/gpio_interface.hpp |
| 40 | +index c25317e..b1b27a3 100644 |
| 41 | +--- a/include/turtlebot4_base/gpio_interface.hpp |
| 42 | ++++ b/include/turtlebot4_base/gpio_interface.hpp |
| 43 | +@@ -19,7 +19,6 @@ |
| 44 | + #ifndef TURTLEBOT4_BASE__GPIO_INTERFACE_HPP_ |
| 45 | + #define TURTLEBOT4_BASE__GPIO_INTERFACE_HPP_ |
| 46 | + |
| 47 | +-#include <linux/gpio.h> |
| 48 | + #include <gpiod.h> |
| 49 | + |
| 50 | + #include <string> |
| 51 | +@@ -56,7 +55,11 @@ private: |
| 52 | + |
| 53 | + gpiod_chip * chip_; |
| 54 | + |
| 55 | ++#ifdef TURTLEBOT4_BASE_GPIOD_V2 |
| 56 | ++ std::map<uint8_t, gpiod_line_request *> lines_; |
| 57 | ++#else |
| 58 | + std::map<uint8_t, gpiod_line *> lines_; |
| 59 | ++#endif |
| 60 | + }; |
| 61 | + |
| 62 | + } // namespace turtlebot4_base |
| 63 | +diff --git a/src/gpio_interface.cpp b/src/gpio_interface.cpp |
| 64 | +index 3127ba3..a21b977 100644 |
| 65 | +--- a/src/gpio_interface.cpp |
| 66 | ++++ b/src/gpio_interface.cpp |
| 67 | +@@ -50,7 +50,16 @@ GpioInterface::GpioInterface(const uint8_t & gpio_chip_number) |
| 68 | + */ |
| 69 | + void GpioInterface::open_chip() |
| 70 | + { |
| 71 | ++#ifdef TURTLEBOT4_BASE_GPIOD_V2 |
| 72 | ++ std::string gpio_chip_path = gpio_chip_; |
| 73 | ++ if (gpio_chip_path.empty() || gpio_chip_path.front() != '/') { |
| 74 | ++ gpio_chip_path = "/dev/" + gpio_chip_path; |
| 75 | ++ } |
| 76 | ++ |
| 77 | ++ chip_ = gpiod_chip_open(gpio_chip_path.c_str()); |
| 78 | ++#else |
| 79 | + chip_ = gpiod_chip_open_by_name(gpio_chip_.c_str()); |
| 80 | ++#endif |
| 81 | + } |
| 82 | + |
| 83 | + /** |
| 84 | +@@ -59,7 +68,11 @@ void GpioInterface::open_chip() |
| 85 | + void GpioInterface::close_chip() |
| 86 | + { |
| 87 | + for (auto line : lines_) { |
| 88 | ++#ifdef TURTLEBOT4_BASE_GPIOD_V2 |
| 89 | ++ gpiod_line_request_release(line.second); |
| 90 | ++#else |
| 91 | + gpiod_line_release(line.second); |
| 92 | ++#endif |
| 93 | + } |
| 94 | + |
| 95 | + gpiod_chip_close(chip_); |
| 96 | +@@ -70,6 +83,47 @@ void GpioInterface::close_chip() |
| 97 | + */ |
| 98 | + void GpioInterface::add_line(uint8_t line, GpioInterfaceLineDirection direction) |
| 99 | + { |
| 100 | ++#ifdef TURTLEBOT4_BASE_GPIOD_V2 |
| 101 | ++ unsigned int offset = line; |
| 102 | ++ gpiod_request_config * request_config = gpiod_request_config_new(); |
| 103 | ++ gpiod_line_config * line_config = gpiod_line_config_new(); |
| 104 | ++ gpiod_line_settings * line_settings = gpiod_line_settings_new(); |
| 105 | ++ |
| 106 | ++ if (request_config == nullptr || line_config == nullptr || line_settings == nullptr) { |
| 107 | ++ gpiod_request_config_free(request_config); |
| 108 | ++ gpiod_line_config_free(line_config); |
| 109 | ++ gpiod_line_settings_free(line_settings); |
| 110 | ++ std::cerr << "Failed to configure GPIO Line" << std::endl; |
| 111 | ++ return; |
| 112 | ++ } |
| 113 | ++ |
| 114 | ++ if (direction != LINE_DIRECTION_INPUT && direction != LINE_DIRECTION_OUTPUT) { |
| 115 | ++ gpiod_request_config_free(request_config); |
| 116 | ++ gpiod_line_config_free(line_config); |
| 117 | ++ gpiod_line_settings_free(line_settings); |
| 118 | ++ std::cerr << "Invalid GPIO Line Direction" << std::endl; |
| 119 | ++ return; |
| 120 | ++ } |
| 121 | ++ |
| 122 | ++ gpiod_request_config_set_consumer(request_config, "Turtlebot4"); |
| 123 | ++ gpiod_line_settings_set_direction(line_settings, static_cast<gpiod_line_direction>(direction)); |
| 124 | ++ if (direction == LINE_DIRECTION_OUTPUT) { |
| 125 | ++ gpiod_line_settings_set_output_value(line_settings, GPIOD_LINE_VALUE_INACTIVE); |
| 126 | ++ } |
| 127 | ++ gpiod_line_config_add_line_settings(line_config, &offset, 1, line_settings); |
| 128 | ++ |
| 129 | ++ gpiod_line_request * gpio_line = gpiod_chip_request_lines(chip_, request_config, line_config); |
| 130 | ++ |
| 131 | ++ gpiod_request_config_free(request_config); |
| 132 | ++ gpiod_line_config_free(line_config); |
| 133 | ++ gpiod_line_settings_free(line_settings); |
| 134 | ++ |
| 135 | ++ if (gpio_line != nullptr) { |
| 136 | ++ lines_.insert(std::pair<uint8_t, gpiod_line_request *>(line, gpio_line)); |
| 137 | ++ } else { |
| 138 | ++ std::cerr << "Invalid GPIO Line" << std::endl; |
| 139 | ++ } |
| 140 | ++#else |
| 141 | + gpiod_line * gpio_line = gpiod_chip_get_line(chip_, line); |
| 142 | + |
| 143 | + if (gpio_line != nullptr) { |
| 144 | +@@ -86,6 +140,7 @@ void GpioInterface::add_line(uint8_t line, GpioInterfaceLineDirection direction) |
| 145 | + } else { |
| 146 | + std::cerr << "Invalid GPIO Line Direction" << std::endl; |
| 147 | + } |
| 148 | ++#endif |
| 149 | + } |
| 150 | + |
| 151 | + /** |
| 152 | +@@ -93,7 +148,14 @@ void GpioInterface::add_line(uint8_t line, GpioInterfaceLineDirection direction) |
| 153 | + */ |
| 154 | + void GpioInterface::write(uint8_t line, uint8_t value) |
| 155 | + { |
| 156 | ++#ifdef TURTLEBOT4_BASE_GPIOD_V2 |
| 157 | ++ gpiod_line_request_set_value( |
| 158 | ++ lines_[line], |
| 159 | ++ line, |
| 160 | ++ value ? GPIOD_LINE_VALUE_ACTIVE : GPIOD_LINE_VALUE_INACTIVE); |
| 161 | ++#else |
| 162 | + gpiod_line_set_value(lines_[line], value); |
| 163 | ++#endif |
| 164 | + } |
| 165 | + |
| 166 | + /** |
| 167 | +@@ -101,5 +163,9 @@ void GpioInterface::write(uint8_t line, uint8_t value) |
| 168 | + */ |
| 169 | + uint8_t GpioInterface::read(uint8_t line) |
| 170 | + { |
| 171 | ++#ifdef TURTLEBOT4_BASE_GPIOD_V2 |
| 172 | ++ return static_cast<uint8_t>(gpiod_line_request_get_value(lines_[line], line)); |
| 173 | ++#else |
| 174 | + return gpiod_line_get_value(lines_[line]); |
| 175 | ++#endif |
| 176 | + } |
| 177 | + |
| 178 | +-- |
| 179 | +2.54.0 |
0 commit comments