feat: port gpio_node to C++ using libgpiod; update package structure and dependencies for native Raspberry Pi support
This commit is contained in:
parent
5738255423
commit
82cf1cb48e
|
|
@ -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/) |
|
||||
|
||||
---
|
||||
|
||||
|
|
|
|||
|
|
@ -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 │
|
||||
└──────────────────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
||||
|
|
|
|||
16
pixi.lock
16
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
|
||||
|
|
|
|||
|
|
@ -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"] }
|
||||
|
|
|
|||
61
readme.md
61
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 <repo> ~/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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
${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()
|
||||
|
|
@ -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()
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
#ifndef GPIO_NODE__GPIO_NODE_HPP_
|
||||
#define GPIO_NODE__GPIO_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <gpiod.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#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<int64_t> output_pins_;
|
||||
std::vector<int64_t> input_pins_;
|
||||
|
||||
// gpiod v2.x line requests — one per direction group
|
||||
std::unique_ptr<gpiod::line_request> output_request_;
|
||||
std::unique_ptr<gpiod::line_request> input_request_;
|
||||
|
||||
rclcpp::Subscription<blockly_interfaces::msg::GpioWrite>::SharedPtr write_sub_;
|
||||
rclcpp::Publisher<blockly_interfaces::msg::GpioRead>::SharedPtr state_pub_;
|
||||
rclcpp::TimerBase::SharedPtr read_timer_;
|
||||
};
|
||||
|
||||
#endif // GPIO_NODE__GPIO_NODE_HPP_
|
||||
|
|
@ -3,14 +3,16 @@
|
|||
<package format="3">
|
||||
<name>gpio_node</name>
|
||||
<version>0.1.0</version>
|
||||
<description>ROS2 GPIO node for Raspberry Pi — digital input/output via gpiod</description>
|
||||
<description>ROS2 GPIO node for Raspberry Pi — digital input/output via libgpiod (C++)</description>
|
||||
<maintainer email="dev@example.com">developer</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>blockly_interfaces</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
|
|
|||
|
|
@ -1,4 +0,0 @@
|
|||
[develop]
|
||||
script_dir=$base/lib/gpio_node
|
||||
[install]
|
||||
install_scripts=$base/lib/gpio_node
|
||||
|
|
@ -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",
|
||||
],
|
||||
},
|
||||
)
|
||||
|
|
@ -0,0 +1,144 @@
|
|||
#include "gpio_node/gpio_node.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
|
||||
GpioNode::GpioNode()
|
||||
: Node("gpio_node")
|
||||
{
|
||||
// Declare ROS2 parameters — same defaults as the Python version
|
||||
this->declare_parameter("output_pins", std::vector<int64_t>{17, 27, 22});
|
||||
this->declare_parameter("input_pins", std::vector<int64_t>{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<blockly_interfaces::msg::GpioWrite>(
|
||||
"/gpio/write", 10,
|
||||
std::bind(&GpioNode::write_callback, this, std::placeholders::_1));
|
||||
|
||||
// Publisher for /gpio/state to executor
|
||||
state_pub_ = this->create_publisher<blockly_interfaces::msg::GpioRead>(
|
||||
"/gpio/state", 10);
|
||||
|
||||
// Polling timer for input pins
|
||||
if (!input_pins_.empty()) {
|
||||
auto period = std::chrono::duration<double>(1.0 / rate);
|
||||
read_timer_ = this->create_wall_timer(
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(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<unsigned int>(pin), out_settings);
|
||||
RCLCPP_INFO(this->get_logger(), "Configured pin %ld as OUTPUT", pin);
|
||||
}
|
||||
output_request_ = std::make_unique<gpiod::line_request>(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<unsigned int>(pin), in_settings);
|
||||
RCLCPP_INFO(this->get_logger(), "Configured pin %ld as INPUT", pin);
|
||||
}
|
||||
input_request_ = std::make_unique<gpiod::line_request>(in_rb.do_request());
|
||||
}
|
||||
}
|
||||
|
||||
void GpioNode::write_callback(
|
||||
const blockly_interfaces::msg::GpioWrite::SharedPtr msg)
|
||||
{
|
||||
int pin = static_cast<int>(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<uint8_t>(pin);
|
||||
|
||||
if (input_request_) {
|
||||
msg.state =
|
||||
input_request_->get_value(pin) == gpiod::line::value::ACTIVE;
|
||||
}
|
||||
|
||||
state_pub_->publish(msg);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "gpio_node/gpio_node.hpp"
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<GpioNode>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue