From 82cf1cb48e5f7d77469e1ae11337ce75c3783db7 Mon Sep 17 00:00:00 2001 From: a2nr Date: Thu, 12 Mar 2026 10:27:47 +0700 Subject: [PATCH] feat: port gpio_node to C++ using libgpiod; update package structure and dependencies for native Raspberry Pi support --- DOCUMENTATION.md | 2 +- docs/architecture.md | 5 +- docs/installation.md | 16 +- pixi.lock | 16 +- pixi.toml | 6 +- readme.md | 61 ++++++ src/gpio_node/CMakeLists.txt | 39 ++++ src/gpio_node/gpio_node/__init__.py | 0 src/gpio_node/gpio_node/gpio_node.py | 207 ------------------ src/gpio_node/include/gpio_node/gpio_node.hpp | 37 ++++ src/gpio_node/package.xml | 8 +- src/gpio_node/resource/gpio_node | 0 src/gpio_node/setup.cfg | 4 - src/gpio_node/setup.py | 24 -- src/gpio_node/src/gpio_node.cpp | 144 ++++++++++++ src/gpio_node/src/main.cpp | 11 + 16 files changed, 320 insertions(+), 260 deletions(-) create mode 100644 src/gpio_node/CMakeLists.txt delete mode 100644 src/gpio_node/gpio_node/__init__.py delete mode 100644 src/gpio_node/gpio_node/gpio_node.py create mode 100644 src/gpio_node/include/gpio_node/gpio_node.hpp delete mode 100644 src/gpio_node/resource/gpio_node delete mode 100644 src/gpio_node/setup.cfg delete mode 100644 src/gpio_node/setup.py create mode 100644 src/gpio_node/src/gpio_node.cpp create mode 100644 src/gpio_node/src/main.cpp diff --git a/DOCUMENTATION.md b/DOCUMENTATION.md index b5b9aa5..bec0d20 100644 --- a/DOCUMENTATION.md +++ b/DOCUMENTATION.md @@ -17,7 +17,7 @@ See [readme.md](readme.md) for project overview and status. | `blockly_app` — creating custom blocks (full guide + reference) | [src/blockly_app/BLOCKS.md](src/blockly_app/BLOCKS.md) | | `blockly_executor` — file reference, handlers & testing guide | [src/blockly_executor/README.md](src/blockly_executor/README.md) | | `blockly_interfaces` — ROS2 action & message interfaces | [src/blockly_interfaces/README.md](src/blockly_interfaces/README.md) | -| `gpio_node` — Raspberry Pi GPIO node (gpiod) | [src/gpio_node/gpio_node/gpio_node.py](src/gpio_node/gpio_node/gpio_node.py) | +| `gpio_node` — Raspberry Pi GPIO node (C++, libgpiod) | [src/gpio_node/](src/gpio_node/) | --- diff --git a/docs/architecture.md b/docs/architecture.md index 79d92a5..762468d 100644 --- a/docs/architecture.md +++ b/docs/architecture.md @@ -58,12 +58,11 @@ └─────────────┼────────────────────────────────────────────────────┘ │ ROS2 Topics: /gpio/write, /gpio/state ┌─────────────▼────────────────────────────────────────────────────┐ -│ GPIO Node (Raspberry Pi) │ -│ gpiod digital I/O │ +│ GPIO Node (Raspberry Pi, C++) │ +│ libgpiod digital I/O (hardware-only) │ │ │ │ Subscribes /gpio/write → set pin HIGH/LOW │ │ Publishes /gpio/state → input pin readings (10 Hz) │ -│ Falls back to simulation mode if gpiod unavailable │ └──────────────────────────────────────────────────────────────────┘ ``` diff --git a/docs/installation.md b/docs/installation.md index 44bbd30..3ca53da 100644 --- a/docs/installation.md +++ b/docs/installation.md @@ -40,14 +40,14 @@ amr-ros-k4/ # ROS2 Workspace root │ ├── test_block_gpio.py # digital_out + digital_in tests │ └── test_block_delay.py │ - ├── gpio_node/ # ROS2 GPIO node for Raspberry Pi (ament_python) + ├── gpio_node/ # ROS2 GPIO node for Raspberry Pi (ament_cmake, C++) + │ ├── CMakeLists.txt # ament_cmake build — rclcpp + libgpiodcxx │ ├── package.xml - │ ├── setup.py - │ ├── setup.cfg - │ ├── resource/gpio_node # Ament index marker - │ └── gpio_node/ # Python module - │ ├── __init__.py - │ └── gpio_node.py # ROS2 node: gpiod digital I/O + │ ├── include/gpio_node/ + │ │ └── gpio_node.hpp # GpioNode class declaration + │ └── src/ + │ ├── main.cpp # Entry point — rclcpp::spin(node) + │ └── gpio_node.cpp # ROS2 node: libgpiod digital I/O │ └── blockly_app/ # pywebview desktop application (ament_python) ├── package.xml @@ -169,7 +169,7 @@ pixi run gpio-node pixi run gpio-node -- --ros-args -p output_pins:=[17,27] -p input_pins:=[5,6] ``` -The GPIO node requires `gpiod` (auto-installed on `linux-aarch64`). On x86 dev machines it runs in simulation mode (log only). +The GPIO node is a C++ node that requires `libgpiod` (auto-installed on `linux-aarch64` via Pixi). It is hardware-only and must be built and run on the Raspberry Pi (`pixi run build-gpio && pixi run gpio-node`). The executor logs all received goals and their results to the terminal. diff --git a/pixi.lock b/pixi.lock index 2012add..1674e46 100644 --- a/pixi.lock +++ b/pixi.lock @@ -848,6 +848,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libglx-1.7.0-hd24410f_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libglx-devel-1.7.0-hd24410f_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libgomp-15.2.0-h8acb6b2_18.conda + - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libgpiod-2.2.1-he30d5cf_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libiconv-1.18-h90929bb_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libjpeg-turbo-3.1.2-he30d5cf_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/liblapack-3.11.0-5_h88aeb00_openblas.conda @@ -1160,7 +1161,6 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/yaml-cpp-0.8.0-h5ad3122_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/zipp-3.23.0-pyhcf101f3_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/zstd-1.5.7-h85ac4a6_6.conda - - pypi: https://files.pythonhosted.org/packages/c3/ab/c4ca0163eafb6eb9f78dbb4b6fc335c099f96040eedb1dd3c774378e754a/gpiod-2.4.0-cp312-cp312-manylinux2014_aarch64.manylinux_2_17_aarch64.manylinux_2_28_aarch64.whl packages: - conda: https://conda.anaconda.org/conda-forge/linux-64/_openmp_mutex-4.5-20_gnu.conda build_number: 20 @@ -2999,11 +2999,6 @@ packages: purls: [] size: 460055 timestamp: 1718980856608 -- pypi: https://files.pythonhosted.org/packages/c3/ab/c4ca0163eafb6eb9f78dbb4b6fc335c099f96040eedb1dd3c774378e754a/gpiod-2.4.0-cp312-cp312-manylinux2014_aarch64.manylinux_2_17_aarch64.manylinux_2_28_aarch64.whl - name: gpiod - version: 2.4.0 - sha256: 7433f9651baaf7ce8cc1269ee36eb981546cb6a078bc654fd1e51d78a8dd12ce - requires_python: '>=3.9.0' - conda: https://conda.anaconda.org/conda-forge/linux-64/graphite2-1.3.14-hecca717_2.conda sha256: 25ba37da5c39697a77fce2c9a15e48cf0a84f1464ad2aafbe53d8357a9f6cc8c md5: 2cd94587f3a401ae05e03a6caf09539d @@ -4646,6 +4641,15 @@ packages: purls: [] size: 588060 timestamp: 1771378040807 +- conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libgpiod-2.2.1-he30d5cf_1.conda + sha256: c05e193e121f6cebc8c235851b74f038419d4751b02668d1e607d0da3b32f7da + md5: 5cf7e8c67b3a7cebf16a68843f021530 + depends: + - libgcc >=14 + license: LGPL-2.1-or-later + purls: [] + size: 92400 + timestamp: 1756367973549 - conda: https://conda.anaconda.org/conda-forge/linux-64/libgz-cmake3-3.5.5-h54a6638_0.conda sha256: 6092ccfec5a52200a2dd5cfa33f67e7c75d473dbb1673baf145a56456589196f md5: 046a934130154ef383da67712d179235 diff --git a/pixi.toml b/pixi.toml index 6cc570d..adfb920 100644 --- a/pixi.toml +++ b/pixi.toml @@ -24,16 +24,14 @@ qtpy = "*" [target.linux-aarch64.dependencies] ros-jazzy-ros-base = "*" +ros-jazzy-rclcpp = "*" # C++ ROS2 client library for gpio_node +libgpiod = "*" # C/C++ libgpiod (gpiodcxx) for GPIO hardware access # pywebview is only on PyPI, not conda-forge. # Desktop-only — Raspberry Pi only runs Executor Node. [target.linux-64.pypi-dependencies] pywebview = "*" -# gpiod for GPIO control on Raspberry Pi -[target.linux-aarch64.pypi-dependencies] -gpiod = "*" - [tasks] build-interfaces = "colcon build --symlink-install --packages-select blockly_interfaces" build-executor = { cmd = "colcon build --symlink-install --packages-select blockly_executor", depends-on = ["build-interfaces"] } diff --git a/readme.md b/readme.md index 599a014..d7f4faf 100644 --- a/readme.md +++ b/readme.md @@ -64,3 +64,64 @@ Debug mode tidak bisa step into ke function blocks karena `highlightBlock()` ber - Debug mode pause di block pertama tanpa perlu breakpoint - Non-debug mode (`runProgram()`) tidak terpengaruh — `await` pada synchronous `highlightBlock()` adalah no-op - `pixi run build-app` berhasil tanpa error + +## 2 Enhancement: Port gpio_node to C++ : [ ] +gpio_node saat ini ditulis dalam Python (`ament_python`) menggunakan `rclpy` dan `gpiod` (Python binding). Untuk performa dan deployment yang lebih baik di Raspberry Pi, port ke C++ menggunakan `rclcpp` dan `libgpiod` C++ API (`libgpiodcxx`). Node ini **hardware-only** — tidak ada simulation fallback, hanya berjalan di Raspberry Pi dengan akses ke `/dev/gpiochipX`. + +### Scope +1. **C++ port** — Rewrite `gpio_node.py` → `gpio_node.cpp` menggunakan `rclcpp`, `libgpiod` C++ bindings +2. **ament_cmake** — Ubah package structure dari `ament_python` ke `ament_cmake` (`CMakeLists.txt` + `package.xml`) +3. **Pixi dependency management** — Tambahkan `ros-jazzy-rclcpp`, `libgpiod` (C++ library) sebagai dependency di `pixi.toml` untuk `linux-aarch64` +4. **Native build on Pi** — Build langsung di Raspberry Pi via `pixi run build-gpio` (cross-compilation ROS2 C++ terlalu kompleks — butuh full aarch64 sysroot dengan semua ROS2 libs) + +### Perubahan yang Dibutuhkan + +#### A. Package Structure (hapus Python, buat C++) +``` +src/gpio_node/ +├── CMakeLists.txt # ament_cmake, find libgpiod, build gpio_node executable +├── package.xml # ament_cmake, depend: rclcpp, blockly_interfaces, libgpiod +├── include/gpio_node/ +│ └── gpio_node.hpp # GpioNode class declaration +└── src/ + ├── gpio_node.cpp # GpioNode class implementation + └── main.cpp # main() entry point — rclcpp::spin(node) +``` + +Hapus: `gpio_node/gpio_node.py`, `gpio_node/__init__.py`, `setup.py`, `setup.cfg`, `resource/gpio_node` + +#### B. C++ Node — Same API Surface +- **Subscribe** `/gpio/write` (`blockly_interfaces::msg::GpioWrite`) — set pin output via `gpiod::line_request::set_value()` +- **Publish** `/gpio/state` (`blockly_interfaces::msg::GpioRead`) — poll input pins via timer (10 Hz default) +- **Parameters**: `output_pins` (int array), `input_pins` (int array), `input_publish_rate` (double), `gpio_chip` (string, default `/dev/gpiochip0`) +- Hanya pin yang ada di `output_pins` yang bisa di-write; pin tidak terdaftar → log warning +- Cleanup: `gpiod::line_request::release()` di destructor + +#### C. pixi.toml — Dependency Updates +```toml +[target.linux-aarch64.dependencies] +ros-jazzy-ros-base = "*" +ros-jazzy-rclcpp = "*" # C++ ROS2 client library +libgpiod = "*" # C/C++ libgpiod (gpiodcxx) +``` +Hapus `gpiod` dari `[target.linux-aarch64.pypi-dependencies]` + +#### D. Build on Pi +Build dilakukan **natively di Raspberry Pi** (cross-compilation ROS2 C++ tidak praktis — butuh full aarch64 sysroot dengan semua ROS2 shared libraries). + +```bash +# Di Pi: clone repo + install deps + build + run +git clone ~/amr-ros-k4 && cd ~/amr-ros-k4 +pixi install && pixi run build-gpio +pixi run gpio-node +``` + +### Definition Of Done +- `src/gpio_node/` berisi `CMakeLists.txt`, `package.xml`, `include/`, `src/` — tidak ada file Python +- `pixi.toml` menyertakan `ros-jazzy-rclcpp` dan `libgpiod` di `linux-aarch64` dependencies +- `pixi.toml` tidak lagi menyertakan `gpiod` di `linux-aarch64` pypi-dependencies +- `pixi run build-gpio` berhasil di Raspberry Pi (native build) tanpa error +- Node berjalan: `pixi run gpio-node` — subscribe `/gpio/write`, publish `/gpio/state` +- Parameter `output_pins`, `input_pins`, `input_publish_rate`, `gpio_chip` berfungsi via `--ros-args -p` +- Executor (`blockly_executor`) tetap berfungsi tanpa perubahan — interface ROS2 identik +- `pixi run build-gpio` di Pi (native build) berhasil tanpa error diff --git a/src/gpio_node/CMakeLists.txt b/src/gpio_node/CMakeLists.txt new file mode 100644 index 0000000..515cf3a --- /dev/null +++ b/src/gpio_node/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.10) +project(gpio_node) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# ROS2 dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(blockly_interfaces REQUIRED) + +# libgpiod C++ bindings (via pkg-config) +find_package(PkgConfig REQUIRED) +pkg_check_modules(GPIOD REQUIRED libgpiodcxx) + +add_executable(gpio_node + src/main.cpp + src/gpio_node.cpp +) + +target_include_directories(gpio_node PUBLIC + $ + $ + ${GPIOD_INCLUDE_DIRS} +) + +target_link_libraries(gpio_node ${GPIOD_LIBRARIES}) + +ament_target_dependencies(gpio_node + rclcpp + blockly_interfaces +) + +install(TARGETS gpio_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/src/gpio_node/gpio_node/__init__.py b/src/gpio_node/gpio_node/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/gpio_node/gpio_node/gpio_node.py b/src/gpio_node/gpio_node/gpio_node.py deleted file mode 100644 index fd554ec..0000000 --- a/src/gpio_node/gpio_node/gpio_node.py +++ /dev/null @@ -1,207 +0,0 @@ -"""ROS2 GPIO node — digital input/output for Raspberry Pi via gpiod. - -Node ini berjalan di Raspberry Pi dan menjadi jembatan antara ROS2 topic -dengan pin GPIO fisik. Executor handler di sisi desktop publish/subscribe -ke topic node ini untuk mengontrol hardware. - -Alur komunikasi: - Executor → publish GpioWrite ke /gpio/write → node ini → gpiod set pin - Node ini → timer baca pin → publish GpioRead ke /gpio/state → Executor - -Jika gpiod tidak tersedia (misalnya di x86 dev machine), node tetap berjalan -dalam simulation mode — hanya logging tanpa akses hardware. Ini memungkinkan -pengembangan dan testing tanpa Pi. - -Menggunakan gpiod v2.x API (request_lines / LineSettings / Value). -""" - -import rclpy -from rclpy.node import Node - -from blockly_interfaces.msg import GpioWrite, GpioRead - -# gpiod hanya tersedia di Raspberry Pi (linux-aarch64). -# Pada dev machine (linux-64), import akan gagal dan node -# berjalan dalam simulation mode. -try: - import gpiod - from gpiod.line import Direction, Value - HAS_GPIOD = True -except ImportError: - HAS_GPIOD = False - - -class GpioNode(Node): - - def __init__(self) -> None: - super().__init__("gpio_node") - - # Pin-pin yang dikonfigurasi bisa diubah via ROS2 parameter saat launch. - # Contoh: ros2 run gpio_node gpio_node --ros-args -p output_pins:=[17,27] - self.declare_parameter("output_pins", [17, 27, 22]) - self.declare_parameter("input_pins", [5, 6, 13]) - self.declare_parameter("input_publish_rate", 10.0) # Hz - self.declare_parameter("gpio_chip", "/dev/gpiochip0") # gpiod v2.x butuh path lengkap - - self._output_pins = list( - self.get_parameter("output_pins").get_parameter_value().integer_array_value - ) - self._input_pins = list( - self.get_parameter("input_pins").get_parameter_value().integer_array_value - ) - rate = ( - self.get_parameter("input_publish_rate").get_parameter_value().double_value - ) - chip_name = ( - self.get_parameter("gpio_chip").get_parameter_value().string_value - ) - - # gpiod v2.x: request_lines() mengembalikan LineRequest object, - # bukan per-line object seperti v1.x. - self._output_request = None - self._input_request = None - - # Instance flag — True hanya jika gpiod tersedia DAN chip berhasil dibuka. - # Berbeda dari module-level HAS_GPIOD yang hanya cek import. - self._has_hardware = False - - if HAS_GPIOD: - try: - self._setup_gpio(chip_name) - self._has_hardware = True - except (FileNotFoundError, PermissionError, OSError) as e: - # gpiod terinstall tapi GPIO chip tidak ada (dev machine x86) - # atau permission denied — fallback ke simulation mode - self.get_logger().warn( - f"gpiod available but cannot open {chip_name}: {e} " - "— running in simulation mode" - ) - else: - self.get_logger().warn( - "gpiod not available — running in simulation mode (log only)" - ) - - # Terima perintah digital output dari executor - self._write_sub = self.create_subscription( - GpioWrite, "/gpio/write", self._write_callback, 10 - ) - - # Publikasikan state digital input untuk dibaca executor - self._state_pub = self.create_publisher(GpioRead, "/gpio/state", 10) - - # Timer polling — baca semua input pin secara periodik. - # Pendekatan polling dipilih karena gpiod event/interrupt - # memerlukan konfigurasi edge detection yang lebih kompleks. - if self._input_pins: - period = 1.0 / rate - self._read_timer = self.create_timer(period, self._read_callback) - - self.get_logger().info( - f"GpioNode ready — outputs={self._output_pins}, " - f"inputs={self._input_pins}, " - f"gpiod={'yes' if self._has_hardware else 'no (simulation)'}" - ) - - def _setup_gpio(self, chip_name: str) -> None: - """Konfigurasi GPIO lines via gpiod v2.x API. - - gpiod v2.x menggunakan request_lines() untuk merequest semua pin - sekaligus, berbeda dari v1.x yang per-line. LineSettings menentukan - direction (input/output) untuk setiap pin. - """ - # Request output lines sebagai satu batch - if self._output_pins: - out_config = { - pin: gpiod.LineSettings(direction=Direction.OUTPUT) - for pin in self._output_pins - } - self._output_request = gpiod.request_lines( - chip_name, consumer="gpio_node", config=out_config - ) - for pin in self._output_pins: - self.get_logger().info(f"Configured pin {pin} as OUTPUT") - - # Request input lines sebagai satu batch - if self._input_pins: - in_config = { - pin: gpiod.LineSettings(direction=Direction.INPUT) - for pin in self._input_pins - } - self._input_request = gpiod.request_lines( - chip_name, consumer="gpio_node", config=in_config - ) - for pin in self._input_pins: - self.get_logger().info(f"Configured pin {pin} as INPUT") - - def _write_callback(self, msg: GpioWrite) -> None: - """Handle perintah digital output dari executor. - - Hanya pin yang sudah dikonfigurasi sebagai output yang akan diproses. - Pin yang tidak terdaftar akan di-ignore dengan warning — ini mencegah - akses ke pin yang belum di-request di gpiod (akan menyebabkan error). - """ - pin = int(msg.pin) - state = msg.state - state_str = "HIGH" if state else "LOW" - - if pin not in self._output_pins and self._has_hardware: - self.get_logger().warn( - f"Pin {pin} not configured as output — ignoring write" - ) - return - - if self._has_hardware and self._output_request: - value = Value.ACTIVE if state else Value.INACTIVE - self._output_request.set_value(pin, value) - - self.get_logger().info(f"GPIO write: pin={pin} state={state_str}") - - def _read_callback(self) -> None: - """Baca semua input pin dan publish state masing-masing. - - Dipanggil oleh timer secara periodik (default 10 Hz). - Setiap pin dipublish sebagai message terpisah agar subscriber - bisa memfilter berdasarkan pin number. - """ - for pin in self._input_pins: - msg = GpioRead() - msg.pin = pin - - if self._has_hardware and self._input_request: - msg.state = self._input_request.get_value(pin) == Value.ACTIVE - else: - msg.state = False # simulation: selalu LOW - - self._state_pub.publish(msg) - - def destroy_node(self) -> None: - """Release GPIO lines saat shutdown. - - gpiod v2.x: release() dipanggil pada LineRequest object, - bukan per-line seperti v1.x. - """ - if self._has_hardware: - if self._output_request: - self._output_request.release() - if self._input_request: - self._input_request.release() - self.get_logger().info("GPIO lines released") - super().destroy_node() - - -def main(args=None): - rclpy.init(args=args) - - node = GpioNode() - - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/gpio_node/include/gpio_node/gpio_node.hpp b/src/gpio_node/include/gpio_node/gpio_node.hpp new file mode 100644 index 0000000..109f219 --- /dev/null +++ b/src/gpio_node/include/gpio_node/gpio_node.hpp @@ -0,0 +1,37 @@ +#ifndef GPIO_NODE__GPIO_NODE_HPP_ +#define GPIO_NODE__GPIO_NODE_HPP_ + +#include +#include +#include + +#include +#include + +#include "blockly_interfaces/msg/gpio_read.hpp" +#include "blockly_interfaces/msg/gpio_write.hpp" + +class GpioNode : public rclcpp::Node +{ +public: + GpioNode(); + ~GpioNode() override; + +private: + void setup_gpio(const std::string & chip_path); + void write_callback(const blockly_interfaces::msg::GpioWrite::SharedPtr msg); + void read_callback(); + + std::vector output_pins_; + std::vector input_pins_; + + // gpiod v2.x line requests — one per direction group + std::unique_ptr output_request_; + std::unique_ptr input_request_; + + rclcpp::Subscription::SharedPtr write_sub_; + rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::TimerBase::SharedPtr read_timer_; +}; + +#endif // GPIO_NODE__GPIO_NODE_HPP_ diff --git a/src/gpio_node/package.xml b/src/gpio_node/package.xml index 570fb8c..e91c088 100644 --- a/src/gpio_node/package.xml +++ b/src/gpio_node/package.xml @@ -3,14 +3,16 @@ gpio_node 0.1.0 - ROS2 GPIO node for Raspberry Pi — digital input/output via gpiod + ROS2 GPIO node for Raspberry Pi — digital input/output via libgpiod (C++) developer MIT - rclpy + ament_cmake + + rclcpp blockly_interfaces - ament_python + ament_cmake diff --git a/src/gpio_node/resource/gpio_node b/src/gpio_node/resource/gpio_node deleted file mode 100644 index e69de29..0000000 diff --git a/src/gpio_node/setup.cfg b/src/gpio_node/setup.cfg deleted file mode 100644 index d65073a..0000000 --- a/src/gpio_node/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/gpio_node -[install] -install_scripts=$base/lib/gpio_node diff --git a/src/gpio_node/setup.py b/src/gpio_node/setup.py deleted file mode 100644 index 1260161..0000000 --- a/src/gpio_node/setup.py +++ /dev/null @@ -1,24 +0,0 @@ -from setuptools import find_packages, setup - -package_name = "gpio_node" - -setup( - name=package_name, - version="0.1.0", - packages=find_packages(exclude=["test"]), - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="developer", - maintainer_email="dev@example.com", - description="ROS2 GPIO node for Raspberry Pi", - license="MIT", - entry_points={ - "console_scripts": [ - "gpio_node = gpio_node.gpio_node:main", - ], - }, -) diff --git a/src/gpio_node/src/gpio_node.cpp b/src/gpio_node/src/gpio_node.cpp new file mode 100644 index 0000000..56c02c7 --- /dev/null +++ b/src/gpio_node/src/gpio_node.cpp @@ -0,0 +1,144 @@ +#include "gpio_node/gpio_node.hpp" + +#include +#include +#include +#include + +GpioNode::GpioNode() +: Node("gpio_node") +{ + // Declare ROS2 parameters — same defaults as the Python version + this->declare_parameter("output_pins", std::vector{17, 27, 22}); + this->declare_parameter("input_pins", std::vector{5, 6, 13}); + this->declare_parameter("input_publish_rate", 10.0); + this->declare_parameter("gpio_chip", std::string("/dev/gpiochip0")); + + output_pins_ = this->get_parameter("output_pins").as_integer_array(); + input_pins_ = this->get_parameter("input_pins").as_integer_array(); + double rate = this->get_parameter("input_publish_rate").as_double(); + std::string chip_path = this->get_parameter("gpio_chip").as_string(); + + // Setup GPIO hardware — no simulation fallback, will throw on failure + setup_gpio(chip_path); + + // Subscribe to /gpio/write from executor + write_sub_ = this->create_subscription( + "/gpio/write", 10, + std::bind(&GpioNode::write_callback, this, std::placeholders::_1)); + + // Publisher for /gpio/state to executor + state_pub_ = this->create_publisher( + "/gpio/state", 10); + + // Polling timer for input pins + if (!input_pins_.empty()) { + auto period = std::chrono::duration(1.0 / rate); + read_timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&GpioNode::read_callback, this)); + } + + // Log ready status + std::ostringstream oss; + oss << "GpioNode ready — outputs=["; + for (size_t i = 0; i < output_pins_.size(); ++i) { + if (i > 0) oss << ","; + oss << output_pins_[i]; + } + oss << "], inputs=["; + for (size_t i = 0; i < input_pins_.size(); ++i) { + if (i > 0) oss << ","; + oss << input_pins_[i]; + } + oss << "]"; + RCLCPP_INFO(this->get_logger(), "%s", oss.str().c_str()); +} + +GpioNode::~GpioNode() +{ + // gpiod::line_request releases lines automatically when destroyed, + // but we log it explicitly for visibility. + if (output_request_) { + output_request_->release(); + output_request_.reset(); + } + if (input_request_) { + input_request_->release(); + input_request_.reset(); + } + RCLCPP_INFO(this->get_logger(), "GPIO lines released"); +} + +void GpioNode::setup_gpio(const std::string & chip_path) +{ + gpiod::chip chip(chip_path); + + // Request output lines as a single batch + if (!output_pins_.empty()) { + auto out_settings = gpiod::line_settings(); + out_settings.set_direction(gpiod::line::direction::OUTPUT); + + auto out_rb = chip.prepare_request().set_consumer("gpio_node"); + for (auto pin : output_pins_) { + out_rb.add_line_settings(static_cast(pin), out_settings); + RCLCPP_INFO(this->get_logger(), "Configured pin %ld as OUTPUT", pin); + } + output_request_ = std::make_unique(out_rb.do_request()); + } + + // Request input lines as a single batch + if (!input_pins_.empty()) { + auto in_settings = gpiod::line_settings(); + in_settings.set_direction(gpiod::line::direction::INPUT); + + auto in_rb = chip.prepare_request().set_consumer("gpio_node"); + for (auto pin : input_pins_) { + in_rb.add_line_settings(static_cast(pin), in_settings); + RCLCPP_INFO(this->get_logger(), "Configured pin %ld as INPUT", pin); + } + input_request_ = std::make_unique(in_rb.do_request()); + } +} + +void GpioNode::write_callback( + const blockly_interfaces::msg::GpioWrite::SharedPtr msg) +{ + int pin = static_cast(msg->pin); + bool state = msg->state; + + // Check if pin is configured as output + if (std::find(output_pins_.begin(), output_pins_.end(), pin) == + output_pins_.end()) + { + RCLCPP_WARN( + this->get_logger(), + "Pin %d not configured as output — ignoring write", pin); + return; + } + + if (output_request_) { + output_request_->set_value( + pin, + state ? gpiod::line::value::ACTIVE : gpiod::line::value::INACTIVE); + } + + RCLCPP_INFO( + this->get_logger(), + "GPIO write: pin=%d state=%s", pin, state ? "HIGH" : "LOW"); +} + +void GpioNode::read_callback() +{ + for (auto pin : input_pins_) { + auto msg = blockly_interfaces::msg::GpioRead(); + msg.pin = static_cast(pin); + + if (input_request_) { + msg.state = + input_request_->get_value(pin) == gpiod::line::value::ACTIVE; + } + + state_pub_->publish(msg); + } +} diff --git a/src/gpio_node/src/main.cpp b/src/gpio_node/src/main.cpp new file mode 100644 index 0000000..48efe5c --- /dev/null +++ b/src/gpio_node/src/main.cpp @@ -0,0 +1,11 @@ +#include +#include "gpio_node/gpio_node.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}