diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..d8f2386 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,56 @@ +# Changelog + +## v0.1.0 (2026-05-07) + +E-Skin 手指力传感器 SDK 首个正式版本，支持 Rust / C/C++ / Python 多语言调用。 + +### ✨ 核心功能 + +- **串口通信**：基于 `serialport` 的串口传输层，支持 UART 连接 +- **协议编解码**：完整的请求/响应帧编解码，内置 CRC-8/X25 校验 +- **寄存器读写**：底层寄存器原始字节读写接口 +- **设备配置管理**：硬件版本读取、矩阵行列尺寸读写、设备配置寄存器读写 +- **流式采集**：基于 `crossbeam-channel` 的高性能线程间数据传输 +- **设备管理**：设备打开/关闭状态机，支持 Open / Streaming / Error 状态 + +### 🔌 FFI 接口 + +提供完整的 C FFI 导出，支持 C/C++ 和 Python 调用： + +| 接口 | 说明 | +|------|------| +| `eskin_version` | SDK 版本 | +| `eskin_open` / `eskin_close` | 设备打开/关闭 | +| `eskin_read_register` / `eskin_write_register` | 寄存器原始读写 | +| `eskin_read_hdw_version` | 硬件版本号 | +| `eskin_read_matrix_row` / `col` | 矩阵行列读取 | +| `eskin_write_matrix_row` / `col` | 矩阵行列写入 | +| `eskin_read_device_config1` / `2` | 设备配置寄存器读取 | +| `eskin_write_device_config1` / `2` | 设备配置寄存器写入 | + +### 📦 示例代码 + +- **C++ 示例**：`example/cpp/main.cpp` +- **Python 示例**：`example/python/example.py` + `example/python/eskin_ffi.py` + +### 🛠 构建 + +```bash +# 安装依赖（Ubuntu） +sudo apt install pkg-config libudev-dev + +# 构建 +cargo build --release +# 输出: target/release/libeskin_finger_sdk.so +``` + +### 📋 协议 + +- 请求帧：`[0x55, 0xAA] + data_len(LE) + dev_addr + func + addr(LE) + len(LE) + payload + crc8` +- 响应帧：同上格式 + status 字段，`0x00` 表示成功 +- 支持功能码：`READ=0xFB`、`WRITE=0x79`、`RESPONSE_READ=0xFF`、`RESPONSE_WRITE=0xF9` + +### ⚠️ 已知限制 + +- 当前仅支持串口传输（UART） +- 版本号 `0.1.0`，API 可能在后续版本中调整 \ No newline at end of file diff --git a/README.md b/README.md index 65368d3..39beb74 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Eskin Finger SDK -E-Skin 手指力传感器 Rust SDK，提供串口通信、寄存器读写、流式采集能力，支持 Rust / C/C++ / Python 调用。 +E-Skin 手指力传感器 Rust SDK，提供串口通信、寄存器读写、流式采集能力，支持 Rust / C/C++ / Python / ROS2 调用。 ## 目录结构 @@ -22,8 +22,10 @@ include/ eskin_ffi.h — C/C++ 头文件 example/ - cpp/main.cpp — C++ 使用示例 - python/ — Python 使用示例 + cpp/main.cpp — 独立 C++ 使用示例（含流式采集） + python/eskin_ffi.py — Python FFI 包装器 + python/example.py — Python 使用示例（含流式采集） + ros-cpp/ — ROS2 C++ 示例（publisher/subscriber） ``` --- @@ -51,6 +53,20 @@ println!("Wrote {} bytes", count); device.close().unwrap(); ``` +### 流式采集（Rust） + +```rust +// 启动流式采集 +device.start_stream().unwrap(); + +// 读取采样数据 +let sample = device.read_sample(Some(std::time::Duration::from_millis(200))).unwrap(); +println!("force: fx={} fy={} fz={}", sample.force.fx, sample.force.fy, sample.force.fz); + +// 停止采集 +device.stop_stream().unwrap(); +``` + --- ## 使用 C/C++ @@ -69,26 +85,46 @@ cargo build --release ### 编译 C++ 示例 ```bash -g++ example/cpp/main.cpp -I include -L target/release -leskin_finger_sdk -o example_cpp +g++ -std=c++17 example/cpp/main.cpp -I include -L target/release -leskin_finger_sdk -lpthread -o example_cpp LD_LIBRARY_PATH=target/release ./example_cpp ``` ### C++ 代码示例 +完整示例见 `example/cpp/main.cpp`，包含 Command 模式和 Streaming 模式： + ```cpp #include "eskin_ffi.h" #include +#include +#include +#include +#include int main() { + // 打开设备 EskinDeviceHandle dev = eskin_open("/dev/ttyUSB0", nullptr); if (!dev) return 1; - uint8_t buf[256]; - uint32_t actual; - if (eskin_read_register(dev, 0x0000, 4, buf, sizeof(buf), &actual) == ESkinSuccess) { - printf("Serial: %02X %02X %02X %02X\n", buf[0], buf[1], buf[2], buf[3]); + // Command 模式：读取设备信息 + char hw_buf[64] = {}; + uint32_t hw_len = 0; + eskin_read_hdw_version(dev, hw_buf, sizeof(hw_buf), &hw_len); + printf("Hardware version: %.*s\n", (int)hw_len, hw_buf); + + // Streaming 模式：持续采集力数据 + eskin_start_stream(dev); + + CFingerSample sample; + memset(&sample, 0, sizeof(sample)); + if (eskin_read_sample(dev, 200, &sample) == ESkinSuccess) { + printf("force: fx=%u fy=%u fz=%u\n", + sample.combined_force.force.fx, + sample.combined_force.force.fy, + sample.combined_force.force.fz); } + eskin_stop_stream(dev); eskin_close(dev); return 0; } @@ -108,26 +144,80 @@ cd example/python python3 example.py ``` -> **注意：** Python 示例默认从当前目录加载 `libeskin_finger_sdk.so`，请确保 `.so` 文件已复制到 `example/python/` 目录下，或修改 `example.py` 中的 `LIB_PATH` 指向正确的路径。 +> **注意：** Python 示例默认从当前目录加载 `libeskin_finger_sdk.so`，请确保 `.so` 文件已复制到 `example/python/` 目录下，或修改 `eskin_ffi.py` 中的 `LIB_PATH` 指向正确的路径。 + +### Python 代码示例 + +完整示例见 `example/python/example.py`，包含 Command 模式和 Streaming 模式： ```python from eskin_ffi import EskinDevice -with EskinDevice("libeskin_finger_sdk.so") as dev: +with EskinDevice() as dev: dev.open("/dev/ttyUSB0") - # 读取硬件版本 + # Command 模式：读取设备信息 print(f"Hardware version: {dev.read_hdw_version()}") - - # 读取矩阵尺寸 print(f"Matrix: {dev.read_matrix_row()} x {dev.read_matrix_col()}") - # 读寄存器 - data = dev.read_register(0x0000, 4) - print(f"Serial: {data.hex()}") + # Streaming 模式：持续采集力数据 + dev.start_stream() + for _ in range(10): + sample = dev.read_sample(timeout_ms=200) + f = sample.combined_force.force + print(f"fx={f.fx} fy={f.fy} fz={f.fz}") + dev.stop_stream() +``` - # 写寄存器 - dev.write_register(0x0030, bytes([0x01, 0x00, 0x00, 0x00])) +--- + +## ROS2 示例 + +ROS2 C++ 示例位于 `example/ros-cpp/`，包含： + +- `eskin_publisher.cpp` — 独立线程读取力数据，定时发布到 `/comb_force` topic（`std_msgs/UInt32`） +- `eskin_subscriber.cpp` — 订阅 `/comb_force` topic 并打印 +- `CMakeLists.txt` / `package.xml` — ROS2 包配置 + +### 构建 + +```bash +cd your_ros2_ws/src +ln -s /path/to/eskin-finger-sdk/example/ros-cpp eskin_example +cd .. && colcon build --packages-select eskin_example +``` + +### 运行 + +```bash +# 终端1：启动 publisher（指定设备路径） +ros2 run eskin_example eskin_publisher /dev/ttyUSB0 + +# 终端2：订阅数据 +ros2 run eskin_example eskin_subscriber +``` + +--- + +## 数据类型 + +```c +typedef struct { + uint32_t fx; // X 轴力（uint32） + uint32_t fy; // Y 轴力（uint32） + uint32_t fz; // Z 轴力（uint32） +} CForce3D; + +typedef struct { + uint32_t module; // 模块编号 + CForce3D force; // 三维力 +} CCombinedForce; + +typedef struct { + uint64_t timestamp_us; // 时间戳（微秒） + uint32_t sequence; // 序列号 + CCombinedForce combined_force; // 组合力数据 +} CFingerSample; ``` --- @@ -150,6 +240,10 @@ with EskinDevice("libeskin_finger_sdk.so") as dev: | `eskin_write_device_config2(handle, enable, return_count)` | 写入设备配置寄存器2 | | `eskin_write_matrix_row(handle, row, return_count)` | 写入矩阵行数 | | `eskin_write_matrix_col(handle, col, return_count)` | 写入矩阵列数 | +| `eskin_start_stream(handle)` | 启动流式采集 | +| `eskin_stop_stream(handle)` | 停止流式采集 | +| `eskin_read_sample(handle, timeout_ms, out)` | 读取一个采样数据 | +| `eskin_get_mode(handle, out)` | 查询当前设备模式（0=Command, 1=Streaming） | --- @@ -209,17 +303,17 @@ cargo test protocol::tests # 仅协议层 ## 架构 ```text -User / C / Python - ↓ FFI - DeviceWrapper (handle) - ↓ - EskinDeviceInner - ↓ read_register / write_register - EskinProtocolCodec (encode/decode + CRC8) - ↓ - SerialTransport (write/read bytes) - ↓ - Hardware (UART) +User / C / Python / ROS2 + ↓ FFI + DeviceWrapper (handle) + ↓ + EskinDeviceInner + ↓ read_register / write_register / start_stream / read_sample + EskinProtocolCodec (encode/decode + CRC8) + ↓ + SerialTransport (write/read bytes) + ↓ + Hardware (UART) ``` 详细设计见 `docs/PROGRESS.md` 和 `docs/ARCHITECTURE.md`。 diff --git a/example/cpp/main.cpp b/example/cpp/main.cpp index 5cf35f7..b4116d8 100644 --- a/example/cpp/main.cpp +++ b/example/cpp/main.cpp @@ -1,49 +1,148 @@ -#include "../../include/eskin_ffi.h" +#include +#include #include #include #include +#include +#include +#include +#include -int main() { - printf("ESkin SDK version: %u.%u.%u\n", - eskin_version().major, eskin_version().minor, eskin_version().patch); +#include "../../include/eskin_ffi.h" - EskinDeviceHandle dev = eskin_open("/dev/ttyUSB0", nullptr); - if (!dev) { - printf("Failed to open device\n"); - return 1; +using namespace std::chrono_literals; + +// ── Command 模式：读取设备信息 ───────────────────────────── + +static void demo_command_mode(EskinDeviceHandle device) { + printf("=== Command Mode ===\n"); + + // 硬件版本 + char hw_buf[64] = {}; + uint32_t hw_len = 0; + if (eskin_read_hdw_version(device, hw_buf, sizeof(hw_buf), &hw_len) == ESkinSuccess) { + printf("Hardware version: %.*s\n", (int)hw_len, hw_buf); } - printf("Device opened\n"); - uint8_t buf[256]; + // 矩阵尺寸 + uint8_t row = 0, col = 0; + if (eskin_read_matrix_row(device, &row) == ESkinSuccess && + eskin_read_matrix_col(device, &col) == ESkinSuccess) { + printf("Matrix size: %u x %u\n", row, col); + } + + // 设备配置 + uint8_t cfg1 = 0; + if (eskin_read_device_config1(device, &cfg1) == ESkinSuccess) { + printf("Device config1: 0x%02X\n", cfg1); + } + + // 序列号（原始寄存器读取） + uint8_t buf[256] = {}; uint32_t actual = 0; - EskinSdkErrorCode err = eskin_read_register(dev, 0x0000, 4, buf, sizeof(buf), &actual); - if (err == ESkinSuccess) { - printf("Serial number (%u bytes): ", actual); - for (uint32_t i = 0; i < actual; i++) { - printf("%02X ", buf[i]); - } - - printf("\n"); - } - else { - printf("read_register failed: %d\n", err); - } - - err = eskin_read_register(dev, 0x000F, 2, buf, sizeof(buf), &actual); - if (err == ESkinSuccess) { - printf("Firmware version (%u bytes): ", actual); + if (eskin_read_register(device, 0x1C00, 168, buf, sizeof(buf), &actual) == ESkinSuccess) { + printf("Serial number (raw): "); for (uint32_t i = 0; i < actual; i++) { printf("%02X", buf[i]); } printf("\n"); } +} - err = eskin_read_register(dev, 0x0500, 168, buf, sizeof(buf), &actual); - if (err == ESkinSuccess) { - printf("Combined force raw (%u bytes)\n"); +// ── Streaming 模式：持续采集力数据 ──────────────────────── + +static void demo_streaming(EskinDeviceHandle device, double duration_sec = 5.0) { + printf("\n=== Streaming Mode ===\n"); + + auto err = eskin_start_stream(device); + if (err != ESkinSuccess) { + printf("Failed to start stream, error: %d\n", (int)err); + return; + } + printf("Streaming started, will run for %.1fs ...\n", duration_sec); + + // 线程安全队列（参考 ROS publisher 的 read_loop + publish_callback 分离模式） + std::mutex mtx; + std::queue queue; + bool running = true; + + // 读取线程：持续从设备读取 sample 放入队列 + std::thread read_thread([&]() { + while (running) { + CFingerSample sample; + memset(&sample, 0, sizeof(sample)); + auto e = eskin_read_sample(device, 50, &sample); + if (e == ESkinSuccess) { + std::lock_guard lock(mtx); + queue.push(sample); + while (queue.size() > 100) { + queue.pop(); // 防止堆积 + } + } + // 超时等非致命错误忽略，继续读取 + } + }); + + // 主线程：从队列取数据并打印（类似 ROS 的 publish_callback） + auto start = std::chrono::steady_clock::now(); + int count = 0; + + while (true) { + auto now = std::chrono::steady_clock::now(); + double elapsed = std::chrono::duration(now - start).count(); + if (elapsed >= duration_sec) break; + + { + std::lock_guard lock(mtx); + while (!queue.empty()) { + const auto &s = queue.front(); + printf("[%5u] module=%u fx=%u fy=%u fz=%u\n", + s.sequence, s.combined_force.module, + s.combined_force.force.fx, + s.combined_force.force.fy, + s.combined_force.force.fz); + queue.pop(); + count++; + } + } + + std::this_thread::sleep_for(5ms); } - eskin_close(dev); + running = false; + read_thread.join(); + eskin_stop_stream(device); + printf("Streaming stopped. Total samples: %d\n", count); +} + +// ── Main ────────────────────────────────────────────────── + +int main(int argc, char *argv[]) { + std::string device_path = "/dev/ttyUSB0"; + if (argc > 1) { + device_path = argv[1]; + } + + // SDK 版本 + auto ver = eskin_version(); + printf("ESkin SDK version: %u.%u.%u\n", ver.major, ver.minor, ver.patch); + + // 打开设备 + EskinDeviceHandle device = eskin_open(device_path.c_str(), nullptr); + if (!device) { + fprintf(stderr, "Failed to open device: %s\n", device_path.c_str()); + return 1; + } + printf("Device opened: %s\n", device_path.c_str()); + + // Command 模式演示 + demo_command_mode(device); + + // Streaming 模式演示 + demo_streaming(device, 5.0); + + // 关闭设备 + eskin_close(device); printf("Device closed\n"); return 0; } \ No newline at end of file diff --git a/example/python/eskin_ffi.py b/example/python/eskin_ffi.py index 3e655da..d257725 100644 --- a/example/python/eskin_ffi.py +++ b/example/python/eskin_ffi.py @@ -1,7 +1,7 @@ import ctypes from ctypes import ( Structure, POINTER, c_void_p, c_char, c_char_p, c_uint8, c_uint16, - c_uint32, c_uint64, c_int16, c_bool + c_uint32, c_uint64, c_bool ) LIB_PATH = "./libeskin_finger_sdk.so" @@ -14,6 +14,29 @@ class EskinSdkVersion(Structure): ] +class CForce3D(Structure): + _fields_ = [ + ("fx", c_uint32), + ("fy", c_uint32), + ("fz", c_uint32), + ] + + +class CCombinedForce(Structure): + _fields_ = [ + ("module", c_uint32), + ("force", CForce3D), + ] + + +class CFingerSample(Structure): + _fields_ = [ + ("timestamp_us", c_uint64), + ("sequence", c_uint32), + ("combined_force", CCombinedForce), + ] + + class EskinDevice: def __init__(self): self._lib = ctypes.CDLL(LIB_PATH) @@ -90,6 +113,22 @@ class EskinDevice: c_void_p, c_uint8, POINTER(c_uint16) ] + # Streaming bindings + + lib.eskin_start_stream.restype = c_uint32 + lib.eskin_start_stream.argtypes = [c_void_p] + + lib.eskin_stop_stream.restype = c_uint32 + lib.eskin_stop_stream.argtypes = [c_void_p] + + lib.eskin_read_sample.restype = c_uint32 + lib.eskin_read_sample.argtypes = [ + c_void_p, c_uint32, POINTER(CFingerSample) + ] + + lib.eskin_get_mode.restype = c_uint32 + lib.eskin_get_mode.argtypes = [c_void_p, POINTER(c_uint32)] + def version(self) -> tuple: v = self._lib.eskin_version() return (v.major, v.minor, v.patch) @@ -210,8 +249,38 @@ class EskinDevice: raise RuntimeError(f"write_matrix_col failed: error={err}") return ret.value + # Streaming methods + + def start_stream(self): + """启动流式采集""" + err = self._lib.eskin_start_stream(self._handle) + if err != 0: + raise RuntimeError(f"start_stream failed: error={err}") + + def stop_stream(self): + """停止流式采集""" + err = self._lib.eskin_stop_stream(self._handle) + if err != 0: + raise RuntimeError(f"stop_stream failed: error={err}") + + def read_sample(self, timeout_ms: int = 200) -> CFingerSample: + """读取一个采样数据（流模式下调用）""" + sample = CFingerSample() + err = self._lib.eskin_read_sample(self._handle, timeout_ms, ctypes.byref(sample)) + if err != 0: + raise RuntimeError(f"read_sample failed: error={err}") + return sample + + def get_mode(self) -> int: + """查询当前设备模式（0=Command, 1=Streaming）""" + out = c_uint32(0) + err = self._lib.eskin_get_mode(self._handle, ctypes.byref(out)) + if err != 0: + raise RuntimeError(f"get_mode failed: error={err}") + return out.value + def __enter__(self): return self def __exit__(self, *args): - self.close() + self.close() \ No newline at end of file diff --git a/example/python/example.py b/example/python/example.py index 60b2e7e..5e5b7ee 100644 --- a/example/python/example.py +++ b/example/python/example.py @@ -1,51 +1,92 @@ -from eskin_ffi import EskinDevice +import time +import threading +from collections import deque + +from eskin_ffi import EskinDevice, CFingerSample + + +def demo_command_mode(dev: EskinDevice): + """Command 模式：读取设备信息、寄存器等""" + print("=== Command Mode ===") + + hdw_ver = dev.read_hdw_version() + print(f"Hardware version: {hdw_ver}") + + row = dev.read_matrix_row() + col = dev.read_matrix_col() + print(f"Matrix size: {row} x {col}") + + cfg1 = dev.read_device_config1() + print(f"Device config1: 0x{cfg1:02X}") + + data = dev.read_register(0x1C00, 168) + print(f"Serial number: {data.hex().upper()}") + + +def demo_streaming(dev: EskinDevice, duration_sec: float = 5.0): + """Streaming 模式：持续采集力数据（参考 ROS C++ publisher）""" + print("\n=== Streaming Mode ===") + + # 启动流式采集 + dev.start_stream() + print(f"Streaming started, will run for {duration_sec}s ...") + + # 线程安全的队列（参考 ROS demo 的 read_loop + publish_callback 分离模式） + queue: deque = deque(maxlen=100) + running = True + + def read_loop(): + """独立读取线程：持续从设备读取 sample""" + while running: + try: + sample = dev.read_sample(timeout_ms=50) + queue.append(sample) + except RuntimeError: + # 超时等非致命错误，继续读取 + pass + + # 启动读取线程 + reader = threading.Thread(target=read_loop, daemon=True) + reader.start() + + # 主线程：从队列取数据并打印（类似 ROS 的 publish_callback） + start = time.monotonic() + count = 0 + while time.monotonic() - start < duration_sec: + if queue: + sample: CFingerSample = queue.popleft() + f = sample.combined_force.force + mod = sample.combined_force.module + print( + f"[{sample.sequence:5d}] " + f"module={mod} " + f"fx={f.fx} fy={f.fy} fz={f.fz}" + ) + count += 1 + else: + time.sleep(0.005) + + running = False + reader.join(timeout=1.0) + dev.stop_stream() + print(f"Streaming stopped. Total samples: {count}") + def main(): - dev = EskinDevice() - - # SDK 版本 - ver = dev.version() + ver = EskinDevice().version() print(f"ESkin SDK version: {ver[0]}.{ver[1]}.{ver[2]}") - # 打开设备 - dev.open("/dev/ttyUSB0") - print("Device opened") + device_path = "/dev/ttyUSB0" - try: - # 读取硬件版本 - hdw_ver = dev.read_hdw_version() - print(f"Hardware version: {hdw_ver}") + with EskinDevice() as dev: + dev.open(device_path) + print(f"Device opened: {device_path}") - # 读取矩阵尺寸 - row = dev.read_matrix_row() - col = dev.read_matrix_col() - print(f"Matrix size: {row} x {col}") + demo_command_mode(dev) + demo_streaming(dev, duration_sec=5.0) - # 读取设备配置 - cfg1 = dev.read_device_config1() - # cfg2 = dev.read_device_config2() - print(f"Device config1: 0x{cfg1:02X}") - # print(f"Device config2: 0x{cfg2:02X}") + print("Device closed") - # 写入矩阵尺寸示例 - # ret = dev.write_matrix_row(16) - # print(f"Write matrix row: returned {ret} bytes") - # ret = dev.write_matrix_col(16) - # print(f"Write matrix col: returned {ret} bytes") - - # 写入设备配置示例 - # ret = dev.write_device_config1(True) - # print(f"Write device config1: returned {ret} bytes") - # ret = dev.write_device_config2(False) - # print(f"Write device config2: returned {ret} bytes") - - # 原始寄存器读写 - data = dev.read_register(0x1C00, 168) - print(f"Serial number: {data.hex().upper()}") - - finally: - dev.close() - print("Device closed") if __name__ == "__main__": main() \ No newline at end of file diff --git a/example/ros-cpp/CMakeLists.txt b/example/ros-cpp/CMakeLists.txt new file mode 100644 index 0000000..5baef3f --- /dev/null +++ b/example/ros-cpp/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.8) +project(eskin_ros2_demo) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Force use of system Python (avoid Conda Python interfering with ROS 2) +set(Python3_EXECUTABLE /usr/bin/python3 CACHE FILEPATH "System Python3" FORCE) + +# Ensure ROS 2 Python packages are findable even without ROS 2 sourced (e.g. VS Code CMake Tools) +set(ENV{PYTHONPATH} "/opt/ros/jazzy/lib/python3.12/site-packages:$ENV{PYTHONPATH}") + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Eskin SDK library +set(ESKIN_SDK_DIR "/home/lenn/Workspace/eskin-finger-sdk" CACHE PATH "Path to eskin-finger-sdk") +add_library(eskin_finger_sdk SHARED IMPORTED) +set_target_properties(eskin_finger_sdk PROPERTIES + IMPORTED_LOCATION "${ESKIN_SDK_DIR}/target/release/libeskin_finger_sdk.so" + INTERFACE_INCLUDE_DIRECTORIES "${CMAKE_CURRENT_SOURCE_DIR}/include" +) + +add_executable(eskin_publisher src/eskin_publisher.cpp) +ament_target_dependencies(eskin_publisher rclcpp std_msgs) +target_link_libraries(eskin_publisher eskin_finger_sdk pthread) + +add_executable(eskin_subscriber src/eskin_subscriber.cpp) +ament_target_dependencies(eskin_subscriber rclcpp std_msgs) + +install(TARGETS + eskin_publisher + eskin_subscriber + DESTINATION lib/${PROJECT_NAME} +) +ament_package() diff --git a/example/ros-cpp/eskin_publisher.cpp b/example/ros-cpp/eskin_publisher.cpp new file mode 100644 index 0000000..ef322b8 --- /dev/null +++ b/example/ros-cpp/eskin_publisher.cpp @@ -0,0 +1,130 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/u_int32.hpp" + +#include "../include/eskin_ffi.h" + +using namespace std::chrono_literals; + +class EskinPublisher : public rclcpp::Node { +public: + EskinPublisher(const std::string & device_path) + : Node("eskin_publisher"), running_(true) + { + // 打开设备 + device_ = eskin_open(device_path.c_str(), nullptr); + if (!device_) { + RCLCPP_FATAL(this->get_logger(), "Failed to open device: %s", device_path.c_str()); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(this->get_logger(), "Device opened: %s", device_path.c_str()); + + // 启动 streaming + auto err = eskin_start_stream(device_); + if (err != ESkinSuccess) { + RCLCPP_FATAL(this->get_logger(), "Failed to start stream, error: %d", (int)err); + eskin_close(device_); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(this->get_logger(), "Streaming started"); + + // 创建 publisher + publisher_ = this->create_publisher("comb_force", 10); + + // 启动独立读取线程 + read_thread_ = std::thread(&EskinPublisher::read_loop, this); + + // 10ms 定时器：从队列取数据并发布 + timer_ = this->create_wall_timer(10ms, std::bind(&EskinPublisher::publish_callback, this)); + + RCLCPP_INFO(this->get_logger(), "EskinPublisher setup success"); + } + + ~EskinPublisher() { + // 停止读取线程 + running_ = false; + cv_.notify_all(); + if (read_thread_.joinable()) { + read_thread_.join(); + } + + // 停止 streaming 并关闭设备 + if (device_) { + eskin_stop_stream(device_); + eskin_close(device_); + } + RCLCPP_INFO(this->get_logger(), "EskinPublisher destroyed"); + } + +private: + // 独立读取线程：持续从设备读取 sample 放入队列 + void read_loop() { + while (running_) { + CFingerSample sample; + memset(&sample, 0, sizeof(sample)); + EskinSdkErrorCode err = eskin_read_sample(device_, 50, &sample); + if (err == ESkinSuccess) { + std::lock_guard lock(queue_mutex_); + sample_queue_.push(sample); + // 限制队列大小，防止堆积 + while (sample_queue_.size() > 100) { + sample_queue_.pop(); + } + } else if (err != ESkinTimeout) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "eskin_read_sample error: %d", (int)err); + } + } + } + + // 定时器回调：从队列取数据并发布到 ROS topic + void publish_callback() { + std::lock_guard lock(queue_mutex_); + while (!sample_queue_.empty()) { + const auto & sample = sample_queue_.front(); + auto msg = std_msgs::msg::UInt32(); + // 使用 combined_force 中的 fz（法向力）作为发布值 + msg.data = static_cast(sample.combined_force.force.fz); + publisher_->publish(msg); + sample_queue_.pop(); + } + } + + bool running_; + EskinDeviceHandle device_; + std::thread read_thread_; + std::mutex queue_mutex_; + std::queue sample_queue_; + std::condition_variable cv_; + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; +}; + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + + // 设备路径可通过命令行参数传入，默认 /dev/ttyUSB0 + std::string device_path = "/dev/ttyUSB0"; + if (argc > 1) { + device_path = argv[1]; + } + + auto node = std::make_shared(device_path); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/example/ros-cpp/eskin_subscriber.cpp b/example/ros-cpp/eskin_subscriber.cpp new file mode 100644 index 0000000..4a21df6 --- /dev/null +++ b/example/ros-cpp/eskin_subscriber.cpp @@ -0,0 +1,32 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/u_int32.hpp" + +using std::placeholders::_1; + +class EskinSubscriber : public rclcpp::Node { +public: + EskinSubscriber() + : Node("eskin_subscriber") + { + subscription_ = this->create_subscription( + "comb_force", 10, + std::bind(&EskinSubscriber::topic_callback, this, _1)); + RCLCPP_INFO(this->get_logger(), "EskinSubscriber listening on /comb_force"); + } + +private: + void topic_callback(const std_msgs::msg::UInt32::SharedPtr msg) const { + RCLCPP_INFO(this->get_logger(), "Received force: %u", msg->data); + } + + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/example/ros-cpp/package.xml b/example/ros-cpp/package.xml new file mode 100644 index 0000000..b3eef3c --- /dev/null +++ b/example/ros-cpp/package.xml @@ -0,0 +1,21 @@ + + + + eskin_ros2_demo + 0.0.0 + TODO: Package description + lenn + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/include/eskin_ffi.h b/include/eskin_ffi.h index b2deb1f..87db4fa 100644 --- a/include/eskin_ffi.h +++ b/include/eskin_ffi.h @@ -113,8 +113,32 @@ EskinSdkErrorCode eskin_write_matrix_col( uint16_t* return_count ); +// Streaming interfaces + +typedef struct { + uint32_t fx; + uint32_t fy; + uint32_t fz; +} CForce3D; + +typedef struct { + uint32_t module; + CForce3D force; +} CCombinedForce; + +typedef struct { + uint64_t timestamp_us; + uint32_t sequence; + CCombinedForce combined_force; +} CFingerSample; + +EskinSdkErrorCode eskin_start_stream(EskinDeviceHandle handle); +EskinSdkErrorCode eskin_stop_stream(EskinDeviceHandle handle); +EskinSdkErrorCode eskin_read_sample(EskinDeviceHandle handle, uint32_t timeout_ms, CFingerSample* out); +EskinSdkErrorCode eskin_get_mode(EskinDeviceHandle handle, uint32_t* out); + #ifdef __cplusplus } #endif -#endif \ No newline at end of file +#endif diff --git a/src/ffi/mod.rs b/src/ffi/mod.rs index 7ee86f0..cb99b26 100644 --- a/src/ffi/mod.rs +++ b/src/ffi/mod.rs @@ -1,8 +1,8 @@ use std::{ptr}; use std::ffi::{CStr, c_char}; -use crate::device::EskinDevice; -use crate::device::EskinDeviceFunc; +use crate::device::{DeviceMode, EskinDevice, EskinDeviceFunc}; use crate::transport::SerialPortTransport; +use crate::types::{CombinedForce, FingerSample}; use crate::{config::DeviceConfig, device::EskinDeviceInner, error::SdkErrorCode}; pub type EskinDeviceHandle = *mut core::ffi::c_void; @@ -16,20 +16,31 @@ pub struct EskinSdkVersion { } #[repr(C)] +#[derive(Debug, Clone, Copy)] +pub struct CForce3D { + pub fx: u32, + pub fy: u32, + pub fz: u32, +} + +#[repr(C)] +#[derive(Debug, Clone, Copy)] +pub struct CCombinedForce { + pub module: u32, + pub force: CForce3D, +} + +#[repr(C)] +#[derive(Debug, Clone, Copy)] pub struct CFingerSample { pub timestamp_us: u64, pub sequence: u32, - pub combinded_force_raw: *const u8, - pub combinded_force_len: u32, - pub module_error_raw: *const u8, - pub module_error_len: u32, + pub combined_force: CCombinedForce, } #[allow(dead_code)] struct DeviceWrapper { device: EskinDeviceInner, - last_cf_raw: Vec, - last_me_raw: Vec } fn sdk_error_to_code(err: crate::error::SdkError) -> SdkErrorCode { @@ -91,8 +102,6 @@ pub unsafe extern "C" fn eskin_open( let wrapper = Box::new(DeviceWrapper { device, - last_cf_raw: Vec::new(), - last_me_raw: Vec::new(), }); Box::into_raw(wrapper) as EskinDeviceHandle @@ -371,3 +380,95 @@ pub unsafe extern "C" fn eskin_write_matrix_col( Err(e) => sdk_error_to_code(e), } } + +/// 启动流式采集 +#[unsafe(no_mangle)] +pub unsafe extern "C" fn eskin_start_stream(handle: EskinDeviceHandle) -> SdkErrorCode { + if handle.is_null() { + return SdkErrorCode::InvalidPointer; + } + + let wrapper = unsafe { &mut *(handle as *mut DeviceWrapper) }; + + match wrapper.device.start_stream() { + Ok(()) => SdkErrorCode::Success, + Err(e) => sdk_error_to_code(e), + } +} + +/// 停止流式采集 +#[unsafe(no_mangle)] +pub unsafe extern "C" fn eskin_stop_stream(handle: EskinDeviceHandle) -> SdkErrorCode { + if handle.is_null() { + return SdkErrorCode::InvalidPointer; + } + + let wrapper = unsafe { &mut *(handle as *mut DeviceWrapper) }; + + match wrapper.device.stop_stream() { + Ok(()) => SdkErrorCode::Success, + Err(e) => sdk_error_to_code(e), + } +} + +/// 读取一个采样数据（流模式下调用） +#[unsafe(no_mangle)] +pub unsafe extern "C" fn eskin_read_sample( + handle: EskinDeviceHandle, + timeout_ms: u32, + out: *mut CFingerSample, +) -> SdkErrorCode { + if handle.is_null() || out.is_null() { + return SdkErrorCode::InvalidPointer; + } + + let wrapper = unsafe { &mut *(handle as *mut DeviceWrapper) }; + + match wrapper.device.read_sample(timeout_ms) { + Ok(sample) => { + let c_sample = finger_sample_to_c(&sample); + unsafe { *out = c_sample }; + SdkErrorCode::Success + } + Err(e) => sdk_error_to_code(e), + } +} + +/// 查询当前设备模式（Command=0, Streaming=1） +#[unsafe(no_mangle)] +pub unsafe extern "C" fn eskin_get_mode( + handle: EskinDeviceHandle, + out: *mut u32, +) -> SdkErrorCode { + if handle.is_null() || out.is_null() { + return SdkErrorCode::InvalidPointer; + } + + let wrapper = unsafe { &*(handle as *const DeviceWrapper) }; + + let mode_val = match wrapper.device.mode() { + DeviceMode::Command => 0u32, + DeviceMode::Streaming => 1u32, + }; + unsafe { *out = mode_val }; + SdkErrorCode::Success +} + +fn finger_sample_to_c(sample: &FingerSample) -> CFingerSample { + CFingerSample { + timestamp_us: sample.timestamp_us, + sequence: sample.sequence, + combined_force: combined_force_to_c(&sample.combined_forces), + } +} + +fn combined_force_to_c(cf: &CombinedForce) -> CCombinedForce { + CCombinedForce { + module: cf.module as u32, + force: CForce3D { + fx: cf.force.fx, + fy: cf.force.fy, + fz: cf.force.fz, + }, + } +} diff --git a/src/main.rs b/src/main.rs index 507fdd3..d15fed2 100644 --- a/src/main.rs +++ b/src/main.rs @@ -89,11 +89,11 @@ fn stream_demo() { match device.read_sample(200) { Ok(sample) => { count += 1; - if count % 100 == 0 { + if count % 5 == 0 { println!( - "[#{count} seq={}] combined_forces[0..3]={:?}", + "[#{count} seq={}] combined_force={:?}", sample.sequence, - &sample.combined_forces[..3.min(sample.combined_forces.len())] + sample.combined_forces ); } } diff --git a/src/register.rs b/src/register.rs index d648408..4667b75 100644 --- a/src/register.rs +++ b/src/register.rs @@ -147,32 +147,42 @@ impl RegisterMap for EskinRegisterMap { } -pub fn parse_combined_forces(raw: &[u8]) -> Result, SdkError> { - const MODULE_COUNT: usize = 28; - const BYTES_PER_MODULE: usize = 6; +pub fn parse_combined_forces(raw: &[u8], addr: u32) -> Result { + // println!("{:02X?}", raw); + // const MODULE_COUNT: usize = 28; + // const BYTES_PER_MODULE: usize = 6; - if raw.len() < MODULE_COUNT * BYTES_PER_MODULE { - return Err(SdkError::FrameError(format!( - "combined force raw too short: expected {} bytes, got {}", - MODULE_COUNT * BYTES_PER_MODULE, - raw.len() - ))); - } + // if raw.len() < MODULE_COUNT * BYTES_PER_MODULE { + // return Err(SdkError::FrameError(format!( + // "combined force raw too short: expected {} bytes, got {}", + // MODULE_COUNT * BYTES_PER_MODULE, + // raw.len() + // ))); + // } - let mut forces = Vec::with_capacity(MODULE_COUNT); - for i in 0..MODULE_COUNT { - let offset = i * BYTES_PER_MODULE; - let fx = i16::from_le_bytes([raw[offset], raw[offset + 1]]); - let fy = i16::from_le_bytes([raw[offset + 2], raw[offset + 3]]); - let fz = i16::from_le_bytes([raw[offset + 4], raw[offset + 5]]); + // let mut forces = Vec::with_capacity(MODULE_COUNT); + // for i in 0..MODULE_COUNT { + // let offset = i * BYTES_PER_MODULE; + // let fx = i16::from_le_bytes([raw[offset], raw[offset + 1]]); + // let fy = i16::from_le_bytes([raw[offset + 2], raw[offset + 3]]); + // let fz = i16::from_le_bytes([raw[offset + 4], raw[offset + 5]]); - forces.push(CombinedForce { - module: SensorModule::from_index(i as u8), - force: Force3D { fx, fy, fz }, - }); - } + // forces.push(CombinedForce { + // module: SensorModule::from_index(i as u8), + // force: Force3D { fx, fy, fz }, + // }); + // } + let comb_force: u32 = raw + .chunks(2) + .map(|ch| u16::from_le_bytes([ch[0], ch[1]]) as u32) + .sum(); + let force = CombinedForce { + module: addr.into(), + force: Force3D { fx: 0, fy: 0, fz: comb_force } + }; - Ok(forces) + + Ok(force) } pub fn parse_module_errors(raw: &[u8]) -> Result, SdkError> { diff --git a/src/stream.rs b/src/stream.rs index df7122d..360df89 100644 --- a/src/stream.rs +++ b/src/stream.rs @@ -298,7 +298,6 @@ impl PollingSampleCollector { }; let request_frame = self.codec.encode_read_request(&request)?; - println!("streaming send: {:02X?}", request_frame); let response_frame = { let mut transport = self .transport @@ -320,10 +319,8 @@ impl SampleCollector for PollingSampleCollector { let sequence = self.next_sequence(); let combined_force_raw = self.read_register(self.config.finger_addr, 168)?; - let module_error_raw = self.read_register(REG_MODULE_ERROR, 56)?; - let combined_forces = crate::register::parse_combined_forces(&combined_force_raw)?; - let module_errors = crate::register::parse_module_errors(&module_error_raw)?; + let combined_forces = crate::register::parse_combined_forces(&combined_force_raw, self.config.finger_addr)?; let now = chrono::Utc::now().timestamp_micros() as u64; @@ -331,8 +328,8 @@ impl SampleCollector for PollingSampleCollector { timestamp_us: now, sequence, combined_forces, - distribution_forces: Vec::new(), - module_errors + // distribution_forces: Vec::new(), + // module_errors }; Ok(Some(sample)) diff --git a/src/types.rs b/src/types.rs index b982296..7990fdf 100644 --- a/src/types.rs +++ b/src/types.rs @@ -3,9 +3,9 @@ use serde::{Deserialize, Serialize}; #[repr(C)] #[derive(Debug, Clone, Copy, Default, Serialize, Deserialize)] pub struct Force3D { - pub fx: i16, - pub fy: i16, - pub fz: i16, + pub fx: u32, + pub fy: u32, + pub fz: u32, } #[repr(C)] @@ -90,6 +90,34 @@ impl SensorModule { } } +impl From for SensorModule { + fn from(value: u32) -> Self { + match value { + 0x1000 => SensorModule::ThumbProximal, + 0x1200 => SensorModule::ThumbMiddle, + 0x1400 => SensorModule::ThumbTip, + 0x1600 => SensorModule::ThumbNail, + 0x1800 => SensorModule::IndexProximal, + 0x1A00 => SensorModule::IndexMiddle, + 0x1C00 => SensorModule::IndexTip, + 0x1E00 => SensorModule::IndexNail, + 0x2000 => SensorModule::MiddleProximal, + 0x2200 => SensorModule::MiddleMiddle, + 0x2400 => SensorModule::MiddleTip, + 0x2600 => SensorModule::MiddleNail, + 0x2800 => SensorModule::RingProximal, + 0x2A00 => SensorModule::RingMiddle, + 0x2C00 => SensorModule::RingTip, + 0x2E00 => SensorModule::RingNail, + 0x3000 => SensorModule::PinkyProximal, + 0x3200 => SensorModule::PinkyMiddle, + 0x3400 => SensorModule::PinkyTip, + 0x3600 => SensorModule::PinkyNail, + _ => SensorModule::IndexTip, + } + } +} + pub const SENSOR_MODULE_COUNT: usize = 28; @@ -113,9 +141,9 @@ pub struct CombinedForce { pub struct FingerSample { pub timestamp_us: u64, pub sequence: u32, - pub combined_forces: Vec, - pub distribution_forces: Vec, - pub module_errors: Vec, + pub combined_forces: CombinedForce, + // pub distribution_forces: Vec, + // pub module_errors: Vec, } #[repr(C)]