feat: update examples and README with streaming support and uint32 force types

- Change CForce3D fx/fy/fz from int16 to uint32 to match hardware
- Add independent C++ example with command and streaming modes
- Rewrite Python example with threaded streaming (read_loop + consumer pattern)
- Add ROS2 C++ publisher/subscriber examples
- Update README with streaming APIs, ROS2 docs, data type definitions, and full FFI table
- Add CHANGELOG
This commit is contained in:
lenn
2026-05-08 17:41:46 +08:00
parent c195234771
commit 705375085f
15 changed files with 903 additions and 149 deletions

56
CHANGELOG.md Normal file
View File

@@ -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 可能在后续版本中调整

134
README.md
View File

@@ -1,6 +1,6 @@
# Eskin Finger SDK # 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++ 头文件 eskin_ffi.h — C/C++ 头文件
example/ example/
cpp/main.cpp C++ 使用示例 cpp/main.cpp — 独立 C++ 使用示例(含流式采集)
python/ — Python 使用示例 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(); 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++ ## 使用 C/C++
@@ -69,26 +85,46 @@ cargo build --release
### 编译 C++ 示例 ### 编译 C++ 示例
```bash ```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 LD_LIBRARY_PATH=target/release ./example_cpp
``` ```
### C++ 代码示例 ### C++ 代码示例
完整示例见 `example/cpp/main.cpp`,包含 Command 模式和 Streaming 模式:
```cpp ```cpp
#include "eskin_ffi.h" #include "eskin_ffi.h"
#include <cstdio> #include <cstdio>
#include <cstring>
#include <thread>
#include <mutex>
#include <queue>
int main() { int main() {
// 打开设备
EskinDeviceHandle dev = eskin_open("/dev/ttyUSB0", nullptr); EskinDeviceHandle dev = eskin_open("/dev/ttyUSB0", nullptr);
if (!dev) return 1; if (!dev) return 1;
uint8_t buf[256]; // Command 模式:读取设备信息
uint32_t actual; char hw_buf[64] = {};
if (eskin_read_register(dev, 0x0000, 4, buf, sizeof(buf), &actual) == ESkinSuccess) { uint32_t hw_len = 0;
printf("Serial: %02X %02X %02X %02X\n", buf[0], buf[1], buf[2], buf[3]); 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); eskin_close(dev);
return 0; return 0;
} }
@@ -108,26 +144,80 @@ cd example/python
python3 example.py 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 ```python
from eskin_ffi import EskinDevice from eskin_ffi import EskinDevice
with EskinDevice("libeskin_finger_sdk.so") as dev: with EskinDevice() as dev:
dev.open("/dev/ttyUSB0") dev.open("/dev/ttyUSB0")
# 读取硬件版本 # Command 模式:读取设备信息
print(f"Hardware version: {dev.read_hdw_version()}") print(f"Hardware version: {dev.read_hdw_version()}")
# 读取矩阵尺寸
print(f"Matrix: {dev.read_matrix_row()} x {dev.read_matrix_col()}") print(f"Matrix: {dev.read_matrix_row()} x {dev.read_matrix_col()}")
# 读寄存器 # Streaming 模式:持续采集力数据
data = dev.read_register(0x0000, 4) dev.start_stream()
print(f"Serial: {data.hex()}") 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_device_config2(handle, enable, return_count)` | 写入设备配置寄存器2 |
| `eskin_write_matrix_row(handle, row, return_count)` | 写入矩阵行数 | | `eskin_write_matrix_row(handle, row, return_count)` | 写入矩阵行数 |
| `eskin_write_matrix_col(handle, col, 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,12 +303,12 @@ cargo test protocol::tests # 仅协议层
## 架构 ## 架构
```text ```text
User / C / Python User / C / Python / ROS2
↓ FFI ↓ FFI
DeviceWrapper (handle) DeviceWrapper (handle)
EskinDeviceInner EskinDeviceInner
↓ read_register / write_register ↓ read_register / write_register / start_stream / read_sample
EskinProtocolCodec (encode/decode + CRC8) EskinProtocolCodec (encode/decode + CRC8)
SerialTransport (write/read bytes) SerialTransport (write/read bytes)

View File

@@ -1,49 +1,148 @@
#include "../../include/eskin_ffi.h" #include <chrono>
#include <condition_variable>
#include <cstdint> #include <cstdint>
#include <cstdio> #include <cstdio>
#include <cstring> #include <cstring>
#include <mutex>
#include <queue>
#include <string>
#include <thread>
int main() { #include "../../include/eskin_ffi.h"
printf("ESkin SDK version: %u.%u.%u\n",
eskin_version().major, eskin_version().minor, eskin_version().patch);
EskinDeviceHandle dev = eskin_open("/dev/ttyUSB0", nullptr); using namespace std::chrono_literals;
if (!dev) {
printf("Failed to open device\n"); // ── 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);
}
// 矩阵尺寸
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;
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");
}
}
// ── 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<CFingerSample> 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<std::mutex> 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<double>(now - start).count();
if (elapsed >= duration_sec) break;
{
std::lock_guard<std::mutex> 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);
}
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; return 1;
} }
printf("Device opened\n"); printf("Device opened: %s\n", device_path.c_str());
uint8_t buf[256]; // Command 模式演示
uint32_t actual = 0; demo_command_mode(device);
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"); // Streaming 模式演示
} demo_streaming(device, 5.0);
else {
printf("read_register failed: %d\n", err);
}
err = eskin_read_register(dev, 0x000F, 2, buf, sizeof(buf), &actual); // 关闭设备
if (err == ESkinSuccess) { eskin_close(device);
printf("Firmware version (%u bytes): ", actual);
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");
}
eskin_close(dev);
printf("Device closed\n"); printf("Device closed\n");
return 0; return 0;
} }

View File

@@ -1,7 +1,7 @@
import ctypes import ctypes
from ctypes import ( from ctypes import (
Structure, POINTER, c_void_p, c_char, c_char_p, c_uint8, c_uint16, 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" 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: class EskinDevice:
def __init__(self): def __init__(self):
self._lib = ctypes.CDLL(LIB_PATH) self._lib = ctypes.CDLL(LIB_PATH)
@@ -90,6 +113,22 @@ class EskinDevice:
c_void_p, c_uint8, POINTER(c_uint16) 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: def version(self) -> tuple:
v = self._lib.eskin_version() v = self._lib.eskin_version()
return (v.major, v.minor, v.patch) return (v.major, v.minor, v.patch)
@@ -210,6 +249,36 @@ class EskinDevice:
raise RuntimeError(f"write_matrix_col failed: error={err}") raise RuntimeError(f"write_matrix_col failed: error={err}")
return ret.value 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): def __enter__(self):
return self return self

View File

@@ -1,51 +1,92 @@
from eskin_ffi import EskinDevice import time
import threading
from collections import deque
def main(): from eskin_ffi import EskinDevice, CFingerSample
dev = EskinDevice()
# SDK 版本
ver = dev.version()
print(f"ESkin SDK version: {ver[0]}.{ver[1]}.{ver[2]}")
# 打开设备 def demo_command_mode(dev: EskinDevice):
dev.open("/dev/ttyUSB0") """Command 模式:读取设备信息、寄存器等"""
print("Device opened") print("=== Command Mode ===")
try:
# 读取硬件版本
hdw_ver = dev.read_hdw_version() hdw_ver = dev.read_hdw_version()
print(f"Hardware version: {hdw_ver}") print(f"Hardware version: {hdw_ver}")
# 读取矩阵尺寸
row = dev.read_matrix_row() row = dev.read_matrix_row()
col = dev.read_matrix_col() col = dev.read_matrix_col()
print(f"Matrix size: {row} x {col}") print(f"Matrix size: {row} x {col}")
# 读取设备配置
cfg1 = dev.read_device_config1() cfg1 = dev.read_device_config1()
# cfg2 = dev.read_device_config2()
print(f"Device config1: 0x{cfg1:02X}") print(f"Device config1: 0x{cfg1:02X}")
# print(f"Device config2: 0x{cfg2:02X}")
# 写入矩阵尺寸示例
# 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) data = dev.read_register(0x1C00, 168)
print(f"Serial number: {data.hex().upper()}") print(f"Serial number: {data.hex().upper()}")
finally:
dev.close() 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():
ver = EskinDevice().version()
print(f"ESkin SDK version: {ver[0]}.{ver[1]}.{ver[2]}")
device_path = "/dev/ttyUSB0"
with EskinDevice() as dev:
dev.open(device_path)
print(f"Device opened: {device_path}")
demo_command_mode(dev)
demo_streaming(dev, duration_sec=5.0)
print("Device closed") print("Device closed")
if __name__ == "__main__": if __name__ == "__main__":
main() main()

View File

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

View File

@@ -0,0 +1,130 @@
#include <chrono>
#include <condition_variable>
#include <cstdint>
#include <cstdio>
#include <cstring>
#include <functional>
#include <memory>
#include <mutex>
#include <queue>
#include <string>
#include <thread>
#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<std_msgs::msg::UInt32>("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<std::mutex> 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<std::mutex> 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<uint32_t>(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<CFingerSample> sample_queue_;
std::condition_variable cv_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::UInt32>::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<EskinPublisher>(device_path);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

View File

@@ -0,0 +1,32 @@
#include <memory>
#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<std_msgs::msg::UInt32>(
"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<std_msgs::msg::UInt32>::SharedPtr subscription_;
};
int main(int argc, char * argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<EskinSubscriber>());
rclcpp::shutdown();
return 0;
}

View File

@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>eskin_ros2_demo</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="lenn@todo.todo">lenn</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

View File

@@ -113,6 +113,30 @@ EskinSdkErrorCode eskin_write_matrix_col(
uint16_t* return_count 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 #ifdef __cplusplus
} }
#endif #endif

View File

@@ -1,8 +1,8 @@
use std::{ptr}; use std::{ptr};
use std::ffi::{CStr, c_char}; use std::ffi::{CStr, c_char};
use crate::device::EskinDevice; use crate::device::{DeviceMode, EskinDevice, EskinDeviceFunc};
use crate::device::EskinDeviceFunc;
use crate::transport::SerialPortTransport; use crate::transport::SerialPortTransport;
use crate::types::{CombinedForce, FingerSample};
use crate::{config::DeviceConfig, device::EskinDeviceInner, error::SdkErrorCode}; use crate::{config::DeviceConfig, device::EskinDeviceInner, error::SdkErrorCode};
pub type EskinDeviceHandle = *mut core::ffi::c_void; pub type EskinDeviceHandle = *mut core::ffi::c_void;
@@ -16,20 +16,31 @@ pub struct EskinSdkVersion {
} }
#[repr(C)] #[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 struct CFingerSample {
pub timestamp_us: u64, pub timestamp_us: u64,
pub sequence: u32, pub sequence: u32,
pub combinded_force_raw: *const u8, pub combined_force: CCombinedForce,
pub combinded_force_len: u32,
pub module_error_raw: *const u8,
pub module_error_len: u32,
} }
#[allow(dead_code)] #[allow(dead_code)]
struct DeviceWrapper { struct DeviceWrapper {
device: EskinDeviceInner, device: EskinDeviceInner,
last_cf_raw: Vec<u8>,
last_me_raw: Vec<u8>
} }
fn sdk_error_to_code(err: crate::error::SdkError) -> SdkErrorCode { 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 { let wrapper = Box::new(DeviceWrapper {
device, device,
last_cf_raw: Vec::new(),
last_me_raw: Vec::new(),
}); });
Box::into_raw(wrapper) as EskinDeviceHandle 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), 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,
},
}
}

View File

@@ -89,11 +89,11 @@ fn stream_demo() {
match device.read_sample(200) { match device.read_sample(200) {
Ok(sample) => { Ok(sample) => {
count += 1; count += 1;
if count % 100 == 0 { if count % 5 == 0 {
println!( println!(
"[#{count} seq={}] combined_forces[0..3]={:?}", "[#{count} seq={}] combined_force={:?}",
sample.sequence, sample.sequence,
&sample.combined_forces[..3.min(sample.combined_forces.len())] sample.combined_forces
); );
} }
} }

View File

@@ -147,32 +147,42 @@ impl RegisterMap for EskinRegisterMap {
} }
pub fn parse_combined_forces(raw: &[u8]) -> Result<Vec<CombinedForce>, SdkError> { pub fn parse_combined_forces(raw: &[u8], addr: u32) -> Result<CombinedForce, SdkError> {
const MODULE_COUNT: usize = 28; // println!("{:02X?}", raw);
const BYTES_PER_MODULE: usize = 6; // const MODULE_COUNT: usize = 28;
// const BYTES_PER_MODULE: usize = 6;
if raw.len() < MODULE_COUNT * BYTES_PER_MODULE { // if raw.len() < MODULE_COUNT * BYTES_PER_MODULE {
return Err(SdkError::FrameError(format!( // return Err(SdkError::FrameError(format!(
"combined force raw too short: expected {} bytes, got {}", // "combined force raw too short: expected {} bytes, got {}",
MODULE_COUNT * BYTES_PER_MODULE, // MODULE_COUNT * BYTES_PER_MODULE,
raw.len() // raw.len()
))); // )));
} // }
let mut forces = Vec::with_capacity(MODULE_COUNT); // let mut forces = Vec::with_capacity(MODULE_COUNT);
for i in 0..MODULE_COUNT { // for i in 0..MODULE_COUNT {
let offset = i * BYTES_PER_MODULE; // let offset = i * BYTES_PER_MODULE;
let fx = i16::from_le_bytes([raw[offset], raw[offset + 1]]); // let fx = i16::from_le_bytes([raw[offset], raw[offset + 1]]);
let fy = i16::from_le_bytes([raw[offset + 2], raw[offset + 3]]); // 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 fz = i16::from_le_bytes([raw[offset + 4], raw[offset + 5]]);
forces.push(CombinedForce { // forces.push(CombinedForce {
module: SensorModule::from_index(i as u8), // module: SensorModule::from_index(i as u8),
force: Force3D { fx, fy, fz }, // 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<Vec<ModuleError>, SdkError> { pub fn parse_module_errors(raw: &[u8]) -> Result<Vec<ModuleError>, SdkError> {

View File

@@ -298,7 +298,6 @@ impl PollingSampleCollector {
}; };
let request_frame = self.codec.encode_read_request(&request)?; let request_frame = self.codec.encode_read_request(&request)?;
println!("streaming send: {:02X?}", request_frame);
let response_frame = { let response_frame = {
let mut transport = self let mut transport = self
.transport .transport
@@ -320,10 +319,8 @@ impl SampleCollector for PollingSampleCollector {
let sequence = self.next_sequence(); let sequence = self.next_sequence();
let combined_force_raw = self.read_register(self.config.finger_addr, 168)?; 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 combined_forces = crate::register::parse_combined_forces(&combined_force_raw, self.config.finger_addr)?;
let module_errors = crate::register::parse_module_errors(&module_error_raw)?;
let now = chrono::Utc::now().timestamp_micros() as u64; let now = chrono::Utc::now().timestamp_micros() as u64;
@@ -331,8 +328,8 @@ impl SampleCollector for PollingSampleCollector {
timestamp_us: now, timestamp_us: now,
sequence, sequence,
combined_forces, combined_forces,
distribution_forces: Vec::new(), // distribution_forces: Vec::new(),
module_errors // module_errors
}; };
Ok(Some(sample)) Ok(Some(sample))

View File

@@ -3,9 +3,9 @@ use serde::{Deserialize, Serialize};
#[repr(C)] #[repr(C)]
#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize)] #[derive(Debug, Clone, Copy, Default, Serialize, Deserialize)]
pub struct Force3D { pub struct Force3D {
pub fx: i16, pub fx: u32,
pub fy: i16, pub fy: u32,
pub fz: i16, pub fz: u32,
} }
#[repr(C)] #[repr(C)]
@@ -90,6 +90,34 @@ impl SensorModule {
} }
} }
impl From<u32> 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; pub const SENSOR_MODULE_COUNT: usize = 28;
@@ -113,9 +141,9 @@ pub struct CombinedForce {
pub struct FingerSample { pub struct FingerSample {
pub timestamp_us: u64, pub timestamp_us: u64,
pub sequence: u32, pub sequence: u32,
pub combined_forces: Vec<CombinedForce>, pub combined_forces: CombinedForce,
pub distribution_forces: Vec<DistributionForce>, // pub distribution_forces: Vec<DistributionForce>,
pub module_errors: Vec<ModuleError>, // pub module_errors: Vec<ModuleError>,
} }
#[repr(C)] #[repr(C)]