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
@@ -1,49 +1,148 @@
|
||||
#include "../../include/eskin_ffi.h"
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <mutex>
|
||||
#include <queue>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
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<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);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
52
example/ros-cpp/CMakeLists.txt
Normal file
52
example/ros-cpp/CMakeLists.txt
Normal 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()
|
||||
130
example/ros-cpp/eskin_publisher.cpp
Normal file
130
example/ros-cpp/eskin_publisher.cpp
Normal 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;
|
||||
}
|
||||
32
example/ros-cpp/eskin_subscriber.cpp
Normal file
32
example/ros-cpp/eskin_subscriber.cpp
Normal 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;
|
||||
}
|
||||
21
example/ros-cpp/package.xml
Normal file
21
example/ros-cpp/package.xml
Normal 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>
|
||||
Reference in New Issue
Block a user