Add project sources for Mujoco LinkerHand ROS2

Co-authored-by: Cursor <cursoragent@cursor.com>

.gitignore

@@ -0,0 +1,27 @@
+# ROS / colcon build artifacts
+build/
+install/
+log/
+**/build/
+**/install/
+**/log/
+
+# Python
+venv/
+__pycache__/
+*.py[cod]
+*.egg-info/
+.eggs/
+dist/
+
+# Local / runtime
+.mplconfig/
+.ros_home/
+**/.mplconfig/
+.DS_Store
+*.swp
+*~
+
+# Experiment outputs (regenerable)
+reports/
+tools/reports/

README_启动.md

@@ -0,0 +1,283 @@
+# LinkerHand O6 MuJoCo（ROS2）启动说明
+
+本机跑 MuJoCo 左手仿真，可用本机或另一台电脑通过 ROS2 话题控制。
+
+---
+
+## 环境要求
+
+- Ubuntu + ROS 2 **Jazzy**
+- 工作区路径：`~/linker_hand_mujoco_ros2`
+- Python 依赖在 `venv` 中（含 `mujoco`、`PyQt5`）
+- **注意**：`ros2 launch` 使用系统 `/usr/bin/python3`，仅 `source venv` **不够**，需用下文的启动脚本或设置 `PYTHONPATH`
+
+---
+
+## 一、编译（首次或改代码后）
+
+```bash
+cd ~/linker_hand_mujoco_ros2
+source /opt/ros/jazzy/setup.bash
+colcon build --symlink-install --packages-select linker_hand_mujoco_ros2
+source install/setup.bash
+```
+
+`venv` 目录已放 `COLCON_IGNORE`，不会被 colcon 误扫。
+
+---
+
+## 二、本机启动仿真（推荐）
+
+```bash
+cd ~/linker_hand_mujoco_ros2
+./run_sim.sh
+```
+
+脚本会自动：
+
+- `source` ROS2 与本工作区
+- `ROS_DOMAIN_ID=30`，`ROS_LOCALHOST_ONLY=0`（便于多机）
+- 把 `venv` 的 `site-packages` 加入 `PYTHONPATH`（解决找不到 `mujoco`）
+
+成功后应出现：
+
+1. MuJoCo 3D 窗口（左手 O6）
+2. Joint 进度条窗口（实时关节角）
+
+### 手动启动（等效）
+
+```bash
+cd ~/linker_hand_mujoco_ros2
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+export PYTHONPATH="$PWD/venv/lib/python3.12/site-packages:$PYTHONPATH"
+ros2 launch linker_hand_mujoco_ros2 linker_hand_mujoco_ros2.launch.py
+```
+
+当前 launch 默认：
+
+| 参数 | 值 |
+|------|-----|
+| `hand_type` | `left` |
+| `hand_joint` | `O6` |
+| 控制话题 | `/cb_left_hand_control_cmd` |
+
+---
+
+## 三、控制话题说明
+
+- 话题：`/cb_left_hand_control_cmd`
+- 类型：`sensor_msgs/msg/JointState`
+- `position`：**6 个数**，范围约 **0–255**
+
+| 索引 | 含义 |
+|------|------|
+| 0 | 拇指弯曲 |
+| 1 | 拇指横摆 |
+| 2 | 食指弯曲 |
+| 3 | 中指弯曲 |
+| 4 | 无名指弯曲 |
+| 5 | 小指弯曲 |
+
+约定（当前映射）：约 **255 = 张开，0 = 握紧**。远指节由 mimic 跟随近指节。
+
+### 本机发一帧（握拳示例）
+
+```bash
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=30
+export ROS_LOCALHOST_ONLY=0
+
+ros2 topic pub --once /cb_left_hand_control_cmd sensor_msgs/msg/JointState "{
+  header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''},
+  name: [],
+  position: [0.0, 100.0, 0.0, 0.0, 0.0, 0.0],
+  velocity: [],
+  effort: []
+}"
+```
+
+张开：
+
+```bash
+ros2 topic pub --once /cb_left_hand_control_cmd sensor_msgs/msg/JointState "{
+  header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''},
+  name: [],
+  position: [255.0, 255.0, 255.0, 255.0, 255.0, 255.0],
+  velocity: [],
+  effort: []
+}"
+```
+
+---
+
+## 四、两台电脑共用 ROS2
+
+ROS2 **没有** ROS1 的 `roscore`。两边约定同一个 Domain 即可。
+
+### 两边都设置
+
+```bash
+export ROS_DOMAIN_ID=30
+export ROS_LOCALHOST_ONLY=0
+```
+
+### 电脑 A：仿真机
+
+```bash
+cd ~/linker_hand_mujoco_ros2
+./run_sim.sh
+```
+
+### 电脑 B：控制机（发指令 / handretarget / SDK）
+
+```bash
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=30
+export ROS_LOCALHOST_ONLY=0
+
+# 确认话题与收发端
+ros2 topic list
+ros2 topic info /cb_left_hand_control_cmd -v
+```
+
+`topic info -v` 中应能看到：
+
+- **Publisher**：例如 `handretarget_node`
+- **Subscription**：至少包含 `linker_hand_mujoco_ros2_node`
+
+### 查是否真的在发数据
+
+有 Publisher ≠ 正在发数据。请用：
+
+```bash
+ros2 topic hz /cb_left_hand_control_cmd
+ros2 topic echo /cb_left_hand_control_cmd
+```
+
+- `hz` 无输出 → 对端没在 publish  
+- `echo --once` 会一直等到有一帧；无数据时像卡住，优先用上面两条
+
+### 跨机能看见话题但收不到数据时
+
+1. 确认两边 `ROS_DOMAIN_ID`、`ROS_LOCALHOST_ONLY=0` 一致，并重启节点  
+2. 互相 `ping` 通；尽量同一网段 / 有线；关闭路由器 **AP 隔离**  
+3. 本机 `ufw` 可先确认：`sudo ufw status`（不活动则不是防火墙）  
+4. 需要时可配置 CycloneDDS 静态 Peer（两边 IP 写入 `~/cyclonedds.xml`），然后：
+
+```bash
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+export CYCLONEDDS_URI=file://$HOME/cyclonedds.xml
+```
+
+---
+
+## 五、常用自检命令
+
+```bash
+# 节点是否在
+ros2 node list
+
+# 话题列表（仿真未启动时可能还没有 /cb_left_hand_control_cmd）
+ros2 topic list
+
+# 订阅/发布数量与节点名
+ros2 topic info /cb_left_hand_control_cmd -v
+
+# 频率 / 内容
+ros2 topic hz /cb_left_hand_control_cmd
+ros2 topic echo /cb_left_hand_control_cmd
+```
+
+---
+
+## 六、常见问题
+
+### `ModuleNotFoundError: No module named 'mujoco'`
+
+`ros2 launch` 没用到 venv。请用：
+
+```bash
+./run_sim.sh
+```
+
+或设置 `PYTHONPATH`（见上文「手动启动」）。
+
+### `topic list` 只有 `/rosout`，没有左手话题
+
+仿真节点未运行。先 `./run_sim.sh`，再 `ros2 topic list`。
+
+### 改左右手
+
+编辑：
+
+`src/linkerhand-sim/linker_hand_mujoco_ros2/launch/linker_hand_mujoco_ros2.launch.py`
+
+- 左手：`hand_type: 'left'` → 话题 `/cb_left_hand_control_cmd`
+- 右手：`hand_type: 'right'` → 话题 `/cb_right_hand_control_cmd`
+
+改完后若未用 symlink，再执行一次 `colcon build --symlink-install`。
+
+---
+
+## 七、通用曲线录制（仿真 / 真机同一套话题）
+
+**只传一个文件给对端即可：**
+
+`tools/hand_curve_recorder_standalone.py`
+
+两边都要：已 `source` ROS2、`numpy`、`matplotlib`。
+
+```bash
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=30
+export ROS_LOCALHOST_ONLY=0
+
+# 仿真端
+python3 tools/hand_curve_recorder_standalone.py --hand left --channel 3 --label sim
+
+# 真机端（把该 py 拷过去后）
+python3 hand_curve_recorder_standalone.py --hand left --channel 3 --label real
+```
+
+对端发 `/cb_left_hand_control_cmd`；本机 **Ctrl+C** → `reports/hand_curves/` 下生成 csv + 三曲线图。
+
+默认还订 `/cb_left_hand_state`（`position`=0～255，`effort` 可放电流）。话题名不同时：
+
+```bash
+python3 hand_curve_recorder_standalone.py --state-topic /你的状态话题 --channel 3 --label real
+```
+
+通道：`0拇指弯 1拇指横摆 2食指 3中指 4无名指 5小指`。
+
+---
+
+要「ROS 发指令 + 仿真里出 q/v/τ 图」：
+
+```bash
+cd ~/linker_hand_mujoco_ros2
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=30
+export ROS_LOCALHOST_ONLY=0
+
+# 自测：订话题 + 本地 MuJoCo + 自动发中指慢扫 + 可选 viewer
+./venv/bin/python tools/ros_mujoco_record_qvt.py \
+  --hand left --finger middle --viewer --self-sweep
+
+# 对端发指令时：去掉 --self-sweep
+./venv/bin/python tools/ros_mujoco_record_qvt.py \
+  --hand left --finger middle --viewer
+# Ctrl+C → reports/O6_middle_ros/*_qvt.png
+```
+
+---
+
+## 八、文件与模型位置
+
+| 路径 | 说明 |
+|------|------|
+| `./run_sim.sh` | 本机一键启动 |
+| `src/.../launch/linker_hand_mujoco_ros2.launch.py` | 左右手 / 型号 |
+| `src/.../urdf/O6/` | O6 左右手 MJCF（来自 mujoco_testwork 验证模型） |
+| `src/.../utils/joint_monitor.py` | Joint 进度条 UI |
+| `venv/` | Python 依赖（勿放进 colcon 扫描，已 IGNORE） |

linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/graphic_display.py

@@ -0,0 +1,289 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+import matplotlib
+matplotlib.use('Qt5Agg')
+from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg
+from matplotlib.figure import Figure
+from PyQt5 import QtWidgets, QtCore
+import numpy as np
+import json
+import sys
+from typing import Dict, List
+
+from linker_hand_ros2_sdk.LinkerHand.utils.init_linker_hand import InitLinkerHand
+
+class ForceGroupWindow(QtWidgets.QMainWindow):
+    """专用力传感器组可视化窗口"""
+    def __init__(self, group_id: int):
+        super().__init__()
+        self.setWindowTitle(f"Force Sensor Group {group_id+1}")
+        self.setGeometry(100 + group_id*50, 100 + group_id*50, 800, 400)
+        
+        # 图形设置
+        self.canvas = FigureCanvasQTAgg(Figure(figsize=(8, 4)))
+        self.setCentralWidget(self.canvas)
+        self.ax = self.canvas.figure.add_subplot(111)
+        self.ax.set_title(f'Force Group {group_id+1} (5 channels)')
+        self.ax.set_xlabel('Time Step')
+        self.ax.set_ylabel('Force (N)')
+        self.ax.grid(True)
+        
+        # 数据存储
+        self.buffer_size = 200
+        self.x_data = np.arange(self.buffer_size)
+        #self.channels = [f'Channel {i+1}' for i in range(5)]
+        self.channels = ["thumb","index finger","middle finger","ring finger","little finger"]
+        self.data = {name: np.full(self.buffer_size, np.nan) for name in self.channels}
+        self.lines = {}
+        self.data_ptr = 0
+        
+        # 颜色设置
+        colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd']
+        
+        # 创建曲线
+        for i, name in enumerate(self.channels):
+            self.lines[name], = self.ax.plot(
+                self.x_data,
+                self.data[name],
+                color=colors[i],
+                label=name,
+                linewidth=1.5
+            )
+        
+        self.ax.legend(loc='upper right')
+        self.ax.set_xlim(0, self.buffer_size)
+        self.ax.set_ylim(0, 300)  # 假设力传感器范围0-300N
+        
+        # 定时刷新
+        self.timer = QtCore.QTimer()
+        self.timer.timeout.connect(self.update_plot)
+        self.timer.start(50)  # 20fps
+    
+    def add_data(self, new_data: List[float]):
+        """添加新数据点"""
+        self.data_ptr = (self.data_ptr + 1) % self.buffer_size
+        for name, val in zip(self.channels, new_data):
+            self.data[name][self.data_ptr] = float(val)
+    
+    def update_plot(self):
+        """更新绘图"""
+        # 更新曲线数据
+        for name, line in self.lines.items():
+            line.set_ydata(np.roll(self.data[name], -self.data_ptr))
+        
+        self.canvas.draw()
+
+class HandMonitor(Node):
+    def __init__(self):
+        super().__init__('graphic_display')
+        
+        # 初始化Qt应用
+        self.app = QtWidgets.QApplication(sys.argv)
+        
+        # 窗口管理
+        self.force_windows = {}  # 存储force组窗口 {group_id: window}
+        self.temp_window = None  # 温度窗口
+        self.torque_window = None  # 扭矩窗口
+        self.hand_joint, self.hand_type = InitLinkerHand().current_hand()
+        if self.hand_type == "left":
+            self.topic = "/cb_left_hand_info"
+        else:
+            self.topic = "/cb_right_hand_info"
+        #self.topic = "/cb_left_hand_info"
+        # ROS2订阅
+        self.subscription = self.create_subscription(
+            String,
+            self.topic,
+            self.data_callback,
+            10)
+        
+        # Qt事件处理定时器
+        self.timer = self.create_timer(0.1, self.process_qt_events)
+        
+        self.get_logger().info("Hand monitor initialized")
+    
+    def data_callback(self, msg: String):
+        """处理手部数据回调"""
+        try:
+            data = json.loads(msg.data)
+            if self.hand_type == "left":
+                tmp = "left_hand"
+            else:
+                tmp = "right_hand"
+            if isinstance(data, dict) and tmp in data:
+                hand_data = data[tmp]
+                
+                # 处理force数据 (每组force一个独立窗口)
+                if 'force' in hand_data:
+                    force_data = hand_data['force']
+                    for group_id, group_values in enumerate(force_data):
+                        if len(group_values) == 5:  # 每组应有5个值
+                            if group_id not in self.force_windows:
+                                self.force_windows[group_id] = ForceGroupWindow(group_id)
+                                self.force_windows[group_id].show()
+                            
+                            # 跨线程安全更新
+                            if QtCore.QThread.currentThread() == self.app.thread():
+                                self.force_windows[group_id].add_data(group_values)
+                            else:
+                                QtCore.QMetaObject.invokeMethod(
+                                    self.force_windows[group_id],
+                                    'add_data',
+                                    QtCore.Qt.QueuedConnection,
+                                    QtCore.Q_ARG(list, group_values)
+                                )
+                
+                # 处理温度数据 (单个窗口)
+                if 'motor_temperature' in hand_data:
+                    temp_data = hand_data['motor_temperature']
+                    if len(temp_data) == 10:  # 应有10个温度值
+                        if self.temp_window is None:
+                            self.create_temp_window()
+                        self.update_window_data(self.temp_window, temp_data)
+                
+                # 处理扭矩数据 (单个窗口)
+                # if 'torque' in hand_data:
+                #     torque_data = hand_data['torque']
+                #     if len(torque_data) == 5:  # 应有5个扭矩值
+                #         if self.torque_window is None:
+                #             self.create_torque_window()
+                #         self.update_window_data(self.torque_window, torque_data)
+        
+        except Exception as e:
+            self.get_logger().error(f"Data processing error: {str(e)}")
+    
+    def create_temp_window(self):
+        """创建温度窗口"""
+        self.temp_window = DataPlotWindow(
+            title="Motor Temperatures",
+            ylabel="Temperature (°C)",
+            channel_count=10,
+            y_range=(20, 50)
+        )
+        self.temp_window.show()
+    
+    def create_torque_window(self):
+        """创建扭矩窗口"""
+        self.torque_window = DataPlotWindow(
+            title="Joint Torque",
+            ylabel="Torque (Nm)",
+            channel_count=5,
+            y_range=(-0.5, 0.5)
+        )
+        self.torque_window.show()
+    
+    def update_window_data(self, window, data):
+        """通用窗口数据更新"""
+        if QtCore.QThread.currentThread() == self.app.thread():
+            window.add_data(data)
+        else:
+            QtCore.QMetaObject.invokeMethod(
+                window,
+                'add_data',
+                QtCore.Qt.QueuedConnection,
+                QtCore.Q_ARG(list, data)
+            )
+    
+    def process_qt_events(self):
+        """处理Qt事件循环"""
+        self.app.processEvents()
+        
+        # 清理已关闭的窗口
+        self.force_windows = {k: v for k, v in self.force_windows.items() if v.isVisible()}
+        if self.temp_window and not self.temp_window.isVisible():
+            self.temp_window = None
+        if self.torque_window and not self.torque_window.isVisible():
+            self.torque_window = None
+    
+    def run(self):
+        """启动Qt应用"""
+        self.app.exec_()
+    
+    def destroy_node(self):
+        """清理资源"""
+        for window in self.force_windows.values():
+            window.close()
+        if self.temp_window:
+            self.temp_window.close()
+        if self.torque_window:
+            self.torque_window.close()
+        super().destroy_node()
+
+class DataPlotWindow(QtWidgets.QMainWindow):
+    """通用数据绘图窗口"""
+    def __init__(self, title: str, ylabel: str, channel_count: int, y_range: tuple):
+        super().__init__()
+        self.setWindowTitle(title)
+        self.setGeometry(100, 100, 800, 400)
+        
+        # 图形设置
+        self.canvas = FigureCanvasQTAgg(Figure(figsize=(8, 4)))
+        self.setCentralWidget(self.canvas)
+        self.ax = self.canvas.figure.add_subplot(111)
+        self.ax.set_title(title)
+        self.ax.set_xlabel('Time Step')
+        self.ax.set_ylabel(ylabel)
+        self.ax.grid(True)
+        
+        # 数据存储
+        self.buffer_size = 200
+        self.x_data = np.arange(self.buffer_size)
+        self.channels = [f'Channel {i+1}' for i in range(channel_count)]
+        #self.channels = ["thumb","index finger","middle finger","ring finger","little finger"]
+        self.data = {name: np.full(self.buffer_size, np.nan) for name in self.channels}
+        self.lines = {}
+        self.data_ptr = 0
+        
+        # 创建曲线
+        colors = matplotlib.colormaps['tab20'].colors
+        for i, name in enumerate(self.channels):
+            self.lines[name], = self.ax.plot(
+                self.x_data,
+                self.data[name],
+                color=colors[i % len(colors)],
+                label=name,
+                linewidth=1.5
+            )
+        
+        self.ax.legend(bbox_to_anchor=(1.05, 1), loc='upper left')
+        self.ax.set_xlim(0, self.buffer_size)
+        self.ax.set_ylim(*y_range)
+        
+        # 定时刷新
+        self.timer = QtCore.QTimer()
+        self.timer.timeout.connect(self.update_plot)
+        self.timer.start(50)
+    
+    def add_data(self, new_data: List[float]):
+        """添加新数据点"""
+        self.data_ptr = (self.data_ptr + 1) % self.buffer_size
+        for name, val in zip(self.channels, new_data):
+            self.data[name][self.data_ptr] = float(val)
+    
+    def update_plot(self):
+        """更新绘图"""
+        for name, line in self.lines.items():
+            line.set_ydata(np.roll(self.data[name], -self.data_ptr))
+        self.canvas.draw()
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    # 必须在主线程创建节点
+    monitor = HandMonitor()
+    
+    # 启动Qt线程
+    from threading import Thread
+    qt_thread = Thread(target=monitor.run, daemon=True)
+    qt_thread.start()
+    
+    try:
+        rclpy.spin(monitor)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        monitor.destroy_node()
+        rclpy.shutdown()
+
+

linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/views/temperature_plot.py

@@ -0,0 +1,68 @@
+from PyQt5.QtWidgets import QApplication, QVBoxLayout, QWidget
+from PyQt5.QtCore import QTimer
+import sys
+import random
+import matplotlib.pyplot as plt
+from matplotlib import font_manager, rcParams
+from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
+
+class TemperaturePlot(QWidget):
+    def __init__(self, num_lines=10, labels=None, title="Temperature Plot"):
+        super().__init__()
+        self.num_lines = num_lines
+        self.labels = labels if labels else [f"Line {i+1}" for i in range(num_lines)]
+        self.data = [[] for _ in range(self.num_lines)]  # 初始化数据列表
+        self.max_points = 100  # 最大显示点数
+        self.setWindowTitle(title)
+        # 设置布局
+        self.layout = QVBoxLayout()
+        self.setLayout(self.layout)
+
+        # 初始化 matplotlib 图形
+        self.figure, self.ax = plt.subplots()
+        self.canvas = FigureCanvas(self.figure)
+        self.layout.addWidget(self.canvas)
+
+        # 初始化绘图曲线
+        self.lines = [self.ax.plot([], [], label=self.labels[i])[0] for i in range(self.num_lines)]
+        self.ax.set_xlim(0, self.max_points)
+        self.ax.set_ylim(0, 100)
+        self.ax.set_xlabel("Time")
+        self.ax.set_ylabel("Value")
+        self.ax.legend(loc='upper left')  # 固定标签位置到左上角
+
+    def update_data(self, new_data):
+        for i in range(self.num_lines):
+            self.data[i].append(new_data[i])
+            if len(self.data[i]) > self.max_points:
+                self.data[i].pop(0)
+        self._update_plot()
+
+    def _update_plot(self):
+        """内部方法: 更新绘图"""
+        for i, line in enumerate(self.lines):
+            line.set_data(range(len(self.data[i])), self.data[i])
+
+        self.ax.set_xlim(0, self.max_points)
+        self.canvas.draw()
+
+if __name__ == "__main__":
+    app = QApplication(sys.argv)
+
+    # 示例: 创建一个包含 3 条曲线的波形图
+    waveform_plot = TemperaturePlot(num_lines=10)
+    waveform_plot.resize(800, 400)
+    waveform_plot.show()
+
+    # 模拟数据更新
+    timer = QTimer()
+
+    def update():
+        import random
+        values = [random.randint(0, 300) for _ in range(10)]  # 生成随机数据
+        waveform_plot.update_data(values)
+
+    timer.timeout.connect(update)
+    timer.start(100)  # 每 100 毫秒更新一次
+
+    sys.exit(app.exec_())

linker_hand_ros2_sdk_ws/src/graphic_display/graphic_display/views/wave_form_plot.py

@@ -0,0 +1,68 @@
+from PyQt5.QtWidgets import QApplication, QVBoxLayout, QWidget
+from PyQt5.QtCore import QTimer
+import sys
+import random
+import matplotlib.pyplot as plt
+from matplotlib import font_manager, rcParams
+from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
+
+class WaveformPlot(QWidget):
+    def __init__(self, num_lines=3, labels=None, title="Waveform Plot"):
+        super().__init__()
+        self.num_lines = num_lines
+        self.labels = labels if labels else [f"Line {i+1}" for i in range(num_lines)]
+        self.data = [[] for _ in range(self.num_lines)]  # 初始化数据列表
+        self.max_points = 100  # 最大显示点数
+        self.setWindowTitle(title)
+        # 设置布局
+        self.layout = QVBoxLayout()
+        self.setLayout(self.layout)
+
+        # 初始化 matplotlib 图形
+        self.figure, self.ax = plt.subplots()
+        self.canvas = FigureCanvas(self.figure)
+        self.layout.addWidget(self.canvas)
+
+        # 初始化绘图曲线
+        self.lines = [self.ax.plot([], [], label=self.labels[i])[0] for i in range(self.num_lines)]
+        self.ax.set_xlim(0, self.max_points)
+        self.ax.set_ylim(0, 300)
+        self.ax.set_xlabel("Time")
+        self.ax.set_ylabel("Value")
+        self.ax.legend(loc='upper left')  # 固定标签位置到左上角
+
+    def update_data(self, new_data):
+        for i in range(self.num_lines):
+            self.data[i].append(new_data[i])
+            if len(self.data[i]) > self.max_points:
+                self.data[i].pop(0)
+        self._update_plot()
+
+    def _update_plot(self):
+        """内部方法: 更新绘图"""
+        for i, line in enumerate(self.lines):
+            line.set_data(range(len(self.data[i])), self.data[i])
+
+        self.ax.set_xlim(0, self.max_points)
+        self.canvas.draw()
+
+if __name__ == "__main__":
+    app = QApplication(sys.argv)
+
+    # 示例: 创建一个包含 3 条曲线的波形图
+    waveform_plot = WaveformPlot(num_lines=3)
+    waveform_plot.resize(800, 400)
+    waveform_plot.show()
+
+    # 模拟数据更新
+    timer = QTimer()
+
+    def update():
+        import random
+        values = [random.randint(0, 300) for _ in range(3)]  # 生成随机数据
+        waveform_plot.update_data(values)
+
+    timer.timeout.connect(update)
+    timer.start(100)  # 每 100 毫秒更新一次
+
+    sys.exit(app.exec_())

linker_hand_ros2_sdk_ws/src/graphic_display/package.xml

@@ -0,0 +1,19 @@
+<?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>graphic_display</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="linker-robot@todo.todo">linker-robot</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  <exec_depend>linker_hand_ros2_sdk</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

linker_hand_ros2_sdk_ws/src/graphic_display/pyproject.toml

@@ -0,0 +1,3 @@
+[build-system]
+requires = ["setuptools>=61.0"]
+build-backend = "setuptools.build_meta"

linker_hand_ros2_sdk_ws/src/graphic_display/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/graphic_display
+[install]
+install_scripts=$base/lib/graphic_display

linker_hand_ros2_sdk_ws/src/graphic_display/setup.py

@@ -0,0 +1,37 @@
+'''
+Author: HJX
+Date: 2025-04-02 17:52:22
+LastEditors: Please set LastEditors
+LastEditTime: 2025-04-03 09:45:57
+FilePath: /linker_hand_ros2_sdk/src/graphic_display/setup.py
+Description: 
+symbol_custom_string_obkorol_copyright: 
+'''
+from setuptools import find_packages, setup
+
+package_name = 'graphic_display'
+
+setup(
+    name=package_name,
+    version='0.0.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', 'linker_hand_ros2_sdk'],
+    zip_safe=True,
+    maintainer='linker-robot',
+    maintainer_email='linker-robot@todo.todo',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    extras_require={
+        'test': ['pytest'],
+    },
+    entry_points={
+        'console_scripts': [
+            'graphic_display = graphic_display.graphic_display:main'
+        ],
+    },
+)

linker_hand_ros2_sdk_ws/src/graphic_display/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

linker_hand_ros2_sdk_ws/src/graphic_display/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

linker_hand_ros2_sdk_ws/src/graphic_display/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

linker_hand_ros2_sdk_ws/src/gui_control/gui_control/config/constants.py

@@ -0,0 +1,262 @@
+# hand_config_const.py
+from typing import Dict, List, Optional
+from dataclasses import dataclass, field
+from types import MappingProxyType
+
+@dataclass(frozen=True)        # frozen=True 让实例真正只读
+class HandConfig:
+    joint_names: List[str] = field(default_factory=list)
+    joint_names_en: Optional[List[str]] = None
+    init_pos: List[int] = field(default_factory=list)
+    preset_actions: Optional[Dict[str, List[int]]] = None
+
+# ------------------------------------------------------------------
+# 常量字典（仅构建一次）
+# ------------------------------------------------------------------
+_HAND_CONFIGS: Dict[str, HandConfig] = {
+    "L25": HandConfig(
+        joint_names=["大拇指根部", "食指根部", "中指根部", "无名指根部", "小拇指根部",
+                     "大拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小拇指侧摆",
+                     "大拇指横滚", "预留", "预留", "预留", "预留", "大拇指中部", "食指中部",
+                     "中指中部", "无名指中部", "小拇指中部", "大拇指指尖", "食指指尖",
+                     "中指指尖", "无名指指尖", "小拇指指尖"],
+        init_pos=[255] * 25,
+        preset_actions={
+            "握拳": [0] * 25,
+            "张开": [255] * 25,
+            "OK": [255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+                   255, 255, 255, 255, 255, 255, 0, 0, 255, 255,
+                   0, 0, 0, 255, 255]
+        }
+    ),
+    "L21": HandConfig(
+        joint_names=["大拇指根部", "食指根部", "中指根部", "无名指根部", "小拇指根部",
+                     "大拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小拇指侧摆",
+                     "大拇指横滚", "预留", "预留", "预留", "预留", "大拇指中部", "预留",
+                     "预留", "预留", "预留", "大拇指指尖", "食指指尖", "中指指尖",
+                     "无名指指尖", "小拇指指尖"],
+        init_pos=[255] * 25
+    ),
+    "L20": HandConfig(
+        joint_names=["拇指根部", "食指根部", "中指根部", "无名指根部", "小指根部",
+                     "拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小指侧摆",
+                     "拇指横摆", "预留", "预留", "预留", "预留", "拇指尖部", "食指末端",
+                     "中指末端", "无名指末端", "小指末端"],
+        init_pos=[255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+        preset_actions={
+            "握拳": [40, 0, 0, 0, 0, 131, 10, 100, 180, 240, 19, 255, 255, 255, 255, 135, 0, 0, 0, 0],
+            "张开": [255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "OK": [191, 95, 255, 255, 255, 136, 107, 100, 180, 240, 72, 255, 255, 255, 255, 116, 99, 255, 255, 255],
+            "点赞": [255, 0, 0, 0, 0, 127, 10, 100, 180, 240, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0],
+            "拇指对食指": [0, 0, 255, 255, 255, 186, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 183, 255, 255, 255],
+            "拇指对中指": [0, 255, 0, 255, 255, 145, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 255, 202, 255, 255],
+            "拇指对无名指": [0, 255, 255, 0, 255, 108, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "拇指对小指": [0, 255, 255, 255, 0, 70, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "准备1": [255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "壹": [40, 255, 0, 0, 0, 131, 125, 100, 180, 240, 19, 255, 255, 255, 255, 135, 255, 0, 0, 0],
+            "贰": [40, 255, 255, 0, 0, 81, 35, 177, 180, 240, 19, 255, 255, 255, 255, 135, 255, 255, 0, 0],
+            "叁": [40, 255, 255, 255, 0, 161, 62, 123, 180, 240, 13, 255, 255, 255, 255, 0, 255, 255, 255, 0],
+            "肆": [40, 255, 255, 255, 255, 161, 62, 123, 180, 242, 13, 255, 255, 255, 255, 0, 255, 255, 255, 255],
+            "伍": [255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "陆": [255, 0, 0, 0, 255, 220, 10, 100, 180, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 255],
+            "漆": [0, 0, 0, 0, 0, 161, 10, 127, 180, 219, 18, 255, 255, 255, 255, 255, 195, 205, 0, 0],
+            "捌": [255, 255, 0, 0, 0, 202, 104, 100, 180, 240, 233, 255, 255, 255, 255, 255, 255, 0, 0, 0],
+            "玖": [40, 255, 0, 0, 0, 131, 103, 100, 180, 240, 19, 255, 255, 255, 255, 135, 47, 0, 0, 0],
+            
+        }
+    ),
+    "G20": HandConfig(
+        joint_names=["拇指根部", "食指根部", "中指根部", "无名指根部", "小指根部",
+                     "拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小指侧摆",
+                     "拇指横摆", "预留", "预留", "预留", "预留", "拇指尖部", "食指末端",
+                     "中指末端", "无名指末端", "小指末端"],
+        init_pos=[255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+        preset_actions={
+            "点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
+            "握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
+            "张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "OK": [148, 110, 255, 255, 255, 44, 164, 100, 114, 127, 178, 255, 255, 255, 255, 94, 71, 255, 255, 255],
+            "拇指对中指": [191, 255, 55, 255, 255, 96, 95, 100, 114, 127, 105, 255, 255, 255, 255, 94, 255, 108, 255, 255],
+            "拇指对无名指": [191, 255, 255, 72, 255, 115, 95, 100, 114, 127, 60, 255, 255, 255, 255, 94, 255, 255, 97, 255],
+            "拇指对小指": [191, 255, 255, 255, 55, 0, 95, 100, 114, 121, 70, 255, 255, 255, 255, 94, 255, 255, 255, 100],
+            "准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
+            "壹": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0],
+            "贰": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0],
+            "叁": [96, 255, 255, 255, 0, 0, 200, 132, 76, 80, 68, 255, 255, 255, 255, 144, 255, 255, 255, 0],
+            "肆": [80, 255, 255, 255, 255, 78, 200, 132, 114, 48, 129, 255, 255, 255, 255, 64, 255, 255, 255, 255],
+            "伍": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "陆": [255, 0, 0, 0, 255, 255, 156, 126, 125, 42, 245, 255, 255, 255, 255, 255, 0, 0, 0, 255],
+            "漆": [38, 0, 0, 0, 0, 55, 156, 126, 125, 117, 145, 255, 255, 255, 255, 255, 164, 163, 0, 0],
+            "捌": [255, 255, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 255, 0, 0, 0],
+            "玖": [67, 255, 0, 0, 0, 37, 162, 162, 144, 100, 85, 255, 255, 255, 255, 169, 0, 0, 0, 0],
+            "动作1": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作3": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作4": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作5": [255, 255, 255, 255, 255, 255, 255, 255, 247, 255, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作6": [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作7": [255, 255, 255, 255, 255, 255, 255, 255, 247, 255, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作8": [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作9": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "末端1": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0],
+            "末端2": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "末端3": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0],
+            "末端4": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "默认": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+
+
+        }
+    ),
+    # 大拇指关节球版L10
+    # "L10": HandConfig(
+    #     joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
+    #                     "index_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", "thumb_cmc_yaw"],
+    #     joint_names=["拇指根部", "拇指侧摆", "食指根部", "中指根部", "无名指根部",
+    #                  "小指根部", "食指侧摆", "无名指侧摆", "小指侧摆", "拇指旋转"],
+    #     init_pos=[255] * 10,
+    #     preset_actions={
+    #         "握拳": [75, 128, 0, 0, 0, 0, 128, 128, 128, 57],
+    #         "张开": [255, 128, 255, 255, 255, 255, 128, 128, 128, 128],
+    #         "OK": [110, 128, 75, 255, 255, 255, 128, 128, 128, 68],
+    #         "点赞": [255, 145, 0, 0, 0, 0, 0, 255, 255, 65]
+    #     }
+    # ),
+    # 大拇指关节齿轮版L10
+    "L10": HandConfig(
+        joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
+                        "index_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", "thumb_cmc_roll"],
+        joint_names=["拇指根部", "拇指侧摆", "食指根部", "中指根部", "无名指根部",
+                     "小指根部", "食指侧摆", "无名指侧摆", "小指侧摆", "拇指旋转"],
+        init_pos=[255] * 10,
+        preset_actions={
+            "张开": [255, 255, 255, 255, 255, 255, 128, 67, 89, 255],
+            "点赞": [255, 255, 0, 0, 0, 0, 128, 67, 89, 255],
+            "握拳": [90, 0, 0, 0, 0, 0, 128, 67, 89, 197],
+            "壹": [55, 0, 255, 0, 0, 0, 128, 67, 89, 124],
+            "贰": [55, 0, 255, 255, 0, 0, 128, 67, 89, 124],
+            "叁": [116, 255, 255, 255, 255, 0, 128, 67, 89, 255],
+            "肆": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+            "伍": [255, 255, 255, 255, 255, 255, 128, 67, 89, 255],
+            "陆": [255, 255, 0, 0, 0, 255, 128, 67, 89, 255],
+            "柒1": [255, 37, 119, 112, 0, 0, 128, 67, 89, 211],
+            "柒2": [91, 37, 119, 112, 0, 0, 128, 67, 89, 211],
+            "捌": [255, 255, 255, 0, 0, 0, 128, 67, 89, 255],
+            "玖": [59, 0, 134, 0, 0, 0, 128, 67, 89, 153],
+            "侧摆0": [255, 0, 255, 255, 255, 255, 128, 67, 89, 153],
+            "侧摆1": [0, 0, 255, 255, 255, 255, 255, 255, 255, 255],
+            "侧摆2": [0, 0, 255, 255, 255, 255, 0, 0, 0, 255],
+            "侧摆3": [0, 0, 255, 255, 255, 255, 255, 255, 255, 255],
+            "侧摆4": [0, 0, 255, 255, 255, 255, 0, 0, 0, 255],
+            "侧摆5": [0, 0, 255, 255, 255, 255, 255, 255, 255, 255],
+            "侧摆6": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+            "弯曲1": [209, 0, 124, 122, 123, 122, 128, 67, 89, 255],
+            "弯曲2": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+            "弯曲3": [209, 0, 124, 122, 123, 122, 128, 67, 89, 255],
+            "弯曲4": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+            "弯曲5": [209, 0, 124, 122, 123, 122, 128, 67, 89, 255],
+            "弯曲6": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+            "OK": [84, 39, 122, 255, 255, 255, 128, 67, 89, 255],
+            "拇指压感1": [134, 39, 99, 255, 255, 255, 128, 67, 89, 255],
+            "拇指压感2": [73, 39, 91, 255, 255, 255, 128, 67, 89, 255],
+            "食指压感1": [151, 39, 190, 255, 255, 255, 128, 67, 89, 255],
+            "食指压感2": [58, 39, 103, 255, 255, 255, 128, 67, 89, 255],
+            "中指压感1": [40, 39, 255, 128, 255, 255, 128, 67, 89, 209],
+            "中指压感2": [40, 39, 255, 89, 255, 255, 128, 67, 89, 209],
+            "无名指压感1": [51, 39, 255, 255, 139, 255, 128, 67, 89, 154],
+            "无名指压感2": [51, 39, 255, 255, 83, 255, 128, 67, 89, 154],
+            "小拇指压感1": [62, 39, 255, 255, 255, 155, 128, 67, 89, 101],
+            "小拇指压感2": [62, 39, 255, 255, 255, 75, 128, 67, 89, 101],
+        }
+    ),
+    # 大拇指关节球版L7
+    # "L7": HandConfig(
+    #     joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲",
+    #                  "小拇指弯曲", "拇指旋转"],
+    #     init_pos=[250] * 7,
+    #     preset_actions={
+    #         "点赞": [255, 111, 0, 0, 0, 0, 86],
+    #         "握拳": [71, 79, 0, 0, 0, 0, 64],
+    #         "张开": [255, 111, 250, 250, 250, 250, 55],
+    #         "OK": [141, 111, 168, 250, 250, 250, 86],
+            
+    #     }
+    # ),
+    # 大拇指关节齿轮版L7
+    "L7": HandConfig(
+        joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", "thumb_cmc_roll"],
+        joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲", "拇指旋转"],
+        init_pos=[250] * 7,
+        preset_actions={
+            "张开": [255, 111, 250, 250, 250, 250, 55],
+            "点赞": [255, 255, 0, 0, 0, 0, 255],
+            "赞2": [255, 0, 0, 0, 0, 0, 255],
+            "握拳": [65, 0, 0, 0, 0, 0, 93],
+            "壹": [66, 0, 255, 0, 0, 0, 93],
+            "壹1": [61, 0, 255, 0, 0, 0, 255],
+            "贰": [0, 0, 255, 255, 0, 0, 255],
+            "叁": [0, 0, 255, 255, 255, 0, 255],
+            "肆": [0, 0, 255, 255, 255, 255, 119],
+            "伍": [255, 111, 250, 250, 250, 250, 55],
+            "OK": [99, 15, 146, 250, 250, 250, 206],
+            "拇指压感1": [99, 15, 206, 250, 250, 250, 206],
+            "拇指压感2": [109, 15, 70, 250, 250, 250, 206],
+            "食指压感1": [99, 15, 206, 250, 250, 250, 206],
+            "食指压感2": [69, 15, 140, 250, 250, 250, 206],
+            "中指压感": [82, 15, 255, 136, 250, 250, 170],
+            "无名指压感": [70, 15, 255, 255, 141, 250, 125],
+            "小拇指压感": [70, 15, 255, 255, 255, 120, 78],
+            "准备1": [70, 15, 255, 255, 255, 255, 78]
+            
+        }
+    ),
+    "O6": HandConfig(
+        joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"],
+        joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"],
+        init_pos=[250] * 6,
+        preset_actions={
+            "张开": [250, 250, 250, 250, 250, 250],
+            "壹": [125, 18, 255, 0, 0, 0],
+            "贰": [92, 87, 255, 255, 0, 0],
+            "叁": [92, 87, 255, 255, 255, 0],
+            "肆": [92, 87, 255, 255, 255, 255],
+            "伍": [255, 255, 255, 255, 255, 255],
+            "OK": [139, 91, 103, 250, 250, 250],
+            "点赞": [250, 79, 0, 0, 0, 0],
+            "握拳": [102, 18, 0, 0, 0, 0],
+        }
+    ),
+    "L6": HandConfig(
+        joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"],
+        joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"],
+        init_pos=[250] * 6,
+        preset_actions={
+            "张开": [250, 250, 250, 250, 250, 250],
+            "壹": [0, 18, 255, 0, 0, 0],
+            "贰": [0, 39, 255, 255, 0, 0],
+            "叁": [0, 39, 255, 255, 255, 0],
+            "肆": [0, 0, 255, 255, 255, 255],
+            "伍": [255, 255, 255, 255, 255, 255],
+            "OK": [74, 13, 153, 255, 255, 255],
+            "点赞": [255, 255, 0, 0, 0, 0],
+            "握拳": [79, 11, 0, 0, 0, 0],
+            "序列动作1": [250, 250, 250, 250, 250, 250],
+            "序列动作2": [250, 250, 0, 250, 250, 0],
+            "序列动作3": [250, 250, 0, 0, 0, 0],
+            "序列动作4": [250, 250, 0, 0, 0, 255],
+            "序列动作5": [250, 250, 0, 0, 255, 255],
+            "序列动作6": [250, 250, 0, 255, 255, 255],
+            "序列动作7": [250, 250, 250, 250, 250, 250],
+            "食指压感准备1": [0, 18, 255, 0, 0, 0],
+            "食指压感测试": [9, 42, 55, 250, 250, 250],
+            "食指压感准备2": [0, 18, 255, 0, 0, 0],
+            "拇指压感准备1": [139, 18, 130, 0, 0, 0],
+            "拇指压感测试": [39, 30, 122, 250, 250, 250],
+            "拇指压感准备2": [139, 18, 130, 0, 0, 0]
+        }
+    ),
+}
+HAND_CONFIGS = MappingProxyType(_HAND_CONFIGS)

linker_hand_ros2_sdk_ws/src/gui_control/gui_control/gui_control.py

@@ -0,0 +1,808 @@
+import sys
+import time, json
+import threading
+from dataclasses import dataclass
+from typing import List, Dict
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String, Header
+from sensor_msgs.msg import JointState
+from PyQt5.QtCore import Qt, pyqtSignal, QTimer, QObject, QEvent
+from PyQt5.QtWidgets import (
+    QApplication, QWidget, QVBoxLayout, QHBoxLayout, QGridLayout, 
+    QSlider, QLabel, QPushButton, QGroupBox, QScrollArea, QTabWidget, 
+    QFrame, QSplitter, QMessageBox, QTextEdit
+)
+from PyQt5.QtGui import QFont
+
+from .utils.mapping import *
+
+from .config.constants import _HAND_CONFIGS
+LOOP_TIME = 1000 # 循环动作间隔时间 毫秒
+class ROS2NodeManager(QObject):
+    """ROS2节点管理器，处理ROS通信"""
+    status_updated = pyqtSignal(str, str)  # 状态类型, 消息内容
+
+    def __init__(self, node_name: str = "hand_control_node"):
+        super().__init__()
+        self.node = None
+        self.publisher = None
+        self.joint_state = JointState()
+        self.joint_state.header = Header()
+        
+        # 初始化ROS2节点
+        self.init_node(node_name)
+
+    def init_node(self, node_name: str):
+        """初始化ROS2节点"""
+        try:
+            if not rclpy.ok():
+                rclpy.init(args=None)
+            self.node = Node(node_name)
+            
+            # 声明参数
+            self.node.declare_parameter('hand_type', 'left')
+            self.node.declare_parameter('hand_joint', 'L10')
+            self.node.declare_parameter('topic_hz', 30)
+            self.node.declare_parameter('is_arc', False)
+            
+            # 获取参数
+            self.hand_type = self.node.get_parameter('hand_type').value
+            self.hand_joint = self.node.get_parameter('hand_joint').value
+            self.hz = self.node.get_parameter('topic_hz').value
+            self.is_arc = self.node.get_parameter('is_arc').value
+            
+            if self.is_arc == True:
+                # 创建发布者
+                self.publisher_arc = self.node.create_publisher(
+                    JointState, f'/cb_{self.hand_type}_hand_control_cmd_arc', 10
+                )
+            # 创建发布者
+            self.publisher = self.node.create_publisher(
+                JointState, f'/cb_{self.hand_type}_hand_control_cmd', 10
+            )
+                    # 新增 speed / torque 发布者
+            self.speed_pub = self.node.create_publisher(
+                String, f'/cb_hand_setting_cmd', 10)
+            self.torque_pub = self.node.create_publisher(
+                String, f'/cb_hand_setting_cmd', 10)
+            self.status_updated.emit("info", f"ROS2节点初始化成功: {self.hand_type} {self.hand_joint}")
+            
+            # 启动ROS2自旋线程
+            self.spin_thread = threading.Thread(target=self.spin_node, daemon=True)
+            self.spin_thread.start()
+        except Exception as e:
+            self.status_updated.emit("error", f"ROS2初始化失败: {str(e)}")
+            raise
+
+    def spin_node(self):
+        """运行ROS2节点自旋循环"""
+        while rclpy.ok() and self.node:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+
+    def publish_joint_state(self, positions: List[int]):
+        """发布关节状态消息"""
+        if not self.publisher or not self.node:
+            self.status_updated.emit("error", "ROS2发布者未初始化")
+            return
+            
+        try:
+            self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
+            self.joint_state.position = [float(pos) for pos in positions]
+            # self.joint_state.velocity = [0.1] * len(positions)
+            # self.joint_state.effort = [0.01] * len(positions)
+            # 如果有关节名称，添加到消息中
+            #hand_config = HandConfig.from_hand_type(self.hand_joint)
+            hand_config = _HAND_CONFIGS[self.hand_joint]
+            if len(hand_config.joint_names) == len(positions):
+                if hand_config.joint_names_en != None:
+                    self.joint_state.name = hand_config.joint_names_en
+                else:
+                    self.joint_state.name = hand_config.joint_names
+                
+            self.publisher.publish(self.joint_state)
+            if self.is_arc == True:
+                if self.hand_joint == "O6":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left(positions,self.hand_joint)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right(positions,self.hand_joint)
+                elif self.hand_joint == "L7" or self.hand_joint == "L21" or self.hand_joint == "L25":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left(positions,self.hand_joint)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right(positions,self.hand_joint)
+                elif self.hand_joint == "L10":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left_10(positions)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right_10(positions)
+                elif self.hand_joint == "L20":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left_l20(positions)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right_l20(positions)
+                else:
+                    #print(f"当前{self.hand_joint} {self.hand_type}不支持弧度转换", flush=True)
+                    pass
+                self.joint_state.position = [float(pos) for pos in pose]
+                self.publisher_arc.publish(self.joint_state)
+            self.status_updated.emit("info", "关节状态已发布")
+        except Exception as e:
+            self.status_updated.emit("error", f"发布失败: {str(e)}")
+
+    def publish_speed(self, val: int):
+        joint_len = 0
+        if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+            joint_len = 6
+        elif self.hand_joint == "L7":
+            joint_len = 7
+        elif self.hand_joint == "L10":
+            joint_len = 10
+        else:
+            joint_len = 5
+        msg = String()
+        v = [val] * joint_len
+        data = {
+            "setting_cmd": "set_speed",
+            "params": {"hand_type":self.hand_type,"speed": v},
+        }
+        msg.data = json.dumps(data)
+        print(f"速度值：{v}", flush=True)
+        self.speed_pub.publish(msg)
+
+    def publish_torque(self, val: int):
+        joint_len = 0
+        if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+            joint_len = 6
+        elif self.hand_joint == "L7":
+            joint_len = 7
+        elif self.hand_joint == "L10":
+            joint_len = 10
+        else:
+            joint_len = 5
+        msg = String()
+        v = [val] * joint_len
+        data = {
+            "setting_cmd": "set_max_torque_limits",
+            "params": {"hand_type":self.hand_type,"torque": v},
+        }
+        
+        msg.data = json.dumps(data)
+        print(f"扭矩值：{v}", flush=True)
+        self.torque_pub.publish(msg)
+
+    def shutdown(self):
+        """关闭ROS2节点"""
+        if self.node:
+            self.node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+class HandControlGUI(QWidget):
+    """灵巧手控制界面"""
+    status_updated = pyqtSignal(str, str)  # 状态类型, 消息内容
+
+    def __init__(self, ros_manager: ROS2NodeManager):
+        super().__init__()
+        
+        # 循环控制变量
+        self.cycle_timer = None  # 循环定时器
+        self.current_action_index = -1  # 当前动作索引
+        self.preset_buttons = []  # 存储预设动作按钮引用
+        
+        # 设置ROS管理器
+        self.ros_manager = ros_manager
+        self.ros_manager.status_updated.connect(self.update_status)
+        
+        # 获取手部配置
+        self.hand_joint = self.ros_manager.hand_joint
+        self.hand_type = self.ros_manager.hand_type
+        self.hand_config = _HAND_CONFIGS[self.hand_joint]
+        
+        # 初始化UI
+        self.init_ui()
+        
+        # 设置定时器发布关节状态
+        self.publish_timer = QTimer(self)
+        self.publish_timer.setInterval(int(1000 / self.ros_manager.hz))
+        self.publish_timer.timeout.connect(self.publish_joint_state)
+        self.publish_timer.start()
+
+    def init_ui(self):
+        """初始化用户界面"""
+        # 设置窗口属性
+        self.setWindowTitle(f'灵巧手控制界面 - {self.hand_type} {self.hand_joint}')
+        self.setMinimumSize(1200, 900)
+        
+        # 设置样式
+        self.setStyleSheet("""
+            QWidget {
+                font-family: 'Microsoft YaHei', 'SimHei', sans-serif;
+                font-size: 12px;
+            }
+            QGroupBox {
+                border: 1px solid #CCCCCC;
+                border-radius: 6px;
+                margin-top: 6px;
+                padding: 10px;
+            }
+            QGroupBox::title {
+                subcontrol-origin: margin;
+                left: 10px;
+                padding: 0 5px 0 5px;
+                color: #165DFF;
+                font-weight: bold;
+            }
+            QSlider::groove:horizontal {
+                border: 1px solid #999999;
+                height: 8px;
+                border-radius: 4px;
+                background: #CCCCCC;
+                margin: 2px 0;
+            }
+            QSlider::handle:horizontal {
+                background: qlineargradient(x1:0, y1:0, x2:1, y2:1, stop:0 #165DFF, stop:1 #0E42D2);
+                border: 1px solid #5C8AFF;
+                width: 18px;
+                margin: -5px 0;
+                border-radius: 9px;
+            }
+            QPushButton {
+                background-color: #E0E0E0;
+                border: 1px solid #CCCCCC;
+                border-radius: 4px;
+                padding: 5px 10px;
+                min-width: 80px;
+            }
+            QPushButton:hover {
+                background-color: #F0F0F0;
+            }
+            QPushButton:pressed {
+                background-color: #D0D0D0;
+            }
+            QPushButton[category="preset"] {
+                background-color: #E6F7FF;
+                color: #1890FF;
+                border-color: #91D5FF;
+            }
+            QPushButton[category="preset"]:hover {
+                background-color: #B3E0FF;
+            }
+            QPushButton[category="action"] {
+                background-color: #FFF7E6;
+                color: #FA8C16;
+                border-color: #FFD591;
+            }
+            QPushButton[category="action"]:hover {
+                background-color: #FFE6B3;
+            }
+            QPushButton[category="danger"] {
+                background-color: #FFF1F0;
+                color: #F5222D;
+                border-color: #FFCCC7;
+            }
+            QPushButton[category="danger"]:hover {
+                background-color: #FFE8E6;
+            }
+            QLabel#StatusLabel {
+                padding: 5px;
+                border-radius: 4px;
+            }
+            QLabel#StatusInfo {
+                background-color: #F0F7FF;
+                color: #0066CC;
+            }
+            QLabel#StatusError {
+                background-color: #FFF0F0;
+                color: #CC0000;
+            }
+            /* 数值显示面板样式 */
+            QTextEdit#ValueDisplay {
+                background-color: #F8F8F8;
+                border: 1px solid #CCCCCC;
+                border-radius: 4px;
+                padding: 10px;
+                font-family: Consolas, monospace;
+                font-size: 12px;
+            }
+        """)
+        
+        # 创建主垂直布局
+        main_layout = QVBoxLayout(self)
+        
+        # 创建水平分割器（原有三个面板）
+        splitter = QSplitter(Qt.Horizontal)
+        
+        # 创建左侧关节控制面板
+        self.joint_control_panel = self.create_joint_control_panel()
+        splitter.addWidget(self.joint_control_panel)
+        
+        # 创建中间预设动作面板
+        self.preset_actions_panel = self.create_preset_actions_panel()
+        splitter.addWidget(self.preset_actions_panel)
+        
+        # 创建右侧状态监控面板
+        self.status_monitor_panel = self.create_status_monitor_panel()
+        splitter.addWidget(self.status_monitor_panel)
+        
+        # 设置分割器比例
+        splitter.setSizes([500, 300, 400])
+        
+        # 添加分割器到主布局，并设置拉伸因子为1（可伸缩）
+        main_layout.addWidget(splitter, stretch=1)
+        
+        # 创建并添加数值显示面板，设置拉伸因子为0（不可伸缩）
+        self.value_display_panel = self.create_value_display_panel()
+        main_layout.addWidget(self.value_display_panel, stretch=0)
+        
+        # 初始更新数值显示
+        self.update_value_display()
+
+    def create_joint_control_panel(self):
+        """创建关节控制面板"""
+        panel = QWidget()
+        layout = QVBoxLayout(panel)
+        
+        # 创建标题
+        title_label = QLabel(f"关节控制 - {self.hand_joint}")
+        title_label.setFont(QFont("Microsoft YaHei", 14, QFont.Bold))
+        layout.addWidget(title_label)
+
+        # 创建滑动条滚动区域
+        scroll_area = QScrollArea()
+        scroll_area.setWidgetResizable(True)
+        scroll_area.setFrameShape(QFrame.NoFrame)
+        
+        scroll_content = QWidget()
+        self.sliders_layout = QGridLayout(scroll_content)
+        self.sliders_layout.setSpacing(10)
+        
+        # 创建滑动条
+        self.create_joint_sliders()
+        
+        scroll_area.setWidget(scroll_content)
+        layout.addWidget(scroll_area)
+        
+        return panel
+
+    
+
+    def create_joint_sliders(self):
+        """创建关节滑动条"""
+        # 清除现有滑动条
+        for i in reversed(range(self.sliders_layout.count())):
+            item = self.sliders_layout.itemAt(i)
+            if item.widget():
+                item.widget().deleteLater()
+        
+        # 创建新滑动条
+        self.sliders = []
+        self.slider_labels = []
+        
+        for i, (name, value) in enumerate(zip(
+            self.hand_config.joint_names, self.hand_config.init_pos
+        )):
+            # 创建标签
+            label = QLabel(f"{name}: {value}")
+            label.setMinimumWidth(120)
+            
+            # 创建滑动条
+            slider = QSlider(Qt.Horizontal)
+            slider.setRange(0, 255)
+            slider.setValue(value)
+            slider.valueChanged.connect(
+                lambda val, idx=i: self.on_slider_value_changed(idx, val)
+            )
+            
+            # 添加到布局
+            row, col = divmod(i, 1)
+            self.sliders_layout.addWidget(label, row, 0)
+            self.sliders_layout.addWidget(slider, row, 1)
+            
+            self.sliders.append(slider)
+            self.slider_labels.append(label)
+
+    def create_preset_actions_panel(self):
+        """创建预设动作面板"""
+        panel = QWidget()
+        layout = QVBoxLayout(panel)
+        
+        # 自定义预设动作
+        sys_preset_group = QGroupBox("自定义预设动作(名称不可重复)")
+        sys_preset_layout = QGridLayout(sys_preset_group)
+        sys_preset_layout.setSpacing(8)
+        
+        # 添加系统预设动作按钮
+        self.create_system_preset_buttons(sys_preset_layout)
+        layout.addWidget(sys_preset_group)
+        
+        # 添加动作按钮
+        actions_layout = QHBoxLayout()
+        
+        # 添加循环运行按钮
+        self.cycle_button = QPushButton("循环预设动作")
+        self.cycle_button.setProperty("category", "action")
+        self.cycle_button.clicked.connect(self.on_cycle_clicked)
+        actions_layout.addWidget(self.cycle_button)
+        
+        self.home_button = QPushButton("回到初始位置")
+        self.home_button.setProperty("category", "action")
+        self.home_button.clicked.connect(self.on_home_clicked)
+        actions_layout.addWidget(self.home_button)
+        
+        self.stop_button = QPushButton("停止所有动作")
+        self.stop_button.setProperty("category", "danger")
+        self.stop_button.clicked.connect(self.on_stop_clicked)
+        actions_layout.addWidget(self.stop_button)
+        
+        layout.addLayout(actions_layout)
+        
+        return panel
+
+    def create_system_preset_buttons(self, parent_layout):
+        """创建系统预设动作按钮"""
+        self.preset_buttons = []  # 清空按钮列表
+        if self.hand_config.preset_actions:
+            buttons = []
+            for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()):
+                button = QPushButton(name)
+                button.setProperty("category", "preset")
+                button.clicked.connect(
+                    lambda checked, pos=positions: self.on_preset_action_clicked(pos)
+                )
+                buttons.append(button)
+                self.preset_buttons.append(button)  # 保存按钮引用
+                
+            # 添加到网格布局
+            cols = 2
+            for i, button in enumerate(buttons):
+                row, col = divmod(i, cols)
+                parent_layout.addWidget(button, row, col)
+
+    def create_status_monitor_panel(self):
+        """创建状态监控面板（速度/扭矩各占一行，并实时显示滑块值）"""
+        panel = QWidget()
+        layout = QVBoxLayout(panel)
+
+        # —— 1. 标题 ——
+        title_label = QLabel("状态监控")
+        title_label.setFont(QFont("Microsoft YaHei", 14, QFont.Bold))
+        layout.addWidget(title_label)
+
+        # —— 2. 新增：速度与扭矩设置（每行一个）——
+        quick_set_gb = QGroupBox("快速设置")
+        qv_layout = QVBoxLayout(quick_set_gb)
+
+        # 速度行
+        speed_hbox = QHBoxLayout()
+        speed_hbox.addWidget(QLabel("速度:"))
+        self.speed_slider = QSlider(Qt.Horizontal)
+        self.speed_slider.setRange(0, 255)
+        self.speed_slider.setValue(255)
+        self.speed_slider.setMinimumWidth(150)
+        speed_hbox.addWidget(self.speed_slider)
+        self.speed_val_lbl = QLabel("255")          # 实时值
+        self.speed_val_lbl.setMinimumWidth(30)
+        speed_hbox.addWidget(self.speed_val_lbl)
+        self.speed_btn = QPushButton("设置速度")
+        self.speed_btn.clicked.connect(
+            lambda: (
+                self.ros_manager.publish_speed(self.speed_slider.value()),
+                self.status_updated.emit(
+                    "info", f"速度已设为 {self.speed_slider.value()}")
+            ))
+        speed_hbox.addWidget(self.speed_btn)
+        speed_hbox.addStretch()
+        qv_layout.addLayout(speed_hbox)
+
+        # 扭矩行
+        torque_hbox = QHBoxLayout()
+        torque_hbox.addWidget(QLabel("扭矩:"))
+        self.torque_slider = QSlider(Qt.Horizontal)
+        self.torque_slider.setRange(0, 255)
+        self.torque_slider.setValue(255)
+        self.torque_slider.setMinimumWidth(150)
+        torque_hbox.addWidget(self.torque_slider)
+        self.torque_val_lbl = QLabel("255")
+        self.torque_val_lbl.setMinimumWidth(30)
+        torque_hbox.addWidget(self.torque_val_lbl)
+        self.torque_btn = QPushButton("设置扭矩")
+        self.torque_btn.clicked.connect(
+            lambda: (
+                self.ros_manager.publish_torque(self.torque_slider.value()),
+                self.status_updated.emit(
+                    "info", f"扭矩已设为 {self.torque_slider.value()}")
+            ))
+        torque_hbox.addWidget(self.torque_btn)
+        torque_hbox.addStretch()
+        qv_layout.addLayout(torque_hbox)
+
+        layout.addWidget(quick_set_gb)
+
+        # —— 3. 原有标签页部分，完全不动 ——
+        tab_widget = QTabWidget()
+
+        # 系统信息标签页
+        sys_info_widget = QWidget()
+        sys_info_layout = QVBoxLayout(sys_info_widget)
+
+        conn_group = QGroupBox("连接状态")
+        conn_layout = QVBoxLayout(conn_group)
+        if self.ros_manager.publisher.get_subscription_count() > 0:
+            self.connection_status = QLabel("ROS2节点已连接")
+            self.connection_status.setObjectName("StatusLabel")
+            self.connection_status.setObjectName("StatusInfo")
+        else:
+            self.connection_status = QLabel("ROS2节点未连接")
+            self.connection_status.setObjectName("StatusLabel")
+            self.connection_status.setObjectName("StatusError")
+        conn_layout.addWidget(self.connection_status)
+
+        hand_info_group = QGroupBox("手部信息")
+        hand_info_layout = QVBoxLayout(hand_info_group)
+        info_text = f"""手部类型: {self.hand_type}
+关节型号: {self.hand_joint}
+关节数量: {len(self.hand_config.joint_names)}
+发布频率: {self.ros_manager.hz} Hz"""
+        self.hand_info_label = QLabel(info_text)
+        self.hand_info_label.setWordWrap(True)
+        hand_info_layout.addWidget(self.hand_info_label)
+
+        sys_info_layout.addWidget(conn_group)
+        sys_info_layout.addWidget(hand_info_group)
+        sys_info_layout.addStretch()
+        tab_widget.addTab(sys_info_widget, "系统信息")
+
+        # 状态日志标签页
+        log_widget = QWidget()
+        log_layout = QVBoxLayout(log_widget)
+        self.status_log = QLabel("等待系统启动...")
+        self.status_log.setObjectName("StatusLabel")
+        self.status_log.setObjectName("StatusInfo")
+        self.status_log.setWordWrap(True)
+        self.status_log.setMinimumHeight(300)
+        log_layout.addWidget(self.status_log)
+        clear_log_btn = QPushButton("清除日志")
+        clear_log_btn.clicked.connect(self.clear_status_log)
+        log_layout.addWidget(clear_log_btn)
+        tab_widget.addTab(log_widget, "状态日志")
+
+        layout.addWidget(tab_widget)
+
+        # —— 4. 实时更新滑块值 ——
+        self.speed_slider.valueChanged.connect(
+            lambda v: self.speed_val_lbl.setText(str(v)))
+        self.torque_slider.valueChanged.connect(
+            lambda v: self.torque_val_lbl.setText(str(v)))
+        return panel
+
+    def create_value_display_panel(self):
+        """创建滑动条数值显示面板"""
+        panel = QGroupBox("关节数值列表")
+        layout = QVBoxLayout(panel)
+        
+        # 设置布局上下间隔为20像素
+        layout.setContentsMargins(10, 20, 10, 20)
+        
+        self.value_display = QTextEdit()
+        self.value_display.setObjectName("ValueDisplay")
+        self.value_display.setReadOnly(True)  # 设置只读模式，允许复制
+        self.value_display.setMinimumHeight(60)  # 调整最小高度
+        self.value_display.setMaximumHeight(80)  # 限制最大高度
+        self.value_display.setText("[]")
+        
+        layout.addWidget(self.value_display)
+        
+        return panel
+
+    def on_slider_value_changed(self, index: int, value: int):
+        """滑动条值改变事件处理"""
+        if 0 <= index < len(self.slider_labels):
+            joint_name = self.hand_config.joint_names[index]
+            self.slider_labels[index].setText(f"{joint_name}: {value}")
+            
+        # 更新数值显示
+        self.update_value_display()
+
+    def update_value_display(self):
+        """更新数值显示面板内容"""
+        # 获取所有滑动条的当前值
+        values = [slider.value() for slider in self.sliders]
+        
+        # 格式化显示为列表形式
+        self.value_display.setText(f"{values}")
+
+    def on_preset_action_clicked(self, positions: List[int]):
+        """预设动作按钮点击事件处理"""
+        if len(positions) != len(self.sliders):
+            QMessageBox.warning(
+                self, "动作不匹配", 
+                f"预设动作关节数量({len(positions)})与当前关节数量({len(self.sliders)})不匹配"
+            )
+            return
+            
+        # 更新滑动条
+        for i, (slider, pos) in enumerate(zip(self.sliders, positions)):
+            slider.setValue(pos)
+            self.on_slider_value_changed(i, pos)
+            
+        # 发布关节状态
+        self.publish_joint_state()
+
+    def on_home_clicked(self):
+        """回到初始位置按钮点击事件处理"""
+        for slider, pos in zip(self.sliders, self.hand_config.init_pos):
+            slider.setValue(pos)
+            
+        self.publish_joint_state()
+        self.status_updated.emit("info", "回到初始位置")
+        
+        # 更新数值显示
+        self.update_value_display()
+
+    def on_stop_clicked(self):
+        """停止所有动作按钮点击事件处理"""
+        # 停止循环定时器
+        if self.cycle_timer and self.cycle_timer.isActive():
+            self.cycle_timer.stop()
+            self.cycle_timer = None
+            self.cycle_button.setText("循环运行预设动作")
+            self.reset_preset_buttons_color()
+            
+        self.status_updated.emit("warning", "已停止所有动作")
+
+    def on_cycle_clicked(self):
+        """循环运行预设动作按钮点击事件处理"""
+        if not self.hand_config.preset_actions:
+            QMessageBox.warning(self, "无预设动作", "当前手部型号没有预设动作可循环运行")
+            return
+            
+        if self.cycle_timer and self.cycle_timer.isActive():
+            # 停止循环
+            self.cycle_timer.stop()
+            self.cycle_timer = None
+            self.cycle_button.setText("循环运行预设动作")
+            self.reset_preset_buttons_color()
+            self.status_updated.emit("info", "已停止循环运行预设动作")
+        else:
+            # 开始循环
+            self.current_action_index = -1  # 重置索引
+            self.cycle_timer = QTimer(self)
+            self.cycle_timer.timeout.connect(self.run_next_action)
+            self.cycle_timer.start(LOOP_TIME)  # 2秒间隔
+            self.cycle_button.setText("停止循环运行")
+            self.status_updated.emit("info", "开始循环运行预设动作")
+            self.run_next_action()  # 立即运行第一个动作
+
+    def run_next_action(self):
+        """运行下一个预设动作"""
+        if not self.hand_config.preset_actions:
+            return
+            
+        # 重置所有按钮颜色
+        self.reset_preset_buttons_color()
+        
+        # 计算下一个动作索引
+        self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions)
+        
+        # 获取下一个动作
+        action_names = list(self.hand_config.preset_actions.keys())
+        action_name = action_names[self.current_action_index]
+        action_positions = self.hand_config.preset_actions[action_name]
+        
+        # 执行动作
+        self.on_preset_action_clicked(action_positions)
+        
+        # 高亮当前动作按钮
+        if 0 <= self.current_action_index < len(self.preset_buttons):
+            button = self.preset_buttons[self.current_action_index]
+            button.setStyleSheet("background-color: green; color: white; border-color: #91D5FF;")
+            
+        self.status_updated.emit("info", f"运行预设动作: {action_name}")
+
+    def reset_preset_buttons_color(self):
+        """重置所有预设按钮颜色"""
+        for button in self.preset_buttons:
+            button.setStyleSheet("")  # 恢复默认样式
+            button.setProperty("category", "preset")  # 恢复类别属性
+            # 强制样式刷新
+            button.style().unpolish(button)
+            button.style().polish(button)
+
+    def on_joint_type_changed(self, joint_type: str):
+        """关节类型改变事件处理"""
+        self.hand_joint = joint_type
+        self.hand_config = _HAND_CONFIGS[self.hand_joint]
+        
+        # 更新手部信息
+        info_text = f"""手部类型: {self.hand_type}
+关节型号: {self.hand_joint}
+关节数量: {len(self.hand_config.joint_names)}
+发布频率: {self.ros_manager.hz} Hz"""
+        self.hand_info_label.setText(info_text)
+        
+        # 重新创建滑动条和预设按钮
+        self.create_joint_sliders()
+        self.create_system_preset_buttons(self.sys_preset_layout)  # 假设sys_preset_layout是类变量
+        
+        # 更新数值显示
+        self.update_value_display()
+        self.status_updated.emit("info", f"已切换到手部型号: {joint_type}")
+
+    def publish_joint_state(self):
+        """发布当前关节状态"""
+        positions = [slider.value() for slider in self.sliders]
+        self.ros_manager.publish_joint_state(positions)
+
+    def update_status(self, status_type: str, message: str):
+        """更新状态显示"""
+        # 更新连接状态
+        if status_type == "info" and "ROS2节点初始化成功" in message:
+            self.connection_status.setText("ROS2节点已连接")
+            self.connection_status.setObjectName("StatusLabel")
+            self.connection_status.setObjectName("StatusInfo")
+            
+        # 更新日志
+        current_time = time.strftime("%H:%M:%S")
+        log_entry = f"[{current_time}] {message}\n"
+        current_log = self.status_log.text()
+        
+        if len(current_log) > 10000:  # 限制日志长度
+            current_log = current_log[-10000:]
+            
+        self.status_log.setText(log_entry + current_log)
+        
+        # 设置日志样式
+        self.status_log.setObjectName("StatusLabel")
+        if status_type == "error":
+            self.status_log.setObjectName("StatusError")
+        else:
+            self.status_log.setObjectName("StatusInfo")
+
+    def clear_status_log(self):
+        """清除状态日志"""
+        self.status_log.setText("日志已清除")
+        self.status_log.setObjectName("StatusLabel")
+        self.status_log.setObjectName("StatusInfo")
+
+    def closeEvent(self, event):
+        """窗口关闭事件处理"""
+        if self.cycle_timer and self.cycle_timer.isActive():
+            self.cycle_timer.stop()
+        super().closeEvent(event)
+
+def main(args=None):
+    """主函数"""
+    try:
+        # 创建ROS2节点管理器
+        ros_manager = ROS2NodeManager()
+        
+        # 创建Qt应用
+        app = QApplication(sys.argv)
+        
+        # 创建GUI
+        window = HandControlGUI(ros_manager)
+        
+        # 连接状态更新信号
+        ros_manager.status_updated.connect(window.update_status)
+        window.status_updated = ros_manager.status_updated
+        
+        # 显示窗口
+        window.show()
+        
+        # 运行应用
+        exit_code = app.exec_()
+        
+        # 清理ROS2
+        if rclpy.ok():
+            ros_manager.node.destroy_node()
+            rclpy.shutdown()
+            
+        sys.exit(exit_code)
+    except Exception as e:
+        print(f"应用程序启动失败: {str(e)}")
+        sys.exit(1)
+
+if __name__ == '__main__':
+    main()

linker_hand_ros2_sdk_ws/src/gui_control/gui_control/gui_control.py.ttk

@@ -0,0 +1,790 @@
+import sys
+import time
+import json
+import threading
+from dataclasses import dataclass
+from typing import List, Dict
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String, Header
+from sensor_msgs.msg import JointState
+import tkinter as tk
+from tkinter import ttk, scrolledtext, messagebox
+import tkinter.font as tkfont
+
+from .utils.mapping import *
+from .config.constants import _HAND_CONFIGS
+LOOP_TIME = 1000 # 循环动作间隔时间 毫秒
+class ROS2NodeManager:
+    """ROS2节点管理器，处理ROS通信"""
+    
+    def __init__(self, node_name: str = "hand_control_node"):
+        self.node = None
+        self.publisher = None
+        self.joint_state = JointState()
+        self.joint_state.header = Header()
+        self.status_callbacks = []
+        
+        # 初始化ROS2节点
+        self.init_node(node_name)
+
+    def add_status_callback(self, callback):
+        """添加状态回调函数"""
+        self.status_callbacks.append(callback)
+
+    def emit_status(self, status_type: str, message: str):
+        """发射状态信号"""
+        for callback in self.status_callbacks:
+            callback(status_type, message)
+
+    def init_node(self, node_name: str):
+        """初始化ROS2节点"""
+        try:
+            if not rclpy.ok():
+                rclpy.init(args=None)
+            self.node = Node(node_name)
+            
+            # 声明参数
+            self.node.declare_parameter('hand_type', 'left')
+            self.node.declare_parameter('hand_joint', 'L10')
+            self.node.declare_parameter('topic_hz', 30)
+            self.node.declare_parameter('is_arc', False)
+            
+            # 获取参数
+            self.hand_type = self.node.get_parameter('hand_type').value
+            self.hand_joint = self.node.get_parameter('hand_joint').value
+            self.hz = self.node.get_parameter('topic_hz').value
+            self.is_arc = self.node.get_parameter('is_arc').value
+            
+            if self.is_arc == True:
+                # 创建发布者
+                self.publisher_arc = self.node.create_publisher(
+                    JointState, f'/cb_{self.hand_type}_hand_control_cmd_arc', 10
+                )
+            # 创建发布者
+            self.publisher = self.node.create_publisher(
+                JointState, f'/cb_{self.hand_type}_hand_control_cmd', 10
+            )
+            # 新增 speed / torque 发布者
+            self.speed_pub = self.node.create_publisher(
+                String, f'/cb_hand_setting_cmd', 10)
+            self.torque_pub = self.node.create_publisher(
+                String, f'/cb_hand_setting_cmd', 10)
+            self.emit_status("info", f"ROS2节点初始化成功: {self.hand_type} {self.hand_joint}")
+            
+            # 启动ROS2自旋线程
+            self.spin_thread = threading.Thread(target=self.spin_node, daemon=True)
+            self.spin_thread.start()
+        except Exception as e:
+            self.emit_status("error", f"ROS2初始化失败: {str(e)}")
+            raise
+
+    def spin_node(self):
+        """运行ROS2节点自旋循环"""
+        while rclpy.ok() and self.node:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+
+    def publish_joint_state(self, positions: List[int]):
+        """发布关节状态消息"""
+        if not self.publisher or not self.node:
+            self.emit_status("error", "ROS2发布者未初始化")
+            return
+            
+        try:
+            self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
+            self.joint_state.position = [float(pos) for pos in positions]
+            hand_config = _HAND_CONFIGS[self.hand_joint]
+            if len(hand_config.joint_names) == len(positions):
+                if hand_config.joint_names_en != None:
+                    self.joint_state.name = hand_config.joint_names_en
+                else:
+                    self.joint_state.name = hand_config.joint_names
+                
+            self.publisher.publish(self.joint_state)
+            if self.is_arc == True:
+                if self.hand_joint == "O6":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left(positions,self.hand_joint)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right(positions,self.hand_joint)
+                elif self.hand_joint == "L7" or self.hand_joint == "L21" or self.hand_joint == "L25":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left(positions,self.hand_joint)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right(positions,self.hand_joint)
+                elif self.hand_joint == "L10":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left_10(positions)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right_10(positions)
+                elif self.hand_joint == "L20":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left_l20(positions)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right_l20(positions)
+                else:
+                    print(f"当前{self.hand_joint} {self.hand_type}不支持弧度转换", flush=True)
+                self.joint_state.position = [float(pos) for pos in pose]
+                self.publisher_arc.publish(self.joint_state)
+            self.emit_status("info", "关节状态已发布")
+        except Exception as e:
+            self.emit_status("error", f"发布失败: {str(e)}")
+
+    def publish_speed(self, val: int):
+        joint_len = 0
+        if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+            joint_len = 6
+        elif self.hand_joint == "L7":
+            joint_len = 7
+        elif self.hand_joint == "L10":
+            joint_len = 10
+        else:
+            joint_len = 5
+        msg = String()
+        v = [val] * joint_len
+        data = {
+            "setting_cmd": "set_speed",
+            "params": {"hand_type":self.hand_type,"speed": v},
+        }
+        msg.data = json.dumps(data)
+        print(f"速度值：{v}", flush=True)
+        self.speed_pub.publish(msg)
+
+    def publish_torque(self, val: int):
+        joint_len = 0
+        if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+            joint_len = 6
+        elif self.hand_joint == "L7":
+            joint_len = 7
+        elif self.hand_joint == "L10":
+            joint_len = 10
+        else:
+            joint_len = 5
+        msg = String()
+        v = [val] * joint_len
+        data = {
+            "setting_cmd": "set_max_torque_limits",
+            "params": {"hand_type":self.hand_type,"torque": v},
+        }
+        
+        msg.data = json.dumps(data)
+        print(f"扭矩值：{v}", flush=True)
+        self.torque_pub.publish(msg)
+
+    def shutdown(self):
+        """关闭ROS2节点"""
+        if self.node:
+            self.node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+class HandControlGUI:
+    """灵巧手控制界面"""
+    
+    def __init__(self, ros_manager: ROS2NodeManager):
+        self.ros_manager = ros_manager
+        self.ros_manager.add_status_callback(self.update_status)
+        
+        # 获取手部配置
+        self.hand_joint = self.ros_manager.hand_joint
+        self.hand_type = self.ros_manager.hand_type
+        self.hand_config = _HAND_CONFIGS[self.hand_joint]
+        
+        # 循环控制变量
+        self.cycle_timer = None
+        self.current_action_index = -1
+        self.preset_buttons = []
+        
+        # 初始化UI
+        self.init_ui()
+        
+        # 设置定时器发布关节状态
+        self.publish_timer_id = None
+        self.start_publish_timer()
+
+    def init_ui(self):
+        """初始化用户界面"""
+        # 创建主窗口
+        self.root = tk.Tk()
+        self.root.title(f'灵巧手控制界面 - {self.hand_type} {self.hand_joint}')
+        self.root.geometry('1300x700')
+        
+        # 设置样式
+        self.style = ttk.Style()
+        self.style.configure('TFrame', background='#f0f0f0')
+        self.style.configure('TLabel', background='#f0f0f0', font=('Microsoft YaHei', 10))
+        self.style.configure('Title.TLabel', font=('Microsoft YaHei', 14, 'bold'), foreground='#165DFF')
+        self.style.configure('Group.TLabelframe', borderwidth=2, relief='groove')
+        self.style.configure('Group.TLabelframe.Label', font=('Microsoft YaHei', 10, 'bold'), foreground='#165DFF')
+        # 配置信息框样式 - 灰色背景
+        self.style.configure('Info.TFrame', background='#e0e0e0')
+        # 配置高亮样式 - 滑动时的背景色
+        self.style.configure('Highlight.TFrame', background='#e6f7ff')  # 浅蓝色背景
+        
+        # 初始化当前点击按钮索引
+        self.current_clicked_button = None
+        
+        # 创建主框架
+        main_frame = ttk.Frame(self.root)
+        main_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=10)
+        
+        # 创建水平分割的框架
+        self.paned_window = ttk.PanedWindow(main_frame, orient=tk.HORIZONTAL)
+        self.paned_window.pack(fill=tk.BOTH, expand=True)
+        
+        # 创建左侧关节控制面板
+        self.joint_control_panel = self.create_joint_control_panel()
+        self.paned_window.add(self.joint_control_panel, weight=5)
+        
+        # 创建中间预设动作面板
+        self.preset_actions_panel = self.create_preset_actions_panel()
+        self.paned_window.add(self.preset_actions_panel, weight=3)
+        
+        # 创建右侧状态监控面板
+        self.status_monitor_panel = self.create_status_monitor_panel()
+        self.paned_window.add(self.status_monitor_panel, weight=4)
+        
+        # 创建底部数值显示面板
+        self.value_display_panel = self.create_value_display_panel()
+        self.value_display_panel.pack(fill=tk.X, pady=(10, 0))
+        
+        # 绑定窗口大小变化事件
+        self.joint_control_panel.bind('<Configure>', self.on_joint_panel_resize)
+        
+        # 初始更新数值显示
+        self.update_value_display()
+
+    def create_joint_control_panel(self):
+        """创建关节控制面板"""
+        frame = ttk.Frame(self.root)
+        frame.config(width=600)  # 设置最小宽度为400像素
+        frame.pack_propagate(False)  # 阻止子控件改变框架大小
+        # 创建标题
+        title_label = ttk.Label(frame, text=f"关节控制 - {self.hand_joint}", style='Title.TLabel')
+        title_label.pack(pady=(0, 10))
+        
+        # 创建滚动框架
+        self.canvas = tk.Canvas(frame, bg='#f0f0f0', highlightthickness=0)
+        self.scrollbar = ttk.Scrollbar(frame, orient=tk.VERTICAL, command=self.canvas.yview)
+        self.scrollable_frame = ttk.Frame(self.canvas)
+        
+        self.scrollable_frame.bind(
+            "<Configure>",
+            self.on_scrollable_frame_configure  # 修改为调用方法
+        )
+        
+        self.canvas.create_window((0, 0), window=self.scrollable_frame, anchor="nw")
+        self.canvas.configure(yscrollcommand=self.scrollbar.set)
+        
+        # 创建滑动条
+        self.create_joint_sliders(self.scrollable_frame)
+        
+        self.canvas.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)
+        # 初始时不显示滚动条
+        self.scrollbar.pack(side=tk.RIGHT, fill=tk.Y)
+        self.scrollbar.pack_forget()  # 隐藏滚动条
+        
+        return frame
+    def on_scrollable_frame_configure(self, event):
+        """滚动区域配置变化事件 - 动态显示/隐藏滚动条"""
+        # 更新滚动区域
+        self.canvas.configure(scrollregion=self.canvas.bbox("all"))
+        
+        # 检查是否需要显示滚动条
+        scrollable_height = self.scrollable_frame.winfo_reqheight()  # 内容所需高度
+        canvas_height = self.canvas.winfo_height()  # 画布实际高度
+        
+        # 如果内容高度大于画布高度，显示滚动条；否则隐藏
+        if scrollable_height > canvas_height and canvas_height > 0:
+            self.scrollbar.pack(side=tk.RIGHT, fill=tk.Y)
+        else:
+            self.scrollbar.pack_forget()
+
+    def create_joint_sliders(self, parent):
+        """创建关节滑动条"""
+        self.sliders = []
+        self.slider_labels = []
+        self.slider_frames = []
+        
+        # 存储父框架的引用，用于后续调整大小
+        self.sliders_parent = parent
+        
+        for i, (name, value) in enumerate(zip(
+            self.hand_config.joint_names, self.hand_config.init_pos
+        )):
+            # 创建框架 - 添加左右边距
+            slider_frame = ttk.Frame(parent)
+            slider_frame.pack(fill=tk.X, pady=5, padx=(10,0))  # 左右各10像素边距
+            self.slider_frames.append(slider_frame)
+            
+            # 创建标签
+            label = ttk.Label(slider_frame, text=f"{name}: {value}", width=15)
+            label.pack(side=tk.LEFT, padx=(0, 10))
+            
+            # 创建滑动条 - 初始长度设为300，但会随窗口调整
+            slider = ttk.Scale(slider_frame, from_=0, to=255, value=value, 
+                              orient=tk.HORIZONTAL, length=300,  # 初始长度
+                              command=lambda val, idx=i: self.on_slider_value_changed(idx, val))
+            slider.pack(side=tk.LEFT, fill=tk.X, expand=True)
+            
+            self.sliders.append(slider)
+            self.slider_labels.append(label)
+
+    def on_joint_panel_resize(self, event):
+        """关节控制面板大小变化事件"""
+        # 获取面板当前宽度
+        panel_width = event.width
+        
+        # 为所有滑动条设置新的长度
+        for slider in self.sliders:
+            # 计算新的滑动条长度（面板宽度减去标签和其他元素的估计宽度）
+            # 标签宽度约120像素 + 左右边距各10像素 + 标签与滑动条间距10像素
+            new_length = max(panel_width - 150, 100)  # 最小长度100像素
+            # 重新配置滑动条
+            slider.configure(length=new_length-30)
+        # 面板大小变化后重新检查是否需要滚动条
+        self.root.after(100, self.check_scrollbar_visibility)  # 延迟检查，确保高度已更新
+
+    def check_scrollbar_visibility(self):
+        """检查滚动条可见性"""
+        # 只有在画布已经有实际高度时才检查
+        if self.canvas.winfo_height() > 0:
+            scrollable_height = self.scrollable_frame.winfo_reqheight()
+            canvas_height = self.canvas.winfo_height()
+            
+            if scrollable_height > canvas_height:
+                self.scrollbar.pack(side=tk.RIGHT, fill=tk.Y)
+            else:
+                self.scrollbar.pack_forget()
+
+    def create_preset_actions_panel(self):
+        """创建预设动作面板"""
+        frame = ttk.Frame(self.root)
+        
+        # 系统预设动作
+        sys_preset_group = ttk.LabelFrame(frame, text="系统预设", style='Group.TLabelframe')
+        sys_preset_group.pack(fill=tk.BOTH, expand=True, pady=(0, 10))
+        
+        # 创建系统预设动作按钮 - 3列布局
+        self.create_system_preset_buttons(sys_preset_group)
+        
+        # 动作按钮框架
+        actions_frame = ttk.Frame(frame)
+        actions_frame.pack(fill=tk.X, pady=10)
+        
+        # 循环运行按钮
+        self.cycle_button = ttk.Button(actions_frame, text="循环预设动作", 
+                                      command=self.on_cycle_clicked, width=12)
+        self.cycle_button.pack(side=tk.LEFT, padx=5)
+        
+        # 回到初始位置按钮
+        self.home_button = ttk.Button(actions_frame, text="回到初始位置", 
+                                     command=self.on_home_clicked, width=12)
+        self.home_button.pack(side=tk.LEFT, padx=5)
+        
+        # 停止所有动作按钮
+        self.stop_button = ttk.Button(actions_frame, text="停止所有动作", 
+                                     command=self.on_stop_clicked, width=12)
+        self.stop_button.pack(side=tk.LEFT, padx=5)
+        
+        return frame
+
+    def create_system_preset_buttons(self, parent):
+        """创建系统预设动作按钮 - 3列布局"""
+        self.preset_buttons = []
+        if self.hand_config.preset_actions:
+            buttons_frame = ttk.Frame(parent)
+            buttons_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=10)
+            
+            buttons = []
+            for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()):
+                # 使用 tk.Button 而不是 ttk.Button，以便更好地控制背景色
+                button = tk.Button(buttons_frame, text=name, bg='#E6F7FF', fg='#1890FF', width=15,
+                                relief='raised', bd=1, font=('Microsoft YaHei', 10),
+                                command=lambda pos=positions, btn_idx=idx: self.on_preset_action_clicked(pos, btn_idx))
+                buttons.append(button)
+                self.preset_buttons.append(button)
+            
+            # 3列布局
+            cols = 3
+            for i, button in enumerate(buttons):
+                row, col = divmod(i, cols)
+                button.grid(row=row, column=col, sticky='ew', padx=5, pady=5)
+            
+            # 配置列权重
+            for i in range(cols):
+                buttons_frame.columnconfigure(i, weight=1)
+
+    def create_status_monitor_panel(self):
+        """创建状态监控面板"""
+        frame = ttk.Frame(self.root)
+        
+        # 标题
+        title_label = ttk.Label(frame, text="状态监控", style='Title.TLabel')
+        title_label.pack(pady=(0, 10))
+        
+        # 快速设置框架
+        quick_set_group = ttk.LabelFrame(frame, text="快速设置", style='Group.TLabelframe')
+        quick_set_group.pack(fill=tk.X, pady=(0, 10))
+        
+        # 速度设置
+        speed_frame = ttk.Frame(quick_set_group)
+        speed_frame.pack(fill=tk.X, padx=10, pady=5)
+        
+        ttk.Label(speed_frame, text="速度:").pack(side=tk.LEFT)
+        self.speed_var = tk.IntVar(value=255)
+        self.speed_slider = ttk.Scale(speed_frame, from_=0, to=255, 
+                                     variable=self.speed_var, orient=tk.HORIZONTAL)
+        self.speed_slider.pack(side=tk.LEFT, fill=tk.X, expand=True, padx=5)
+        self.speed_val_label = ttk.Label(speed_frame, text="255", width=4)
+        self.speed_val_label.pack(side=tk.LEFT)
+        self.speed_btn = ttk.Button(speed_frame, text="设置速度",
+                                   command=self.on_speed_set)
+        self.speed_btn.pack(side=tk.LEFT, padx=5)
+        
+        # 扭矩设置
+        torque_frame = ttk.Frame(quick_set_group)
+        torque_frame.pack(fill=tk.X, padx=10, pady=5)
+        
+        ttk.Label(torque_frame, text="扭矩:").pack(side=tk.LEFT)
+        self.torque_var = tk.IntVar(value=255)
+        self.torque_slider = ttk.Scale(torque_frame, from_=0, to=255, 
+                                      variable=self.torque_var, orient=tk.HORIZONTAL)
+        self.torque_slider.pack(side=tk.LEFT, fill=tk.X, expand=True, padx=5)
+        self.torque_val_label = ttk.Label(torque_frame, text="255", width=4)
+        self.torque_val_label.pack(side=tk.LEFT)
+        self.torque_btn = ttk.Button(torque_frame, text="设置扭矩",
+                                    command=self.on_torque_set)
+        self.torque_btn.pack(side=tk.LEFT, padx=5)
+        
+        # 绑定滑块值变化事件
+        self.speed_var.trace('w', self.on_speed_changed)
+        self.torque_var.trace('w', self.on_torque_changed)
+        
+        # 创建标签页
+        notebook = ttk.Notebook(frame)
+        notebook.pack(fill=tk.BOTH, expand=True)
+        
+        # 系统信息标签页
+        sys_info_frame = ttk.Frame(notebook)
+        notebook.add(sys_info_frame, text="系统信息")
+        
+        # 连接状态 - 灰色背景，左对齐
+        conn_group = ttk.LabelFrame(sys_info_frame, text="连接状态", style='Group.TLabelframe')
+        conn_group.pack(fill=tk.X, pady=5)
+        
+        # 创建灰色背景的框架
+        conn_content_frame = ttk.Frame(conn_group, style='Info.TFrame')
+        conn_content_frame.pack(fill=tk.X, padx=10, pady=10)
+        
+        if self.ros_manager.publisher.get_subscription_count() > 0:
+            self.connection_status = ttk.Label(conn_content_frame, text="ROS2节点已连接", 
+                                              foreground="green", background='#e0e0e0',
+                                              anchor='w')  # 左对齐
+        else:
+            self.connection_status = ttk.Label(conn_content_frame, text="ROS2节点未连接", 
+                                              foreground="red", background='#e0e0e0',
+                                              anchor='w')  # 左对齐
+        self.connection_status.pack(fill=tk.X)
+        
+        # 手部信息 - 灰色背景，左对齐
+        hand_info_group = ttk.LabelFrame(sys_info_frame, text="手部信息", style='Group.TLabelframe')
+        hand_info_group.pack(fill=tk.X, pady=5)
+        
+        # 创建灰色背景的框架
+        hand_info_content_frame = ttk.Frame(hand_info_group, style='Info.TFrame')
+        hand_info_content_frame.pack(fill=tk.X, padx=10, pady=10)
+        
+        info_text = f"""手部类型: {self.hand_type}
+关节型号: {self.hand_joint}
+关节数量: {len(self.hand_config.joint_names)}
+发布频率: {self.ros_manager.hz} Hz"""
+        self.hand_info_label = ttk.Label(hand_info_content_frame, text=info_text,
+                                        background='#e0e0e0', anchor='w', justify='left')  # 左对齐
+        self.hand_info_label.pack(fill=tk.X)
+        
+        # 状态日志标签页
+        log_frame = ttk.Frame(notebook)
+        notebook.add(log_frame, text="状态日志")
+        
+        # 日志文本框
+        self.status_log = scrolledtext.ScrolledText(log_frame, height=15, width=50)
+        self.status_log.pack(fill=tk.BOTH, expand=True, padx=10, pady=10)
+        self.status_log.insert(tk.END, "等待系统启动...\n")
+        self.status_log.config(state=tk.DISABLED)
+        
+        # 清除日志按钮
+        clear_log_btn = ttk.Button(log_frame, text="清除日志", 
+                                  command=self.clear_status_log)
+        clear_log_btn.pack(pady=5)
+        
+        return frame
+
+    def create_value_display_panel(self):
+        """创建滑动条数值显示面板"""
+        frame = ttk.LabelFrame(self.root, text="关节数值列表", style='Group.TLabelframe')
+        
+        # 创建按钮框架（放在显示框上方）
+        button_frame = ttk.Frame(frame)
+        button_frame.pack(fill=tk.X, padx=10, pady=(10, 5))
+        
+        # 复制按钮 - 左对齐
+        copy_button = ttk.Button(button_frame, text="复制到剪切板", 
+                            command=self.copy_values_to_clipboard, width=10)
+        copy_button.pack(side=tk.LEFT)
+        
+        # 数值显示框
+        self.value_display = scrolledtext.ScrolledText(frame, height=4, width=100)
+        self.value_display.pack(fill=tk.BOTH, expand=True, padx=10, pady=(0, 10))
+        self.value_display.insert(tk.END, "[]")
+        self.value_display.config(state=tk.DISABLED)
+        
+        return frame
+
+    def copy_values_to_clipboard(self):
+        """复制关节数值到系统剪切板"""
+        try:
+            # 获取当前按钮引用
+            button = self.root.focus_get()
+            original_text = "复制到剪切板"
+            
+            # 获取文本框内容
+            content = self.value_display.get(1.0, tk.END).strip()
+            
+            # 清除文本框的选中状态
+            self.value_display.tag_remove(tk.SEL, "1.0", tk.END)
+            
+            # 复制到剪切板
+            self.root.clipboard_clear()
+            self.root.clipboard_append(content)
+            
+            # 改变按钮文本提示复制成功
+            if isinstance(button, ttk.Button):
+                button.config(text="已复制!")
+                # 1.5秒后恢复原文本
+                self.root.after(1500, lambda: button.config(text=original_text))
+            
+            self.update_status("info", f"关节数值已复制到剪切板")
+            
+        except Exception as e:
+            self.update_status("error", f"复制失败: {str(e)}")
+
+    def on_slider_value_changed(self, index: int, value: str):
+        """滑动条值改变事件处理"""
+        value_int = int(float(value))
+        if 0 <= index < len(self.slider_labels):
+            joint_name = self.hand_config.joint_names[index]
+            self.slider_labels[index].config(text=f"{joint_name}: {value_int}")
+            
+        # 更新数值显示
+        self.update_value_display()
+
+    def update_value_display(self):
+        """更新数值显示面板内容"""
+        values = [int(float(slider.get())) for slider in self.sliders]
+        
+        self.value_display.config(state=tk.NORMAL)
+        self.value_display.delete(1.0, tk.END)
+        self.value_display.insert(tk.END, f"{values}")
+        self.value_display.config(state=tk.DISABLED)
+
+    def on_preset_action_clicked(self, positions: List[int], button_index: int = None):
+        """预设动作按钮点击事件处理"""
+        if len(positions) != len(self.sliders):
+            messagebox.showwarning(
+                "动作不匹配", 
+                f"预设动作关节数量({len(positions)})与当前关节数量({len(self.sliders)})不匹配"
+            )
+            return
+        
+        # 重置所有按钮颜色
+        self.reset_preset_buttons_color()
+        
+        # 高亮当前点击的按钮
+        if button_index is not None and 0 <= button_index < len(self.preset_buttons):
+            self.preset_buttons[button_index].config(bg='#1890FF', fg='white')
+            self.current_clicked_button = button_index
+        
+        # 更新滑动条
+        for i, (slider, pos) in enumerate(zip(self.sliders, positions)):
+            slider.set(pos)
+            self.on_slider_value_changed(i, str(pos))
+            
+        # 发布关节状态
+        self.publish_joint_state()
+
+    def on_home_clicked(self):
+        """回到初始位置按钮点击事件处理"""
+        for slider, pos in zip(self.sliders, self.hand_config.init_pos):
+            slider.set(pos)
+            
+        self.publish_joint_state()
+        self.update_status("info", "回到初始位置")
+        
+        # 更新数值显示
+        self.update_value_display()
+
+    def on_stop_clicked(self):
+        """停止所有动作按钮点击事件处理"""
+        # 停止循环定时器
+        if self.cycle_timer:
+            self.root.after_cancel(self.cycle_timer)
+            self.cycle_timer = None
+            self.cycle_button.config(text="循环运行预设动作")
+            self.reset_preset_buttons_color()
+            
+        self.update_status("warning", "已停止所有动作")
+
+    def on_cycle_clicked(self):
+        """循环运行预设动作按钮点击事件处理"""
+        if not self.hand_config.preset_actions:
+            messagebox.showwarning("无预设动作", "当前手部型号没有预设动作可循环运行")
+            return
+            
+        if self.cycle_timer:
+            # 停止循环
+            self.root.after_cancel(self.cycle_timer)
+            self.cycle_timer = None
+            self.cycle_button.config(text="循环运行预设动作")
+            self.reset_preset_buttons_color()
+            # 如果有之前点击的按钮，恢复其点击状态
+            if hasattr(self, 'current_clicked_button') and self.current_clicked_button is not None:
+                if 0 <= self.current_clicked_button < len(self.preset_buttons):
+                    self.preset_buttons[self.current_clicked_button].config(bg='#1890FF', fg='white')
+            self.update_status("info", "已停止循环运行预设动作")
+        else:
+            # 开始循环
+            self.current_action_index = -1
+            self.cycle_button.config(text="停止循环运行")
+            self.update_status("info", "开始循环运行预设动作")
+            self.run_next_action()
+
+    def run_next_action(self):
+        """运行下一个预设动作"""
+        if not self.hand_config.preset_actions:
+            return
+            
+        # 重置所有按钮颜色
+        self.reset_preset_buttons_color()
+        
+        # 计算下一个动作索引
+        self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions)
+        
+        # 获取下一个动作
+        action_names = list(self.hand_config.preset_actions.keys())
+        action_name = action_names[self.current_action_index]
+        action_positions = self.hand_config.preset_actions[action_name]
+        
+        # 执行动作
+        self.on_preset_action_clicked(action_positions, self.current_action_index)
+        
+        # 高亮当前动作按钮（循环模式使用不同的颜色）
+        if 0 <= self.current_action_index < len(self.preset_buttons):
+            button = self.preset_buttons[self.current_action_index]
+            button.config(bg='#52C41A', fg='white')  # 绿色表示循环中的按钮
+        
+        self.update_status("info", f"运行预设动作: {action_name}")
+        
+        # 设置下一个动作定时器
+        self.cycle_timer = self.root.after(LOOP_TIME, self.run_next_action)
+
+    def reset_preset_buttons_color(self):
+        """重置所有预设按钮颜色"""
+        for button in self.preset_buttons:
+            button.config(bg='#E6F7FF', fg='#1890FF')  # 恢复默认颜色
+
+    def on_speed_changed(self, *args):
+        """速度滑块值改变事件"""
+        self.speed_val_label.config(text=str(self.speed_var.get()))
+
+    def on_torque_changed(self, *args):
+        """扭矩滑块值改变事件"""
+        self.torque_val_label.config(text=str(self.torque_var.get()))
+
+    def on_speed_set(self):
+        """设置速度"""
+        speed_val = self.speed_var.get()
+        self.ros_manager.publish_speed(speed_val)
+        self.update_status("info", f"速度已设为 {speed_val}")
+
+    def on_torque_set(self):
+        """设置扭矩"""
+        torque_val = self.torque_var.get()
+        self.ros_manager.publish_torque(torque_val)
+        self.update_status("info", f"扭矩已设为 {torque_val}")
+
+    def publish_joint_state(self):
+        """发布当前关节状态"""
+        positions = [int(float(slider.get())) for slider in self.sliders]
+        self.ros_manager.publish_joint_state(positions)
+
+    def start_publish_timer(self):
+        """开始发布定时器"""
+        self.publish_joint_state()
+        interval = int(1000 / self.ros_manager.hz)
+        self.publish_timer_id = self.root.after(interval, self.start_publish_timer)
+
+    def update_status(self, status_type: str, message: str):
+        """更新状态显示"""
+        # 更新连接状态
+        if status_type == "info" and "ROS2节点初始化成功" in message:
+            self.connection_status.config(text="ROS2节点已连接", foreground="green")
+            
+        # 更新日志
+        current_time = time.strftime("%H:%M:%S")
+        log_entry = f"[{current_time}] {message}\n"
+        
+        self.status_log.config(state=tk.NORMAL)
+        self.status_log.insert(tk.END, log_entry)
+        self.status_log.see(tk.END)
+        
+        # 限制日志长度
+        log_content = self.status_log.get(1.0, tk.END)
+        if len(log_content) > 10000:
+            self.status_log.delete(1.0, f"{len(log_content)-10000}.0")
+            
+        self.status_log.config(state=tk.DISABLED)
+        
+        # 设置日志颜色
+        if status_type == "error":
+            # 可以在Tkinter中为不同消息类型添加颜色标记
+            pass
+
+    def clear_status_log(self):
+        """清除状态日志"""
+        self.status_log.config(state=tk.NORMAL)
+        self.status_log.delete(1.0, tk.END)
+        self.status_log.insert(tk.END, "日志已清除\n")
+        self.status_log.config(state=tk.DISABLED)
+
+    def run(self):
+        """运行GUI"""
+        try:
+            self.root.mainloop()
+        finally:
+            # 清理定时器
+            if self.cycle_timer:
+                self.root.after_cancel(self.cycle_timer)
+            if self.publish_timer_id:
+                self.root.after_cancel(self.publish_timer_id)
+            # 关闭ROS2节点
+            self.ros_manager.shutdown()
+
+def main(args=None):
+    """主函数"""
+    try:
+        # 创建ROS2节点管理器
+        ros_manager = ROS2NodeManager()
+        
+        # 创建GUI
+        gui = HandControlGUI(ros_manager)
+        
+        # 运行应用
+        gui.run()
+            
+    except Exception as e:
+        print(f"应用程序启动失败: {str(e)}")
+        sys.exit(1)
+
+if __name__ == '__main__':
+    main()

linker_hand_ros2_sdk_ws/src/gui_control/gui_control/utils/color_msg.py

@@ -0,0 +1,27 @@
+#! /usr/bin/env python3
+
+import time
+
+class ColorMsg():
+    def __init__(self,msg: str,color: str = '', timestamp: bool = True) -> None:
+        self.msg = msg
+        self.color = color
+        self.timestamp = timestamp
+        self.colorMsg(msg=self.msg, color=self.color, timestamp=self.timestamp)
+
+    def colorMsg(self,msg: str, color: str = '', timestamp: bool = True):
+        str = ""
+        if timestamp:
+            str += time.strftime('%Y-%m-%d %H:%M:%S',
+                                time.localtime(time.time())) + "  "
+        if color == "red":
+            str += "\033[1;31;40m"
+        elif color == "green":
+            str += "\033[1;32;40m"
+        elif color == "yellow":
+            str += "\033[1;33;40m"
+        else:
+            print(str + msg)
+            return
+        str += msg + "\033[0m"
+        print(str)

linker_hand_ros2_sdk_ws/src/gui_control/gui_control/utils/mapping.py

@@ -0,0 +1,383 @@
+#---------------------------------------------------------------------------------------------------
+# L6 L
+l6_l_min = [0, 0, 0, 0, 0, 0]
+l6_l_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26]
+l6_l_derict = [-1, -1, -1, -1, -1, -1]
+# L6 R
+l6_r_min = [0, 0, 0, 0, 0, 0]
+l6_r_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26]
+l6_r_derict = [-1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# O6 L
+o6_l_min = [0, 0, 0, 0, 0, 0]
+o6_l_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
+o6_l_derict = [-1, -1, -1, -1, -1, -1]
+# O6 R
+o6_r_min = [0, 0, 0, 0, 0, 0]
+o6_r_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
+o6_r_derict = [-1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L7 L OK
+l7_l_min = [0, 0, 0, 0, 0, 0, 0]
+l7_l_max = [0.44, 1.43, 1.62, 1.62, 1.62, 1.62, 1.01]
+l7_l_derict = [-1, -1, -1, -1, -1, -1, -1]
+# L7 R OK (urdf后续会更改！！！)
+l7_r_min = [0, -1.43, 0, 0, 0, 0, 0]
+l7_r_max = [0.75, 0, 1.62, 1.62, 1.62, 1.62, 1.54]
+l7_r_derict = [-1, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L10 L OK
+l10_l_min = [0, 0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.52]
+l10_l_max = [1.45, 1.43, 1.62, 1.62, 1.62, 1.62, 0.26, 0, 0, 1.01]
+l10_l_derict = [-1, -1, -1, -1, -1, -1, 0, -1, -1, -1]
+# L10 R OK
+l10_r_min = [0, 0, 0, 0, 0, 0, -0.26, 0, 0, -0.52]
+l10_r_max = [0.75, 1.43, 1.62, 1.62, 1.62, 1.62, 0.21, 0.21, 0.34, 1.01]
+l10_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, -1]
+#---------------------------------------------------------------------------------------------------
+# L20 L OK
+l20_l_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0.122, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l20_l_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08]
+l20_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+# L20 R OK
+l20_r_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l20_r_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08]
+l20_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L21 L OK
+l21_l_min = [0, 0, 0, 0, 0, 0, 0, -0.18, -0.18, 0, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l21_l_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57]
+l21_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+# L21 R OK
+l21_r_min = [0, 0, 0, 0, 0, 0, -0.18, -0.18, -0.18, -0.18, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l21_r_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57]
+l21_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+#---------------------------------------------------------------------------------------------------
+# L25 L OK
+l25_l_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l25_l_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57]
+l25_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
+# L25 R OK
+l25_r_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l25_r_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57]
+l25_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+
+def range_to_arc_left(left_range,hand_joint):
+    num=0
+    if hand_joint == "L6":
+        num = 6
+        l_min = l6_l_min
+        l_max = l6_l_max
+        l_derict = l6_l_derict
+    elif hand_joint == "O6":
+        num = 6
+        l_min = o6_l_min
+        l_max = o6_l_max
+        l_derict = o6_l_derict
+    elif hand_joint == "L7":
+        num = 7
+        l_min = l7_l_min
+        l_max = l7_l_max
+        l_derict = l7_l_derict
+    elif hand_joint == "L10":
+        num = 10
+        l_min = l10_l_min
+        l_max = l10_l_max
+        l_derict = l10_l_derict
+    elif hand_joint == "L20":
+        num = 20
+        l_min = l20_l_min
+        l_max = l20_l_max
+        l_derict = l20_l_derict
+    elif hand_joint == "L21":
+        num = 25
+        l_min = l21_l_min
+        l_max = l21_l_max
+        l_derict = l21_l_derict
+    hand_arc = [0] * num
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_l = is_within_range(left_range[i], 0, 255)
+        if l_derict[i] == -1:
+            hand_arc[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i])
+        else:
+            hand_arc[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i])
+    return hand_arc
+
+def range_to_arc_right(right_range,hand_joint):
+    num=0
+    if hand_joint == "L6":
+        num = 6
+        r_min = l6_r_min
+        r_max = l6_r_max
+        r_derict = l6_r_derict
+    elif hand_joint == "O6":
+        num = 6
+        r_min = o6_r_min
+        r_max = o6_r_max
+        r_derict = o6_r_derict
+    elif hand_joint == "L7":
+        num = 7
+        r_min = l7_r_min
+        r_max = l7_r_max
+        r_derict = l7_r_derict
+    elif hand_joint == "L10":
+        num = 10
+        r_min = l10_r_min
+        r_max = l10_r_max
+        r_derict = l10_r_derict
+    elif hand_joint == "L20":
+        num = 20
+        r_min = l20_r_min
+        r_max = l20_r_max
+        r_derict = l20_r_derict
+    elif hand_joint == "L21":
+        num = 25
+        r_min = l21_r_min
+        r_max = l21_r_max
+        r_derict = l21_r_derict
+    hand_arc = [0] * num
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_r = is_within_range(right_range[i], 0, 255)
+        if r_derict[i] == -1:
+            hand_arc[i] = scale_value(val_r, 0, 255, r_max[i], r_min[i])
+        else:
+            hand_arc[i] = scale_value(val_r, 0, 255, r_min[i], r_max[i])
+    return hand_arc
+        
+'''
+def arc_to_range_left(left_arc,hand_joint):
+    num=0
+    if hand_joint == "L7":
+        num = 7
+        l_min = l7_l_min
+        l_max = l7_l_max
+        l_derict = l7_l_derict
+    elif hand_joint == "L10":
+        num = 10
+        l_min = l10_l_min
+        l_max = l10_l_max
+        l_derict = l10_l_derict
+    elif hand_joint == "L20":
+        num = 20
+        l_min = l20_l_min
+        l_max = l20_l_max
+        l_derict = l20_l_derict
+    elif hand_joint == "L21":
+        num = 25
+        l_min = l21_l_min
+        l_max = l21_l_max
+        l_derict = l21_l_derict
+    hand_range = [0] * num
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_l = is_within_range(left_arc[i], 0, 255)
+        if l_derict[i] == -1:
+            hand_range[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i])
+        else:
+            hand_range[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i])
+    return hand_range
+    '''
+def arc_to_range_left(hand_arc_l,hand_joint):
+    num=0
+    if hand_joint == "O6":
+        num = 6
+        l_min = o6_l_min
+        l_max = o6_l_max
+        l_derict = o6_l_derict
+    elif hand_joint == "L7":
+        num = 7
+        l_min = l7_l_min
+        l_max = l7_l_max
+        l_derict = l7_l_derict
+    elif hand_joint == "L10":
+        num = 10
+        l_min = l10_l_min
+        l_max = l10_l_max
+        l_derict = l10_l_derict
+    elif hand_joint == "L20":
+        num = 20
+        l_min = l20_l_min
+        l_max = l20_l_max
+        l_derict = l20_l_derict
+    elif hand_joint == "L21":
+        num = 25
+        l_min = l21_l_min
+        l_max = l21_l_max
+        l_derict = l21_l_derict
+    hand_range = [0] * num
+    #hand_range_l = [0] * 7
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_l = is_within_range(hand_arc_l[i], l_min[i], l_max[i])
+        if l_derict[i] == -1:
+            hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 255, 0)
+        else:
+            hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 0, 255)
+
+    return hand_range
+
+def arc_to_range_right(right_arc,hand_joint):
+    num=0
+    if hand_joint == "O6":
+        num = 6
+        r_min = o6_r_min
+        r_max = o6_r_max
+        r_derict = o6_r_derict
+    elif hand_joint == "L7":
+        num = 7
+        r_min = l7_r_min
+        r_max = l7_r_max
+        r_derict = l7_r_derict
+    elif hand_joint == "L10":
+        num = 10
+        r_min = l10_r_min
+        r_max = l10_r_max
+        r_derict = l10_r_derict
+    elif hand_joint == "L20":
+        num = 20
+        r_min = l20_r_min
+        r_max = l20_r_max
+        r_derict = l20_r_derict
+    elif hand_joint == "L21":
+        num = 25
+        r_min = l21_r_min
+        r_max = l21_r_max
+        r_derict = l21_r_derict
+    hand_range = [0] * num
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_r = is_within_range(right_arc[i], r_min[i], r_max[i])
+        if r_derict[i] == -1:
+            hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 255, 0)
+        else:
+            hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 0, 255)
+    return hand_range
+
+
+
+
+def range_to_arc_right_l20(hand_range_r):
+    hand_arc_r = [0] * 20
+    for i in range(20):
+        if 11 <= i <= 14: continue
+        val_r = is_within_range(hand_range_r[i], 0, 255)
+        if l20_r_derict[i] == -1:
+            hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_max[i], l20_r_min[i])
+        else:
+            hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_min[i], l20_r_max[i])
+    return hand_arc_r
+
+
+def range_to_arc_left_l20(hand_range_l):
+    hand_arc_l = [0] * 20
+    for i in range(20):
+        if 11 <= i <= 14: continue
+        val_l = is_within_range(hand_range_l[i], 0, 255)
+        if l20_l_derict[i] == -1:
+            hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_max[i], l20_l_min[i])
+        else:
+            hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_min[i], l20_l_max[i])
+    return hand_arc_l
+
+
+def arc_to_range_right_l20(hand_arc_r):
+    hand_range_r = [0] * 20
+    for i in range(20):
+        if 11 <= i <= 14: continue
+        val_r = is_within_range(hand_arc_r[i], l20_r_min[i], l20_r_max[i])
+        if l20_r_derict[i] == -1:
+            hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 255, 0)
+        else:
+            hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 0, 255)
+    return hand_range_r
+
+
+def arc_to_range_left_l20(hand_arc_l):
+    hand_range_l = [0] * 20
+    for i in range(20):
+        if 11 <= i <= 14: continue
+        val_l = is_within_range(hand_arc_l[i], l20_l_min[i], l20_l_max[i])
+        if l20_l_derict[i] == -1:
+            hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 255, 0)
+        else:
+            hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 0, 255)
+
+    return hand_range_l
+
+
+def range_to_arc_right_10(hand_range_r):
+    hand_arc_r = [0] * 10
+    for i in range(10):
+        val_r = is_within_range(hand_range_r[i], 0, 255)
+        if l10_r_derict[i] == -1:
+            hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_max[i], l10_r_min[i])
+        else:
+            hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_min[i], l10_r_max[i])
+
+    return hand_arc_r
+
+
+def range_to_arc_left_10(hand_range_l):
+    hand_arc_l = [0] * 10
+    for i in range(10):
+        val_l = is_within_range(hand_range_l[i], 0, 255)
+        if l10_l_derict[i] == -1:
+            hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_max[i], l10_l_min[i])
+        else:
+            hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_min[i], l10_l_max[i])
+    return hand_arc_l
+
+
+def arc_to_range_right_10(hand_arc_r):
+    hand_range_r = [0] * 10
+    for i in range(10):
+        val_r = is_within_range(hand_arc_r[i], l10_r_min[i], l10_r_max[i])
+        if l10_r_derict[i] == -1:
+            hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 255, 0)
+        else:
+            hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 0, 255)
+    return hand_range_r
+
+
+def arc_to_range_left_10(hand_arc_l):
+    hand_range_l = [0] * 10
+    for i in range(10):
+        val_l = is_within_range(hand_arc_l[i], l10_l_min[i], l10_l_max[i])
+        if l10_l_derict[i] == -1:
+            hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 255, 0)
+        else:
+            hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 0, 255)
+
+    return hand_range_l
+
+
+def scale_value(original_value, a_min, a_max, b_min, b_max):
+    return (original_value - a_min) * (b_max - b_min) / (a_max - a_min) + b_min
+
+
+def is_within_range(value, min_value, max_value):
+    return min(max_value, max(min_value, value))

linker_hand_ros2_sdk_ws/src/gui_control/launch/gui_control.launch.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # 声明参数：是否显示压力图
+    declare_show_diagram = DeclareLaunchArgument(
+        'show_pressure_diagram',
+        default_value='true',
+        description='是否启动压力图窗口: true | false'
+    )
+
+    # 根据 show_pressure_diagram 参数决定是否包含 pressure_diagram launch 文件
+    pressure_diagram_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare('pressure_diagram'),
+                'launch',
+                'pressure_diagram.launch.py'
+            ])
+        ),
+        condition=IfCondition(LaunchConfiguration('show_pressure_diagram'))
+    )
+
+    return LaunchDescription([
+        declare_show_diagram,
+        pressure_diagram_launch,
+        Node(
+            package='gui_control',
+            executable='gui_control',
+            name='left_hand_control_node',
+            output='screen',
+            parameters=[{
+                'hand_type': 'left',  # 配置Linker Hand灵巧手类型 left | right 字母为小写
+                'hand_joint': "O6",  # O6\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
+                'topic_hz': 30, # topic发布频率
+                'is_touch': True, # 是否有压力传感器
+                'is_arc': False, # 是否发布弧度值topic
+            }],
+        ),
+        # Node(
+        #     package='gui_control',
+        #     executable='gui_control',
+        #     name='right_hand_control_node',
+        #     output='screen',
+        #     parameters=[{
+        #         'hand_type': 'right',
+        #         'hand_joint': "L10",
+        #         'topic_hz': 30,
+        #         'is_touch': True,
+        #     }],
+        # ),
+    ])

linker_hand_ros2_sdk_ws/src/gui_control/package.xml

@@ -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>gui_control</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="linker-robot@todo.todo">linker-robot</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <exec_depend>linker_hand_ros2_sdk</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+    
+  </export>
+</package>

linker_hand_ros2_sdk_ws/src/gui_control/pyproject.toml

@@ -0,0 +1,3 @@
+[build-system]
+requires = ["setuptools>=61.0"]
+build-backend = "setuptools.build_meta"

linker_hand_ros2_sdk_ws/src/gui_control/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/gui_control
+[install]
+install_scripts=$base/lib/gui_control

linker_hand_ros2_sdk_ws/src/gui_control/setup.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+import os
+from glob import glob
+from setuptools import find_packages, setup
+package_name = 'gui_control'
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        (os.path.join('share', 'gui_control', 'launch'), glob('launch/*.launch.py')),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='linker-robot',
+    maintainer_email='linker-robot@todo.todo',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    extras_require={
+        'test': ['pytest'],
+    },
+    entry_points={
+        'console_scripts': [
+            'gui_control = gui_control.gui_control:main'
+        ],
+    },
+)

linker_hand_ros2_sdk_ws/src/gui_control/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

linker_hand_ros2_sdk_ws/src/gui_control/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

linker_hand_ros2_sdk_ws/src/gui_control/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/launch/linker_hand.launch.py

@@ -0,0 +1,21 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='linker_hand_ros2_sdk',
+            executable='linker_hand_sdk',
+            name='linker_hand_sdk',
+            output='screen',
+            parameters=[{
+                'hand_type': 'left', # 配置Linker Hand灵巧手类型 left | right 字母为小写
+                'hand_joint': "O6", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
+                'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False
+                'can': 'can0', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0
+                "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB* 注意添加sudo chmod 777 /dev/ttyUSB*权限
+            }],
+        ),
+    ])

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py

@@ -0,0 +1,35 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='linker_hand_ros2_sdk',
+            executable='linker_hand_sdk',
+            name='linker_hand_sdk_left',
+            output='screen',
+            parameters=[{
+                'hand_type': 'left', # 配置Linker Hand灵巧手类型 left | right 字母为小写
+                'hand_joint': "G20", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
+                'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False
+                'can': 'can0', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1
+                "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB*
+            }],
+        ),
+
+        Node(
+            package='linker_hand_ros2_sdk',
+            executable='linker_hand_sdk',
+            name='linker_hand_sdk_right',
+            output='screen',
+            parameters=[{
+                'hand_type': 'right', # 配置Linker Hand灵巧手类型 left | right 字母为小写
+                'hand_joint': "G20", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
+                'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False
+                'can': 'can1', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1
+                "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB*
+            }],
+        ),
+    ])

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/launch/test.bak

@@ -0,0 +1,31 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='linker_hand_ros2_sdk',
+            executable='linker_hand_sdk',
+            name='linker_hand_sdk_left',
+            output='screen',
+            parameters=[{
+                'hand_type': 'left',
+                'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
+                'is_touch': True, # 是否带有压力传感器
+                'can': 'can0', # 这里需要修改为实际的CAN总线名称
+            }],
+        ),
+
+        Node(
+            package='linker_hand_ros2_sdk',
+            executable='linker_hand_sdk',
+            name='linker_hand_sdk_right',
+            output='screen',
+            parameters=[{
+                'hand_type': 'right',
+                'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
+                'is_touch': True, # 是否带有压力传感器
+                'can': 'can0', # 这里需要修改为实际的CAN总线名称
+            }],
+        ),
+    ])

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L10_positions.yaml

@@ -0,0 +1,146 @@
+LEFT_HAND:
+- ACTION_NAME: 张开
+  POSITION:
+  - 255
+  - 70
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+- ACTION_NAME: 捏合5CM
+  POSITION:
+  - 165
+  - 70
+  - 165
+  - 165
+  - 255
+  - 255
+  - 113
+  - 255
+  - 255
+  - 88
+- ACTION_NAME: 捏合1CM
+  POSITION:
+  - 150
+  - 70
+  - 155
+  - 155
+  - 255
+  - 255
+  - 113
+  - 255
+  - 255
+  - 88
+- ACTION_NAME: 握3CM物品
+  POSITION:
+  - 113
+  - 70
+  - 85
+  - 85
+  - 85
+  - 85
+  - 85
+  - 255
+  - 255
+  - 88
+- ACTION_NAME: 准备抓握
+  POSITION:
+  - 255
+  - 70
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 121
+- ACTION_NAME: 拇指弯曲
+  POSITION:
+  - 35
+  - 140
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 30
+- ACTION_NAME: 食指弯曲
+  POSITION:
+  - 255
+  - 70
+  - 0
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+- ACTION_NAME: shishi
+  POSITION:
+  - 85
+  - 30
+  - 255
+  - 0
+  - 0
+  - 255
+  - 0
+  - 0
+  - 0
+  - 66
+RIGHT_HAND:
+- ACTION_NAME: 张开
+  POSITION:
+  - 255
+  - 70
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+- ACTION_NAME: 捏合5CM
+  POSITION:
+  - 165
+  - 70
+  - 165
+  - 165
+  - 255
+  - 255
+  - 113
+  - 255
+  - 255
+  - 88
+- ACTION_NAME: 捏合1CM
+  POSITION:
+  - 150
+  - 70
+  - 155
+  - 155
+  - 255
+  - 255
+  - 113
+  - 255
+  - 255
+  - 88
+- ACTION_NAME: 准备抓握
+  POSITION:
+  - 255
+  - 70
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 121

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L21_positions.yaml

@@ -0,0 +1,110 @@
+LEFT_HAND:
+- ACTION_NAME: 张开
+  POSITION:
+  - 96
+  - 255
+  - 255
+  - 255
+  - 255
+  - 150
+  - 114
+  - 151
+  - 189
+  - 255
+  - 180
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+- ACTION_NAME: 握拳
+  POSITION:
+  - 177
+  - 0
+  - 0
+  - 0
+  - 0
+  - 51
+  - 114
+  - 151
+  - 189
+  - 255
+  - 79
+  - 255
+  - 255
+  - 255
+  - 255
+  - 131
+  - 222
+  - 244
+  - 255
+  - 255
+  - 0
+  - 0
+  - 0
+  - 0
+  - 0
+RIGHT_HAND:
+- ACTION_NAME: 张开
+  POSITION:
+  - 96
+  - 255
+  - 255
+  - 255
+  - 255
+  - 150
+  - 114
+  - 151
+  - 189
+  - 255
+  - 180
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+- ACTION_NAME: 握拳
+  POSITION:
+  - 230
+  - 80
+  - 51
+  - 42
+  - 7
+  - 35
+  - 114
+  - 151
+  - 189
+  - 255
+  - 58
+  - 255
+  - 255
+  - 255
+  - 255
+  - 133
+  - 5
+  - 0
+  - 0
+  - 0
+  - 30
+  - 0
+  - 0
+  - 0
+  - 0

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L25_positions.yaml

@@ -0,0 +1,83 @@
+LEFT_HAND:
+- ACTION_NAME: 张开
+  POSITION:
+  - 96
+  - 255
+  - 255
+  - 255
+  - 255
+  - 150
+  - 114
+  - 151
+  - 189
+  - 255
+  - 180
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+RIGHT_HAND:
+- ACTION_NAME: 张开
+  POSITION:
+  - 96
+  - 255
+  - 255
+  - 255
+  - 255
+  - 150
+  - 114
+  - 151
+  - 189
+  - 255
+  - 180
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255
+- ACTION_NAME: 握拳
+  POSITION:
+  - 230
+  - 80
+  - 51
+  - 42
+  - 7
+  - 35
+  - 114
+  - 151
+  - 189
+  - 255
+  - 58
+  - 255
+  - 255
+  - 255
+  - 255
+  - 133
+  - 5
+  - 0
+  - 0
+  - 0
+  - 30
+  - 0
+  - 0
+  - 0
+  - 0

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L6_positions.yaml

@@ -0,0 +1,26 @@
+LEFT_HAND:
+- ACTION_NAME: 握拳
+  POSITION:
+  - 67
+  - 151
+  - 0
+  - 0
+  - 0
+  - 0
+- ACTION_NAME: 张开
+  POSITION:
+  - 255
+  - 179
+  - 255
+  - 255
+  - 255
+  - 255
+RIGHT_HAND:
+- ACTION_NAME: 张开
+  POSITION:
+  - 255
+  - 70
+  - 255
+  - 255
+  - 255
+  - 255

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L7_positions.yaml

@@ -0,0 +1,29 @@
+LEFT_HAND:
+- ACTION_NAME: 握拳
+  POSITION:
+  - 67
+  - 151
+  - 0
+  - 0
+  - 0
+  - 0
+  - 37
+- ACTION_NAME: 张开
+  POSITION:
+  - 255
+  - 179
+  - 255
+  - 255
+  - 255
+  - 255
+  - 83
+RIGHT_HAND:
+- ACTION_NAME: 张开
+  POSITION:
+  - 255
+  - 70
+  - 255
+  - 255
+  - 255
+  - 255
+  - 255

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/O6_positions.yaml

@@ -0,0 +1,26 @@
+LEFT_HAND:
+- ACTION_NAME: 握拳
+  POSITION:
+  - 67
+  - 151
+  - 0
+  - 0
+  - 0
+  - 0
+- ACTION_NAME: 张开
+  POSITION:
+  - 255
+  - 179
+  - 255
+  - 255
+  - 255
+  - 255
+RIGHT_HAND:
+- ACTION_NAME: 张开
+  POSITION:
+  - 255
+  - 70
+  - 255
+  - 255
+  - 255
+  - 255

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml

@@ -0,0 +1,58 @@
+VERSION: 3.1.1 # 支持O6、L6在RS485模式
+LINKER_HAND:  # 手部配置信息
+  LEFT_HAND:
+    EXISTS: True # 是否存在左手
+    TOUCH: True # 是否有压力传感器
+    CAN: "can0"  # 配置CAN端口 默认can0 如果MODUBS不为"None",则CAN配置失效。 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0
+    MODBUS: "None" # 通讯协议是否为485 默认None 如果启动485，则是设备端口 /dev/ttyUSB* CAN配置失效 当前只支持O6/L6.后续版本正在努力增加中
+    JOINT: O6 # 左手型号 O6/L6/L7/L10/L20/G20/L21/L25/
+    NAME: # 默认值，不用修改
+      - joint41
+      - joint42
+      - joint43
+      - joint44
+      - joint45
+      - joint46
+      - joint47
+      - joint48
+      - joint49
+      - joint50
+      - joint51
+      - joint52
+      - joint53
+      - joint54
+      - joint55
+      - joint56
+      - joint57
+      - joint58
+      - joint59
+      - joint60
+    
+  RIGHT_HAND:
+    EXISTS: False # 是否存在右手
+    TOUCH: False # 是否有压力传感器
+    CAN: "can0"  # 配置CAN端口 默认can0 如果MODUBS不为"None",则CAN配置失效。 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0
+    MODBUS: "None" # 通讯协议是否为485 默认None 如果启动485，则是设备端口 /dev/ttyUSB* CAN配置失效 当前只支持O6/L6.后续版本正在努力增加中
+    JOINT: O6 # 右手型号 O6/L6/L7/L10/L20/G20/L21/L25/
+    NAME:  # 默认值，不用修改
+      - joint71
+      - joint72
+      - joint73
+      - joint77
+      - joint75
+      - joint76
+      - joint77
+      - joint78
+      - joint79
+      - joint80
+      - joint81
+      - joint82
+      - joint83
+      - joint84
+      - joint88
+      - joint86
+      - joint87
+      - joint88
+      - joint89
+      - joint90
+PASSWORD: "12345678" # 由于与can通讯，需要激活通讯接口用到系统管理员密码。只有Linux系统需要，windows系统不需要。RS485不需要修改，RS485需要给/dev/ttyUSB* 777权限

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l10_can.py

@@ -0,0 +1,531 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+import can
+import time,sys
+import threading
+import numpy as np
+#from tabulate import tabulate
+from enum import Enum
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+
+
+class FrameProperty(Enum):
+    INVALID_FRAME_PROPERTY = 0x00
+    JOINT_POSITION_RCO = 0x01
+    MAX_PRESS_RCO = 0x02
+    MAX_PRESS_RCO2 = 0x03
+    JOINT_POSITION2_RCO = 0x04
+    JOINT_SPEED = 0x05
+    JOINT_SPEED2 = 0x06
+    REQUEST_DATA_RETURN = 0x09
+    JOINT_POSITION_N = 0x11
+    MAX_PRESS_N = 0x12
+    HAND_NORMAL_FORCE = 0X20
+    HAND_TANGENTIAL_FORCE = 0X21
+    HAND_TANGENTIAL_FORCE_DIR = 0X22
+    HAND_APPROACH_INC = 0X23
+    MOTOR_TEMPERATURE_1 = 0x33
+    MOTOR_TEMPERATURE_2 = 0x34
+
+class LinkerHandL10Can:
+    def __init__(self,can_id, can_channel='can0', baudrate=1000000, yaml=""):
+        self.can_id = can_id
+        self.can_channel = can_channel
+        self.baudrate = baudrate
+        self.open_can = OpenCan(load_yaml=yaml)
+        self.is_cmd = False
+        self.x01 = [-1] * 5
+        self.x02 = [-1] * 5
+        self.x03 = [-1] * 5
+        self.x04 = [-1] * 5
+        self.x05 = [-1] * 5
+        self.x06 = [-1] * 5
+        self.x33 = self.x34 = [0] * 5
+        # Fault codes
+        self.x35,self.x36 = [0] * 5,[0] * 5
+        # New pressure sensors
+        self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+        
+        self.thumb_matrix = np.full((12, 6), -1)
+        self.index_matrix = np.full((12, 6), -1)
+        self.middle_matrix = np.full((12, 6), -1)
+        self.ring_matrix = np.full((12, 6), -1)
+        self.little_matrix = np.full((12, 6), -1)
+        self.matrix_map = {
+            0: 0,
+            16: 1,
+            32: 2,
+            48: 3,
+            64: 4,
+            80: 5,
+            96: 6,
+            112: 7,
+            128: 8,
+            144: 9,
+            160: 10,
+            176: 11,
+        }
+        self.serial_number = []
+        self.serial_number_map = {
+            0: 0,
+            1: 1,
+            2: 2,
+            3: 3,
+        }
+        self.can_id = can_id
+        self.joint_angles = [0] * 10
+        self.pressures = [200] * 5  # Default torque 200
+        self.bus = self.init_can_bus(can_channel, baudrate)
+        self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 5 for _ in range(4)]
+        self.version = None
+        # Start receiving thread
+        self.running = True
+        self.receive_thread = threading.Thread(target=self.receive_response)
+        self.receive_thread.daemon = True
+        self.receive_thread.start()
+        self.version = self.get_version()
+
+    def init_can_bus(self, channel, baudrate):
+        """
+        尝试按优先级连接 CAN 总线，并实现回退机制。
+        """
+        # --- 统一异常处理块开始 ---
+        try:
+            if sys.platform == "linux":
+                # Linux 优先级：1. socketcan
+                try:
+                    self.open_can.open_can(self.can_channel)
+                    # 尝试 socketcan
+                    bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+                    ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+                    raise # 重新抛出异常，让外层 try 捕获
+            elif sys.platform == "win32":
+                # Windows 优先级：1. pcan
+                try:
+                    bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+                # Windows 优先级：2. candle (回退方法)
+                try:
+                    bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+                    raise # 两个接口都失败，抛出异常
+            else:
+                raise EnvironmentError("Unsupported platform for CAN interface")
+        # --- 统一异常处理块结束 ---
+        except Exception as e:
+            # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+            ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+            # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+            raise
+
+    def send_frame(self, frame_property, data_list,sleep=0.002):
+        """Send a single CAN frame with specified properties and data."""
+        frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+        data = [frame_property_value] + [int(val) for val in data_list]
+        msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+        try:
+            self.bus.send(msg)
+        except can.CanError as e:
+            print(f"Failed to send message: {e}")
+            self.open_can.open_can(self.can_channel)
+            time.sleep(1)
+            self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+            time.sleep(1)
+            if self.is_can:
+                self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+            else:
+                print("Reconnecting CAN devices ....")
+            # time.sleep(1)
+            # 
+        time.sleep(sleep)
+
+    def set_joint_positions(self, joint_angles):
+        """Set the positions of 10 joints (joint_angles: list of 10 values)."""
+        self.joint_angles = joint_angles
+        self.is_cmd = True
+        # Send angle control in frames, L10 protocol splits into first 6 and last 4
+        self.send_frame(FrameProperty.JOINT_POSITION2_RCO, self.joint_angles[6:])
+        #time.sleep(0.001)
+        self.send_frame(FrameProperty.JOINT_POSITION_RCO, self.joint_angles[:6])
+        #time.sleep(0.002)
+        self.is_cmd = False
+        
+
+    def set_max_torque_limits(self, pressures,type="get"):
+        """Set maximum torque limits"""
+        if type == "get":
+            self.pressures = [0.0]
+        else:
+            self.pressures = pressures[:5]
+        #self.send_frame(FrameProperty.MAX_PRESS_RCO, self.pressures)
+        
+        
+    def set_joint_speed_l10(self,speed=[180]*5):
+        self.x05 = speed
+        for i in range(2):
+            time.sleep(0.01)
+            self.send_frame(0x05, speed)
+    def set_speed(self,speed=[180]*5):
+        if len(speed) == 5:
+            self.x05 = speed
+            for i in range(2):
+                time.sleep(0.01)
+                self.send_frame(0x05, speed)
+        elif len(speed) == 10:
+            for i in range(2):
+                time.sleep(0.01)
+                self.send_frame(0x05, speed[:5])
+                self.send_frame(0x06, speed[5:])
+        else:
+            raise ValueError("Speed list must have 10 elements.")
+    def request_all_status(self):
+        """Get all joint positions and pressures."""
+        self.send_frame(FrameProperty.REQUEST_DATA_RETURN, [])
+    ''' -------------------Pressure Sensors---------------------- '''
+    def get_normal_force(self):
+        self.send_frame(FrameProperty.HAND_NORMAL_FORCE,[],sleep=0.004)
+
+    def get_tangential_force(self):
+        self.send_frame(FrameProperty.HAND_TANGENTIAL_FORCE,[],sleep=0.004)
+
+    def get_tangential_force_dir(self):
+        self.send_frame(FrameProperty.HAND_TANGENTIAL_FORCE_DIR,[],sleep=0.004)
+    def get_approach_inc(self):
+        self.send_frame(FrameProperty.HAND_APPROACH_INC,[],sleep=0.004)
+    ''' -------------------Motor Temperature---------------------- '''
+    def get_motor_temperature(self):
+        self.send_frame(FrameProperty.MOTOR_TEMPERATURE_1,[],sleep=0.01)
+        self.send_frame(FrameProperty.MOTOR_TEMPERATURE_2,[],sleep=0.01)
+    # Motor fault codes
+    def get_motor_fault_code(self):
+        self.send_frame(0x35,[],sleep=0.1)
+        self.send_frame(0x36,[],sleep=0.1)
+    def receive_response(self):
+        """Receive CAN responses and process them."""
+        while self.running:
+            try:
+                msg = self.bus.recv(timeout=1.0)
+                if msg:
+                    self.process_response(msg)
+            except can.CanError as e:
+                print(f"Error receiving CAN message: {e}")
+
+    def process_response(self, msg):
+        """Process received CAN messages."""
+        if msg.arbitration_id == self.can_id:
+            frame_type = msg.data[0]
+            response_data = msg.data[1:]
+            if len(list(response_data)) == 0:
+                    return
+            if frame_type == FrameProperty.JOINT_POSITION_RCO.value:   # 0x01
+                self.x01 = list(response_data)  
+            elif frame_type == FrameProperty.MAX_PRESS_RCO.value:    # 0x02
+                self.x02 = list(response_data)
+            elif frame_type == FrameProperty.MAX_PRESS_RCO2.value:    # 0x03
+                self.x03 = list(response_data)
+            elif frame_type == FrameProperty.JOINT_POSITION2_RCO.value:    # 0x04
+                self.x04 = list(response_data)
+            elif frame_type == 0x05:
+                self.x05 = list(response_data)
+            elif frame_type == 0x06:
+                self.x06 = list(response_data)
+            elif frame_type == 0x20:
+                # Five-finger normal force
+                d = list(response_data)
+                self.normal_force = [float(i) for i in d]
+            elif frame_type == 0x21:
+                # Five-finger tangential force
+                d = list(response_data)
+                self.tangential_force = [float(i) for i in d]
+            elif frame_type == 0x22:
+                # Five-finger tangential force direction
+                d = list(response_data)
+                self.tangential_force_dir = [float(i) for i in d]
+            elif frame_type == 0x23:
+                # Five-finger approach increment
+                d = list(response_data)
+                self.approach_inc = [float(i) for i in d]
+            elif frame_type == 0x33:
+                self.x33 = list(response_data)
+            elif frame_type == 0x34:
+                self.x34 = list(response_data)
+            elif frame_type == 0x35:
+                self.x35 = list(response_data)
+            elif frame_type == 0x36:
+                self.x36 = list(response_data)
+            elif frame_type == 0xb0:
+                self.xb0 = list(response_data)
+            elif frame_type == 0xb1:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb1 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.thumb_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb2:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb2 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.index_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb3:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb3 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.middle_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb4:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb4 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.ring_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb5:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb5 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.little_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0x64:
+                self.version =  list(response_data)
+            elif frame_type == 0xC2: # version number
+                self.version = list(response_data)
+            elif frame_type == 0xC0:
+                d = list(response_data)
+                index = self.serial_number_map.get(d[0])
+                if index is not None:
+                    self.serial_number=self.serial_number + d[1:]
+                else:
+                    self.serial_number=self.serial_number + [-1] * 6
+
+    def get_version(self):
+        self.send_frame(0x64, [], sleep=0.1)
+        time.sleep(0.1)
+        if self.version is None:
+            self.send_frame(0xC2, [], sleep=0.1)
+            time.sleep(0.1)
+        return self.version
+
+    def set_torque(self,torque=[]):
+        '''Set maximum torque'''
+        if len(torque) == 5:
+            self.send_frame(0x02, torque)
+            time.sleep(0.002)
+            self.send_frame(0x03,torque)
+        elif len(torque) > 5:
+            self.send_frame(0x02, torque[:5])
+            time.sleep(0.002)
+            self.send_frame(0x03,torque[5:])
+
+    
+    def get_current_status(self):
+        '''Get current joint status'''
+        if self.is_cmd == False:
+            #if self.version != None and self.version[4] > 35:
+            self.send_frame(0x01,[],sleep=0.003)
+            self.send_frame(0x04,[],sleep=0.003)
+            state = self.x01 + self.x04
+            return state
+        else:
+            state = self.x01 + self.x04
+            return state
+        
+    def get_current_pub_status(self):
+        state = self.x01 + self.x04
+        return state
+        
+    def get_speed(self):
+        '''Get current speed'''
+        self.send_frame(0x05,[],sleep=0.003)
+        self.send_frame(0x06,[],sleep=0.003)
+        return self.x05 + self.x06
+        
+    def get_force(self):
+        '''Get pressure sensor data'''
+        return [self.normal_force,self.tangential_force , self.tangential_force_dir , self.approach_inc]
+    def get_temperature(self):
+        '''Get current motor temperature'''
+        self.get_motor_temperature()
+        return self.x33+self.x34
+
+    def get_touch_type(self):
+        '''Get touch type'''
+        self.send_frame(0xb0,[],sleep=0.03)
+        self.send_frame(0xb1,[],sleep=0.03)
+        t = []
+        for i in range(3):
+            t = self.xb1
+            time.sleep(0.01)
+        if len(t) == 2:
+            return 2
+        else:
+            self.send_frame(0x20,[],sleep=0.03)
+            time.sleep(0.01)
+            if self.normal_force[0] == -1:
+                return -1
+            else:
+                return 1
+    
+    def get_touch(self):
+        '''Get touch data'''
+        self.send_frame(0xb1,[],sleep=0.03)
+        self.send_frame(0xb2,[],sleep=0.03)
+        self.send_frame(0xb3,[],sleep=0.03)
+        self.send_frame(0xb4,[],sleep=0.03)
+        self.send_frame(0xb5,[],sleep=0.03)
+        return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+    def get_matrix_touch(self):
+        self.send_frame(0xb1,[0xc6],sleep=0.06)
+        self.send_frame(0xb2,[0xc6],sleep=0.06)
+        self.send_frame(0xb3,[0xc6],sleep=0.06)
+        self.send_frame(0xb4,[0xc6],sleep=0.06)
+        self.send_frame(0xb5,[0xc6],sleep=0.06)
+        return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+    
+    def get_matrix_touch_v2(self):
+        self.send_frame(0xb1,[0xc6],sleep=0.005)
+        self.send_frame(0xb2,[0xc6],sleep=0.005)
+        self.send_frame(0xb3,[0xc6],sleep=0.005)
+        self.send_frame(0xb4,[0xc6],sleep=0.005)
+        self.send_frame(0xb5,[0xc6],sleep=0.005)
+        return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+    def get_thumb_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb1,[0xc6],sleep=sleep_time)
+        return self.thumb_matrix
+    
+    def get_index_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb2,[0xc6],sleep=sleep_time)
+        return self.index_matrix
+    
+    def get_middle_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb3,[0xc6],sleep=sleep_time)
+        return self.middle_matrix
+    
+    def get_ring_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb4,[0xc6],sleep=sleep_time)
+        return self.ring_matrix
+    
+    def get_little_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb5,[0xc6],sleep=sleep_time)
+        return self.little_matrix
+
+
+    def get_torque(self):
+        '''Get current motor torque'''
+        if self.version != None and self.version[4]< 36:
+            return [-1] * 5
+        else:
+            self.send_frame(0x02, [])
+            time.sleep(0.002)
+            self.send_frame(0x03,[])
+            time.sleep(0.002)
+            return self.x02+self.x03
+    
+    def get_fault(self):
+        '''Get motor fault'''
+        self.get_motor_fault_code()
+        return self.x35+self.x36
+    
+    def get_current(self):
+        '''Get current'''
+        #return [-1] * 5
+        self.send_frame(0x02, [])
+        time.sleep(0.002)
+        self.send_frame(0x03,[])
+        return self.x02+self.x03
+    
+    def get_serial_number(self):
+        try:
+            self.send_frame(0xC0,[],sleep=0.005)
+            # 1. 使用 bytes() 函数将整数列表转换为字节对象
+            #    bytes() 接收一个由 0-255 之间的整数组成的列表。
+            byte_data = bytes(self.serial_number)
+            # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+            result_string = byte_data.decode('ascii')
+            if result_string == "":
+                return "-1"
+            else:
+                # print(f"原始 ASCII 码列表: {self.serial_number}")
+                # print(f"解码后的字符串: {result_string}")
+                return result_string
+        except:
+            return "-1"
+
+    def get_finger_order(self):
+        return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
+                        "index_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", "thumb_cmc_roll"]
+
+    def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+        """L10 暂不支持清除故障码"""
+        pass
+
+    def show_fun_table(self):
+        # if len(data) != 8 or data[0] != 0x64:
+        #     raise ValueError("数据格式不正确")
+        data = self.version
+        result = {
+            "自由度": data[0],
+            "机械版本": data[1],
+            "版本序号": data[2],
+            "手方向": chr(data[3]),  # ASCII 转字符
+            "软件版本": f"V{data[4] >> 4}.{data[4] & 0x0F}",
+            "硬件版本": f"V{data[5] >> 4}.{data[5] & 0x0F}",
+            "修订标志": data[6],
+            "set_position": "Y",
+            "set_torque": "Y",
+            "set_speed": "Y",
+            "get_version": "Y",
+            "get_current_status": "Y",
+            "get_speed": "Y",
+            "get_temperature": "Y",
+            "get_touch_type": "Y",
+            "get_matrix_touch": "Y",
+            "get_fault": "Y",
+            "get_current": "current == torque"
+        }
+        
+        #return [data[0],data[1],data[2],chr(data[3]),f"V{data[4] >> 4}.{data[4] & 0x0F}",f"V{data[5] >> 4}.{data[5] & 0x0F}",data[6]]
+        table = [[k, v] for k, v in result.items()]
+        #print(tabulate(table, tablefmt="grid"), flush=True)
+
+
+    # # 示例数据
+    # data = [0x64, 0x15, 0x03, 0x0A, 0x4C, 0x11, 0x22, 0x01]
+    # parsed = parse_version_data(data)
+
+    # # 打印结果
+    # for k, v in parsed.items():
+    #     print(f"{k}: {v}")
+
+
+    def close_can_interface(self):
+        """Stop the CAN communication."""
+        self.running = False
+        if self.receive_thread.is_alive():
+            self.receive_thread.join()
+        if self.bus:
+            self.bus.shutdown()

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py

@@ -0,0 +1,478 @@
+import sys
+import time
+import can
+import threading
+from enum import Enum
+import numpy as np
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+class FrameProperty(Enum):
+    INVALID_FRAME_PROPERTY = 0x00  # Invalid CAN frame property | No return
+    JOINT_PITCH_R = 0x01           # Short frame pitch angle - finger base flexion | Returns this type of data
+    JOINT_YAW_R = 0x02             # Short frame yaw angle - finger abduction/adduction | Returns this type of data
+    JOINT_ROLL_R = 0x03            # Short frame roll angle - only used for thumb | Returns this type of data
+    JOINT_TIP_R = 0x04             # Short frame fingertip angle control | Returns this type of data
+    JOINT_SPEED_R = 0x05           # Short frame speed - motor running speed control | Returns this type of data
+    JOINT_CURRENT_R = 0x06         # Short frame current - motor running current feedback | Returns this type of data
+    JOINT_FAULT_R = 0x07           # Short frame fault - motor running fault feedback | Returns this type of data
+    REQUEST_DATA_RETURN = 0x09     # Request data return | Returns all data
+    JOINT_PITCH_NR = 0x11          # Pitch angle - finger base flexion | No return for this type of data
+    JOINT_YAW_NR = 0x12            # Yaw angle - finger abduction/adduction | No return for this type of data
+    JOINT_ROLL_NR = 0x13           # Roll angle - only used for thumb | No return for this type of data
+    JOINT_TIP_NR = 0x14            # Fingertip angle control | No return for this type of data
+    JOINT_SPEED_NR = 0x15          # Speed - motor running speed control | No return for this type of data
+    JOINT_CURRENT_NR = 0x16        # Current - motor running current feedback | No return for this type of data
+    JOINT_FAULT_NR = 0x17          # Fault - motor running fault feedback | No return for this type of data
+    HAND_UID = 0xC0                # Device unique identifier Read only --------
+    HAND_HARDWARE_VERSION = 0xC1   # Hardware version Read only --------
+    HAND_SOFTWARE_VERSION = 0xC2   # Software version Read only --------
+    HAND_COMM_ID = 0xC3            # Device ID Read/Write 1 byte
+    HAND_SAVE_PARAMETER = 0xCF     # Save parameters Write only --------
+
+
+class LinkerHandL20Can:
+    def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28,yaml=""):
+        self.can_id = can_id
+        self.can_channel = can_channel
+        self.baudrate = baudrate
+        self.open_can = OpenCan(load_yaml=yaml)
+
+        self.running = True
+        self.x05 = [255] * 5
+        self.x06, self.x07 = [],[]
+        # New pressure sensors
+        self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+        self.x09 = self.x0b = self.x0c = self.x0d = [-1] * 5
+        self.thumb_matrix = np.full((12, 6), -1)
+        self.index_matrix = np.full((12, 6), -1)
+        self.middle_matrix = np.full((12, 6), -1)
+        self.ring_matrix = np.full((12, 6), -1)
+        self.little_matrix = np.full((12, 6), -1)
+        self.matrix_map = {
+            0: 0,
+            16: 1,
+            32: 2,
+            48: 3,
+            64: 4,
+            80: 5,
+            96: 6,
+            112: 7,
+            128: 8,
+            144: 9,
+            160: 10,
+            176: 11,
+        }
+        
+        # Initialize CAN bus according to operating system
+        # try:
+        #     if sys.platform == "linux":
+        #         self.open_can.open_can(self.can_channel)
+        #         time.sleep(0.1)
+        #         self.bus = can.interface.Bus(
+        #             channel=can_channel, interface="socketcan", bitrate=baudrate, 
+        #             can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+        #         )
+        #     elif sys.platform == "win32":
+        #         self.bus = can.interface.Bus(
+        #             channel=can_channel, interface='pcan', bitrate=baudrate, 
+        #             can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+        #         )
+        #     else:
+        #         raise EnvironmentError("Unsupported platform for CAN interface")
+        # except:
+        #     print("Please insert CAN device",flush=True)
+        self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate)
+        # Initialize data storage
+        self.x01, self.x02, self.x03, self.x04 = [[-1] * 5 for _ in range(4)]
+        self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = \
+            [[-1] * 5 for _ in range(4)]
+
+        # Start receive thread
+        self.get_touch_type()
+        time.sleep(0.1)
+        self.receive_thread = threading.Thread(target=self.receive_response)
+        self.receive_thread.daemon = True
+        self.receive_thread.start()
+
+    def init_can_bus(self, channel, baudrate):
+        """
+        尝试按优先级连接 CAN 总线，并实现回退机制。
+        """
+        # --- 统一异常处理块开始 ---
+        try:
+            if sys.platform == "linux":
+                # Linux 优先级：1. socketcan
+                try:
+                    self.open_can.open_can(self.can_channel)
+                    # 尝试 socketcan
+                    bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+                    ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+                    raise # 重新抛出异常，让外层 try 捕获
+            elif sys.platform == "win32":
+                # Windows 优先级：1. pcan
+                try:
+                    bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+                # Windows 优先级：2. candle (回退方法)
+                try:
+                    bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+                    raise # 两个接口都失败，抛出异常
+            else:
+                raise EnvironmentError("Unsupported platform for CAN interface")
+        # --- 统一异常处理块结束 ---
+        except Exception as e:
+            # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+            ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+            # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+            raise
+    # def send_command(self, frame_property, data_list):
+    #     print("66666")
+    #     """
+    #     Send command to CAN bus
+    #     :param frame_property: Data frame property
+    #     :param data_list: Data payload
+    #     """
+    #     frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+    #     data = [frame_property_value] + [int(val) for val in data_list]
+    #     msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+    #     try:
+    #         self.bus.send(msg)
+    #         print(f"Message sent: ID={hex(self.can_id)}, Data={data}")
+    #     except can.CanError as e:
+    #         print(f"Failed to send message: {e}")
+
+    def receive_response(self):
+        """
+        Receive and process CAN bus response messages
+        """
+        while self.running:
+            try:
+                msg = self.bus.recv(timeout=1.0)  # Blocking receive, 1 second timeout
+                if msg:
+                    self.process_response(msg)
+            except can.CanError as e:
+                print(f"Error receiving message666: {e}",flush=True)
+                
+
+    def set_finger_base(self, angles):
+        self.send_command(FrameProperty.JOINT_PITCH_NR, angles)
+
+    def set_finger_tip(self, angles):
+        self.send_command(FrameProperty.JOINT_TIP_NR, angles)
+
+    def set_finger_middle(self, angles):
+        self.send_command(FrameProperty.JOINT_YAW_NR, angles)
+
+    def set_thumb_roll(self, angle):
+        self.send_command(FrameProperty.JOINT_ROLL_NR, angle)
+
+    def send_command(self, frame_property, data_list,sleep=0.002):
+        frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+        data = [frame_property_value] + [int(val) for val in data_list]
+        
+        msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+        try:
+            self.bus.send(msg)
+        except can.CanError:
+            print("Message NOT sent")
+            self.open_can.open_can(self.can_channel)
+            time.sleep(1)
+            self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+            time.sleep(1)
+            if self.is_can:
+                self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+            else:
+                print("Reconnecting CAN devices ....",flush=True)
+        time.sleep(sleep)
+
+    def set_joint_pitch(self, frame, angles):
+        self.send_command(frame, angles)
+
+    def set_joint_yaw(self, angles):
+        self.send_command(0x02, angles)
+
+    def set_joint_roll(self, thumb_roll):
+        self.send_command(0x03, [thumb_roll, 0, 0, 0, 0])
+
+    def set_joint_speed(self, speed):
+        self.x05 = speed
+        self.send_command(0x05, speed)
+    def set_electric_current(self, e_c=[]):
+        self.send_command(0x06, e_c)
+
+    def get_normal_force(self):
+        self.send_command(0x20,[])
+
+    def get_tangential_force(self):
+        self.send_command(0x21,[])
+
+
+    def get_tangential_force_dir(self):
+        self.send_command(0x22,[])
+
+    def get_approach_inc(self):
+        self.send_command(0x23,[])
+
+
+
+
+    def get_electric_current(self, e_c=[]):
+        self.send_command(0x06, e_c)
+    
+    def request_device_info(self):
+        self.send_command(0xC0, [0])
+        self.send_command(0xC1, [0])
+        self.send_command(0xC2, [0])
+
+    def save_parameters(self):
+        self.send_command(0xCF, [])
+    def process_response(self, msg):
+        if msg.arbitration_id == self.can_id:
+            frame_type = msg.data[0]
+            response_data = msg.data[1:]
+            if len(list(response_data)) == 0:
+                return
+            if frame_type == 0x01:
+                self.x01 = list(response_data)
+            elif frame_type == 0x02:
+                self.x02 = list(response_data)
+            elif frame_type == 0x03:
+                self.x03 = list(response_data)
+            elif frame_type == 0x04:
+                self.x04 = list(response_data)
+            elif frame_type == 0xC0:
+                print(f"Device ID info: {response_data}")
+                if self.can_id == 0x28:
+                    self.right_hand_info = response_data
+                elif self.can_id == 0x27:
+                    self.left_hand_info = response_data
+            elif frame_type == 0x05:
+                self.x05 = list(response_data)
+            elif frame_type == 0x06:
+                self.x06 = list(response_data)
+            elif frame_type == 0x07:
+                self.x07 = list(response_data)
+            elif frame_type == 0x09:
+                self.x09 = list(response_data)
+            elif frame_type == 0x0B:
+                self.x0b = list(response_data)
+            elif frame_type == 0x0C:
+                self.x0c = list(response_data)
+            elif frame_type == 0x0D:
+                self.x0d = list(response_data)
+            elif frame_type == 0x20:
+                d = list(response_data)
+                self.normal_force = [float(i) for i in d] 
+            elif frame_type == 0x21:
+                d = list(response_data)
+                self.tangential_force = [float(i) for i in d]
+            elif frame_type == 0x22:
+                d = list(response_data)
+                self.tangential_force_dir = [float(i) for i in d]
+            elif frame_type == 0x23:
+                d = list(response_data)
+                self.approach_inc = [float(i) for i in d]
+            elif frame_type == 0xb0:
+                self.xb0 = list(response_data)
+            elif frame_type == 0xb1:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb1 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.thumb_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb2:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb2 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.index_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb3:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb3 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.middle_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb4:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb4 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.ring_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb5:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb5 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.little_matrix[index] = d[1:]  # Remove the first flag bit
+    def pose_slice(self, p):
+        """Slice the joint array into finger action arrays"""
+        try:
+            finger_base = [int(val) for val in p[0:5]]   # Finger base
+            yaw_angles = [int(val) for val in p[5:10]]    # Yaw
+            thumb_yaw = [int(val) for val in p[10:15]]     # Thumb yaw to palm, others are 0
+            finger_tip = [int(val) for val in p[15:20]]    # Fingertip flexion
+            return finger_base, yaw_angles, thumb_yaw, finger_tip
+        except Exception as e:
+            print(e)
+    def set_joint_positions(self, position):
+        if len(position) != 20:
+            print("L20 finger joint length is incorrect")
+            return
+        finger_base, yaw_angles, thumb_yaw, finger_tip = self.pose_slice(position)
+        self.set_thumb_roll(thumb_yaw) # Thumb yaw to palm movement
+        self.set_finger_tip(finger_tip) # Fingertip movement
+        self.set_finger_base(finger_base) # Finger base movement
+        self.set_finger_middle(yaw_angles) # Yaw movement
+    def set_speed(self, speed=[]):
+        if len(speed) != 5:
+            raise ValueError("Speed list must have 5 elements.")
+            return
+        self.send_command(0x05,speed)
+    def set_torque(self, torque=[]):
+        '''Set torque, not supported for L20'''
+        print("Set torque, not supported for L20")
+    def set_current(self, current=[]):
+        '''Set current'''
+        self.set_electric_current(e_c=current)
+    def get_version(self):
+        '''Get version, currently not supported'''
+        return [0] * 5
+    def get_current_status(self):
+        '''Get current finger joint status'''
+        self.send_command(0x01,[],sleep=0.01)
+        self.send_command(0x02,[],sleep=0.01)
+        self.send_command(0x03,[],sleep=0.01)
+        self.send_command(0x04,[],sleep=0.01)
+        return self.x01 + self.x02 + self.x03 + self.x04
+    
+    def get_current_pub_status(self):
+        time.sleep(0.01)
+        return self.x01 + self.x02 + self.x03 + self.x04
+
+    def get_speed(self):
+        '''Get current motor speed'''
+        self.send_command(0x05, [0])
+        time.sleep(0.001)
+        return self.x05
+    def get_current(self):
+        '''Get current threshold'''
+        self.send_command(0x06, [0])
+        return self.x06
+    def get_torque(self):
+        '''Get current motor torque, not supported for L20'''
+        return [0] * 5
+    def get_fault(self):
+        self.send_command(0x07,[])
+        time.sleep(0.01)
+        return self.x07
+    
+    def get_temperature(self):
+        '''Get motor temperature'''
+        self.send_command(0x09,[])
+        self.send_command(0x0b,[])
+        self.send_command(0x0c,[])
+        self.send_command(0x0d,[])
+
+        return self.x09+self.x0b+self.x0c+self.x0d
+        
+    def clear_faults(self):
+        '''Clear motor faults'''
+        self.send_command(0x07, [1, 1, 1, 1, 1])
+
+    def get_touch_type(self):
+        '''Get touch type'''
+        t = []
+        for i in range(3):
+            self.send_command(0xb0,[],sleep=0.03)
+        if self.xb0 == [2]:
+            return 2
+        elif self.xb0 == [1]:
+            return 1
+        else:
+            self.send_command(0x20,[],sleep=0.03)
+            time.sleep(0.01)
+            if self.normal_force[0] == -1:
+                return -1
+    
+    def get_touch(self):
+        '''Get touch data'''
+        self.send_command(0xb1,[],sleep=0.03)
+        self.send_command(0xb2,[],sleep=0.03)
+        self.send_command(0xb3,[],sleep=0.03)
+        self.send_command(0xb4,[],sleep=0.03)
+        self.send_command(0xb5,[],sleep=0.03)
+        return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+    def get_matrix_touch(self):
+        self.send_command(0xb1,[0xc6],sleep=0.04)
+        self.send_command(0xb2,[0xc6],sleep=0.04)
+        self.send_command(0xb3,[0xc6],sleep=0.04)
+        self.send_command(0xb4,[0xc6],sleep=0.04)
+        self.send_command(0xb5,[0xc6],sleep=0.04)
+        return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+
+    def get_thumb_matrix_touch(self,sleep_time=0.009):
+        self.send_command(0xb1,[0xc6],sleep=sleep_time)
+        return self.thumb_matrix
+    
+    def get_index_matrix_touch(self,sleep_time=0.009):
+        self.send_command(0xb2,[0xc6],sleep=sleep_time)
+        return self.index_matrix
+    
+    def get_middle_matrix_touch(self,sleep_time=0.009):
+        self.send_command(0xb3,[0xc6],sleep=sleep_time)
+        return self.middle_matrix
+    
+    def get_ring_matrix_touch(self,sleep_time=0.009):
+        self.send_command(0xb4,[0xc6],sleep=sleep_time)
+        return self.ring_matrix
+    
+    def get_little_matrix_touch(self,sleep_time=0.009):
+        self.send_command(0xb5,[0xc6],sleep=sleep_time)
+        return self.little_matrix
+
+
+    def get_faults(self):
+        '''Get motor fault codes'''
+        self.send_command(0x07, [])
+        return self.x07
+    def get_force(self):
+        '''Get pressure sensor data'''
+        return [self.normal_force,self.tangential_force,self.tangential_force_dir,self.approach_inc]
+    
+    def get_serial_number(self):
+        return [0] * 6
+
+    def show_fun_table(self):
+        pass
+    
+    def get_finger_order(self):
+        return []
+    
+    def close_can_interface(self):
+        if self.bus:
+            self.bus.shutdown()  # Close CAN bus

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l21_can.py

@@ -0,0 +1,822 @@
+#!/usr/bin/env python3
+import can
+import time, sys, os
+import threading
+import numpy as np
+from enum import Enum
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+current_dir = os.path.dirname(os.path.abspath(__file__))
+target_dir = os.path.abspath(os.path.join(current_dir, ".."))
+sys.path.append(target_dir)
+
+class FrameProperty(Enum):
+    # Finger motion control - parallel control commands
+    ROLL_POS = 0x01  # Roll joint position
+    YAWPOS = 0x02  # Yaw joint position
+    ROOT1_POS = 0x03  # Root joint 1 position
+    ROOT2_POS = 0x04  # Root joint 2 position
+    ROOT3_POS = 0x05  # Root joint 3 position
+    TIP_POS = 0x06  # Fingertip joint position
+    # Finger motion control - serial control commands
+    THUMB_POS = 0x41  # Thumb joint position
+    INDEX_POS = 0x42  # Index finger joint position
+    MIDDLE_POS = 0x43  # Middle finger joint position
+    RING_POS = 0x44  # Ring finger joint position
+    LITTLE_POS = 0x45  # Little finger joint position
+
+    # Finger motion control - speed
+    ROLL_SPEED = 0x09  # Roll joint speed
+    YAW_SPEED = 0x0A  # Yaw joint speed
+    ROOT1_SPEED = 0x0B  # Root joint 1 speed
+    ROOT2_SPEED = 0x0C  # Root joint 2 speed
+    ROOT3_SPEED = 0x0D  # Root joint 3 speed
+    TIP_SPEED = 0x0E  # Fingertip joint speed
+    THUMB_SPEED = 0x49  # Thumb speed
+    INDEX_SPEED = 0x4A  # Index finger speed
+    MIDDLE_SPEED = 0x4B  # Middle finger speed
+    RING_SPEED = 0x4C  # Ring finger speed
+    LITTLE_SPEED = 0x4D  # Little finger speed
+
+    # Finger motion control - torque
+    ROLL_TORQUE = 0x11  # Roll joint torque
+    YAW_TORQUE = 0x12  # Yaw joint torque
+    ROOT1_TORQUE = 0x13  # Root joint 1 torque
+    ROOT2_TORQUE = 0x14  # Root joint 2 torque
+    ROOT3_TORQUE = 0x15  # Root joint 3 torque
+    TIP_TORQUE = 0x16  # Fingertip joint torque
+    THUMB_TORQUE = 0x51  # Thumb torque
+    INDEX_TORQUE = 0x52  # Index finger torque
+    MIDDLE_TORQUE = 0x53  # Middle finger torque
+    RING_TORQUE = 0x54  # Ring finger torque
+    LITTLE_TORQUE = 0x55  # Little finger torque
+
+    THUMB_FAULT = 0x59  # Thumb fault code | Returns this type of data
+    INDEX_FAULT = 0x5A  # Index finger fault code | Returns this type of data
+    MIDDLE_FAULT = 0x5B  # Middle finger fault code | Returns this type of data
+    RING_FAULT = 0x5C  # Ring finger fault code | Returns this type of data
+    LITTLE_FAULT = 0x5D  # Little finger fault code | Returns this type of data
+
+    # Finger faults and temperature
+    ROLL_FAULT = 0x19  # Roll joint fault code
+    YAW_FAULT = 0x1A  # Yaw joint fault code
+    ROOT1_FAULT = 0x1B  # Root joint 1 fault code
+    ROOT2_FAULT = 0x1C  # Root joint 2 fault code
+    ROOT3_FAULT = 0x1D  # Root joint 3 fault code
+    TIP_FAULT = 0x1E  # Fingertip joint fault code
+    ROLL_TEMPERATURE = 0x21  # Roll joint over-temperature protection threshold
+    YAW_TEMPERATURE = 0x22  # Yaw joint over-temperature protection threshold
+    ROOT1_TEMPERATURE = 0x23  # Root joint 1 over-temperature protection threshold
+    ROOT2_TEMPERATURE = 0x24  # Root joint 2 over-temperature protection threshold
+    ROOT3_TEMPERATURE = 0x25  # Root joint 3 over-temperature protection threshold
+    TIP_TEMPERATURE = 0x26  # Fingertip joint over-temperature protection threshold
+    THUMB_TEMPERATURE = 0x61  # Thumb over-temperature protection threshold
+    INDEX_TEMPERATURE = 0x62  # Index finger over-temperature protection threshold
+    MIDDLE_TEMPERATURE = 0x63  # Middle finger over-temperature protection threshold
+    RING_TEMPERATURE = 0x64  # Ring finger over-temperature protection threshold
+    LITTLE_TEMPERATURE = 0x65  # Little finger over-temperature protection threshold
+
+    # Configuration and preset actions
+    HAND_UID = 0xC0  # Device unique identifier
+    HAND_HARDWARE_VERSION = 0xC1  # Hardware version
+    HAND_SOFTWARE_VERSION = 0xC2  # Software version
+    HAND_COMM_ID = 0xC3  # Device ID
+    HAND_FACTORY_RESET = 0xCE  # Restore factory settings
+    HAND_SAVE_PARAMETER = 0xCF  # Save parameters
+
+    # Tactile sensor data
+    HAND_NORMAL_FORCE = 0x90  # Normal force of five fingers
+    HAND_TANGENTIAL_FORCE = 0x91  # Tangential force of five fingers
+    HAND_TANGENTIAL_FORCE_DIR = 0x92  # Tangential direction of five fingers
+    HAND_APPROACH_INC = 0x93  # Approach sensing of five fingers
+
+    TOUCH_SENSOR_TYPE = 0xB0  # Sensor type
+    THUMB_TOUCH = 0xB1  # Thumb tactile sensing
+    INDEX_TOUCH = 0xB2  # Index finger tactile sensing
+    MIDDLE_TOUCH = 0xB3  # Middle finger tactile sensing
+    RING_TOUCH = 0xB4  # Ring finger tactile sensing
+    LITTLE_TOUCH = 0xB5  # Little finger tactile sensing
+    PALM_TOUCH = 0xB6  # Palm tactile sensing
+
+    # Action control
+    ACTION_PLAY = 0xA0  # Action
+
+    # Combined command area
+    FINGER_SPEED = 0x81  # Set maximum finger speed
+    FINGER_TORQUE = 0x82  # Set maximum finger torque
+    FINGER_FAULT = 0x83  # Clear finger faults and fault codes
+    FINGER_TEMPERATURE = 0x84  # Finger joint temperatures
+
+class LinkerHandL21Can:
+    def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28,yaml=""):
+        self.can_id = can_id
+        self.can_channel = can_channel
+        self.baudrate = baudrate
+        self.open_can = OpenCan(load_yaml=yaml)
+
+        self.running = True
+        self.last_thumb_pos, self.last_index_pos,self.last_ring_pos,self.last_middle_pos, self.last_little_pos = None,None,None,None,None
+        self.x01, self.x02, self.x03, self.x04,self.x05,self.x06,self.x07, self.x08,self.x09,self.x0A,self.x0B,self.x0C,self.x0D,self.x0E,self.speed = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[]
+        self.last_root1,self.last_yaw,self.last_roll,self.last_root2,self.last_tip = None,None,None,None,None
+        # Speed
+        self.x49, self.x4a, self.x4b, self.x4c, self.x4d,self.xc1 = [],[],[],[],[],[]
+        self.x41,self.x42,self.x43,self.x44,self.x45 = [],[],[],[],[]
+        self.x83 = [-1] * 5
+        # Torque
+        self.x51, self.x52, self.x53, self.x54,self.x55 = [],[],[],[],[]
+        # Fault codes
+        self.x59,self.x5a,self.x5b,self.x5c,self.x5d = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+        # Temperature thresholds
+        self.x61,self.x62,self.x63,self.x64,self.x65 = [],[],[],[],[]
+        # Pressure sensors
+        self.x90,self.x91,self.x92,self.x93 = [],[],[],[]
+        # New pressure sensors
+        self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5,self.xb6 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+        self.thumb_matrix = np.full((12, 6), -1)
+        self.index_matrix = np.full((12, 6), -1)
+        self.middle_matrix = np.full((12, 6), -1)
+        self.ring_matrix = np.full((12, 6), -1)
+        self.little_matrix = np.full((12, 6), -1)
+        self.matrix_map = {
+            0: 0,
+            16: 1,
+            32: 2,
+            48: 3,
+            64: 4,
+            80: 5,
+            96: 6,
+            112: 7,
+            128: 8,
+            144: 9,
+            160: 10,
+            176: 11,
+        }
+        # Initialize CAN bus according to operating system
+        # try:
+        #     if sys.platform == "linux":
+        #         self.open_can.open_can(self.can_channel)
+        #         time.sleep(0.1)
+        #         self.bus = can.interface.Bus(
+        #             channel=can_channel, interface="socketcan", bitrate=baudrate, 
+        #             can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+        #         )
+        #     elif sys.platform == "win32":
+        #         self.bus = can.interface.Bus(
+        #             channel=can_channel, interface='pcan', bitrate=baudrate, 
+        #             can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+        #         )
+        #     else:
+        #         raise EnvironmentError("Unsupported platform for CAN interface")
+        # except:
+        #     print("Please insert CAN device")
+        self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate)
+
+        # Start receive thread
+        self.receive_thread = threading.Thread(target=self.receive_response)
+        self.receive_thread.daemon = True
+        self.receive_thread.start()
+
+    def init_can_bus(self, channel, baudrate):
+        """
+        尝试按优先级连接 CAN 总线，并实现回退机制。
+        """
+        # --- 统一异常处理块开始 ---
+        try:
+            if sys.platform == "linux":
+                # Linux 优先级：1. socketcan
+                try:
+                    self.open_can.open_can(self.can_channel)
+                    # 尝试 socketcan
+                    bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+                    ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+                    raise # 重新抛出异常，让外层 try 捕获
+            elif sys.platform == "win32":
+                # Windows 优先级：1. pcan
+                try:
+                    bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+                # Windows 优先级：2. candle (回退方法)
+                try:
+                    bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+                    raise # 两个接口都失败，抛出异常
+            else:
+                raise EnvironmentError("Unsupported platform for CAN interface")
+        # --- 统一异常处理块结束 ---
+        except Exception as e:
+            # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+            ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+            # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+            raise
+
+    def send_command(self, frame_property, data_list,sleep_time=0.003):
+        """
+        Send command to CAN bus
+        :param frame_property: Data frame property
+        :param data_list: Data payload
+        """
+        frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+        data = [frame_property_value] + [int(val) for val in data_list]
+        msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+        try:
+            self.bus.send(msg)
+        except can.CanError as e:
+            print(f"Failed to send message: {e}")
+            self.open_can.open_can(self.can_channel)
+            time.sleep(1)
+            self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+            time.sleep(1)
+            if self.is_can:
+                self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+            else:
+                print("Reconnecting CAN devices ....")
+        time.sleep(sleep_time)
+
+    def receive_response(self):
+        """
+        Receive and process CAN bus response messages
+        """
+        while self.running:
+            try:
+                msg = self.bus.recv(timeout=1.0)
+                if msg:
+                    self.process_response(msg)
+            except can.CanError as e:
+                print(f"Error receiving message: {e}")
+    
+
+    def set_joint_positions(self, joint_ranges):
+        if len(joint_ranges) == 25:
+            l21_pose = self.joint_map(joint_ranges)
+            # Use list comprehension to split the list into subarrays of 6 elements each
+            chunks = [l21_pose[i:i+6] for i in range(0, 30, 6)]
+            for i in range(3):
+                self.send_command(FrameProperty.THUMB_POS, chunks[0])
+                time.sleep(0.001)
+                self.send_command(FrameProperty.INDEX_POS, chunks[1])
+                time.sleep(0.001)
+                self.send_command(FrameProperty.MIDDLE_POS, chunks[2])
+                time.sleep(0.001)
+                self.send_command(FrameProperty.RING_POS, chunks[3])
+                time.sleep(0.001)
+                self.send_command(FrameProperty.LITTLE_POS, chunks[4])
+                time.sleep(0.001)
+
+    def set_joint_positions_by_topic(self, joint_ranges):
+        if len(joint_ranges) == 25:
+            l21_pose = self.slice_list(joint_ranges,5)
+            if self._list_d_value(self.last_root1, l21_pose[0]):
+                self.set_root1_positions(l21_pose[0])
+                self.last_root1 = l21_pose[0]
+            if self._list_d_value(self.last_yaw, l21_pose[1]):  
+                self.set_yaw_positions(l21_pose[1])
+                self.last_yaw = l21_pose[1]
+            if self._list_d_value(self.last_roll, l21_pose[2]):
+                self.set_roll_positions(l21_pose[2])
+                self.last_roll = l21_pose[2]
+            if self._list_d_value(self.last_root2, l21_pose[3]):
+                self.set_root2_positions(l21_pose[3])
+                self.last_root2 = l21_pose[3]
+            if self._list_d_value(self.last_tip, l21_pose[4]): 
+                self.set_tip_positions(l21_pose[4])
+                self.last_tip = l21_pose[4]
+            
+
+    def slice_list(self, input_list, slice_size):
+        """
+        Slice a list into pieces of specified size.
+
+        Args:
+        input_list (list): The list to be sliced.
+        slice_size (int): Number of elements per slice.
+
+        Returns:
+        list of lists: The sliced list.
+        """
+        sliced_list = [input_list[i:i + slice_size] for i in range(0, len(input_list), slice_size)]
+        return sliced_list
+
+    def _list_d_value(self,list1, list2):
+        if list1 == None:
+            return True
+        for a, b in zip(list1, list2):
+            if abs(b - a) > 2:
+                return True
+                break
+        return False
+    # Set all finger roll joint positions
+    def set_roll_positions(self, joint_ranges):
+        self.send_command(FrameProperty.ROLL_POS, joint_ranges)
+    # Set all finger yaw joint positions
+    def set_yaw_positions(self, joint_ranges):
+        self.send_command(FrameProperty.YAW_POS, joint_ranges)
+    # Set all finger root1 joint positions
+    def set_root1_positions(self, joint_ranges):
+        self.send_command(FrameProperty.ROOT1_POS, joint_ranges)
+    # Set all finger root2 joint positions
+    def set_root2_positions(self, joint_ranges):
+        self.send_command(FrameProperty.ROOT2_POS, joint_ranges)
+    # Set all finger root3 joint positions
+    def set_root3_positions(self, joint_ranges):
+        self.send_command(FrameProperty.ROOT3_POS, joint_ranges)
+    # Set all finger tip joint positions
+    def set_tip_positions(self, joint_ranges=[80]*5):
+        self.send_command(FrameProperty.TIP_POS, joint_ranges)
+    # Set thumb torque
+    def set_thumb_torque(self, j=[]):
+        self.send_command(FrameProperty.THUMB_TORQUE, j)
+    # Set index finger torque
+    def set_index_torque(self, j=[]):
+        self.send_command(FrameProperty.INDEX_TORQUE, j)
+    # Set middle finger torque
+    def set_middle_torque(self, j=[]):
+        self.send_command(FrameProperty.MIDDLE_TORQUE, j)
+    # Set ring finger torque
+    def set_ring_torque(self, j=[]):
+        self.send_command(FrameProperty.RING_TORQUE, j)
+    # Set little finger torque
+    def set_little_torque(self, j=[]):
+        self.send_command(FrameProperty.LITTLE_TORQUE, j)
+
+    # Get thumb joint positions
+    def get_thumb_positions(self,j=[0]):
+        self.send_command(FrameProperty.THUMB_POS, j)
+    # Get index finger joint positions
+    def get_index_positions(self, j=[0]):
+        self.send_command(FrameProperty.INDEX_POS,j)
+    # Get middle finger joint positions
+    def get_middle_positions(self, j=[0]):
+        self.send_command(FrameProperty.MIDDLE_POS,j)
+    # Get ring finger joint positions
+    def get_ring_positions(self, j=[0]):
+        self.send_command(FrameProperty.RING_POS,j)
+    # Get little finger joint positions
+    def get_little_positions(self, j=[0]):
+        self.send_command(FrameProperty.LITTLE_POS, j)
+    # Get all thumb motor fault codes
+    def get_thumbn_fault(self,j=[]):
+        self.send_command(FrameProperty.THUMB_FAULT,j)
+    # Get all index finger motor fault codes
+    def get_index_fault(self,j=[]):
+        self.send_command(FrameProperty.INDEX_FAULT,j)
+    # Get all middle finger motor fault codes
+    def get_middle_fault(self,j=[]):
+        self.send_command(FrameProperty.MIDDLE_FAULT,j)
+    # Get all ring finger motor fault codes
+    def get_ring_fault(self,j=[]):
+        self.send_command(FrameProperty.RING_FAULT,j)
+    # Get all little finger motor fault codes
+    def get_little_fault(self,j=[]):
+        self.send_command(FrameProperty.LITTLE_FAULT,j)
+    # Get thumb temperature threshold
+    def get_thumb_threshold(self,j=[]):
+        self.send_command(FrameProperty.THUMB_TEMPERATURE, '')
+    # Get index finger temperature threshold
+    def get_index_threshold(self,j=[]):
+        self.send_command(FrameProperty.INDEX_TEMPERATURE, j)
+    # Get middle finger temperature threshold
+    def get_middle_threshold(self,j=[]):
+        self.send_command(FrameProperty.MIDDLE_TEMPERATURE, j)
+    # Get ring finger temperature threshold
+    def get_ring_threshold(self,j=[]):
+        self.send_command(FrameProperty.RING_TEMPERATURE, j)
+    # Get little finger temperature threshold
+    def get_little_threshold(self,j=[]):
+        self.send_command(FrameProperty.LITTLE_TEMPERATURE, j)
+
+    # Disable mode 01
+    def set_disability_mode(self, j=[1,1,1,1,1]):
+        self.send_command(0x85,j)
+    # Enable mode 00
+    def set_enable_mode(self, j=[00,00,00,00,00]):
+        self.send_command(0x85,j)
+    
+    # Set all finger torques
+    def set_torque(self,torque=[250]*5):
+        t = torque[0]
+        i = torque[1]
+        m = torque[2]
+        r = torque[3]
+        l = torque[4]
+        self.set_thumb_torque(j=[t]*5)
+        self.set_index_torque(j=[i]*5)
+        self.set_middle_torque(j=[m]*5)
+        self.set_ring_torque(j=[r]*5)
+        self.set_little_torque(j=[l]*5)
+    
+    def set_speed(self, speed):
+        self.speed = speed
+        if len(speed) < 25:
+            thumb_speed = [self.speed[0]]*5
+            index_speed = [self.speed[1]]*5
+            middle_speed = [self.speed[2]]*5
+            ring_speed = [self.speed[3]]*5
+            little_speed = [self.speed[4]]*5
+        else:
+            thumb_speed = [self.speed[0],self.speed[1],self.speed[2],self.speed[3],self.speed[4]]
+            index_speed = [self.speed[5],self.speed[6],self.speed[7],self.speed[8],self.speed[9]]
+            middle_speed = [self.speed[10],self.speed[11],self.speed[12],self.speed[13],self.speed[14]]
+            ring_speed = [self.speed[15],self.speed[16],self.speed[17],self.speed[18],self.speed[19]]
+            little_speed = [self.speed[20],self.speed[21],self.speed[22],self.speed[23],self.speed[24]]
+        self.send_command(FrameProperty.THUMB_SPEED, thumb_speed)
+        self.send_command(FrameProperty.INDEX_SPEED, index_speed)
+        self.send_command(FrameProperty.MIDDLE_SPEED, middle_speed)
+        self.send_command(FrameProperty.RING_SPEED, ring_speed)
+        self.send_command(FrameProperty.LITTLE_SPEED, little_speed)
+        
+    def set_finger_torque(self, torque):
+        self.send_command(0x42, torque)
+
+    def request_device_info(self):
+        self.send_command(0xC0, [0])
+        self.send_command(0xC1, [0])
+        self.send_command(0xC2, [0])
+
+    def save_parameters(self):
+        self.send_command(0xCF, [])
+    def process_response(self, msg):
+        if msg.arbitration_id == self.can_id:
+            frame_type = msg.data[0]
+            response_data = msg.data[1:]
+            if len(list(response_data)) == 0:
+                return
+            if frame_type == 0x01:
+                self.x01 = list(response_data)
+            elif frame_type == 0x02:
+                self.x02 = list(response_data)
+            elif frame_type == 0x03:
+                self.x03 = list(response_data)
+            elif frame_type == 0x04:
+                self.x04 = list(response_data)
+            elif frame_type == 0x05:
+                self.x05 = list(response_data)
+            elif frame_type == 0x06:
+                self.x06 = list(response_data)
+            elif frame_type == 0xC0:
+                print(f"Device ID info: {response_data}")
+                if self.can_id == 0x28:
+                    self.right_hand_info = response_data
+                elif self.can_id == 0x27:
+                    self.left_hand_info = response_data
+            elif frame_type == 0x08:
+                self.x08 = list(response_data)
+            elif frame_type == 0x09:
+                self.x09 = list(response_data)
+            elif frame_type == 0x0A:
+                self.x0A = list(response_data)
+            elif frame_type == 0x0B:
+                self.x0B = list(response_data)
+            elif frame_type == 0x0C:
+                self.x0C = list(response_data)
+            elif frame_type == 0x0D:
+                self.x0D = list(response_data)
+            elif frame_type == 0x22:
+                d = list(response_data)
+                self.tangential_force_dir = [float(i) for i in d]
+            elif frame_type == 0x23:
+                d = list(response_data)
+                self.approach_inc = [float(i) for i in d]
+            elif frame_type == 0x41:
+                self.x41 = list(response_data)
+            elif frame_type == 0x42:
+                self.x42 = list(response_data)
+            elif frame_type == 0x43:
+                self.x43 = list(response_data)
+            elif frame_type == 0x44:
+                self.x44 = list(response_data)
+            elif frame_type == 0x45:
+                self.x45 = list(response_data)
+            elif frame_type == 0x49:
+                self.x49 = list(response_data)
+            elif frame_type == 0x4a:
+                self.x4a = list(response_data)
+            elif frame_type == 0x4b:
+                self.x4b = list(response_data)
+            elif frame_type == 0x4c:
+                self.x4c = list(response_data)
+            elif frame_type == 0x4d:
+                self.x4d = list(response_data)
+            elif frame_type == 0xc1:
+                self.xc1 = list(response_data)
+            elif frame_type == 0x51:
+                self.x51 = list(response_data)
+            elif frame_type == 0x52:
+                self.x52 = list(response_data)
+            elif frame_type == 0x53:
+                self.x53 = list(response_data)
+            elif frame_type == 0x54:
+                self.x54 = list(response_data)
+            elif frame_type == 0x55:
+                self.x55 = list(response_data)
+            elif frame_type == 0x59:
+                self.x59 = list(response_data)
+            elif frame_type == 0x5a:
+                self.x5a = list(response_data)
+            elif frame_type == 0x5b:
+                self.x5b = list(response_data)
+            elif frame_type == 0x5c:
+                self.x5c = list(response_data)
+            elif frame_type == 0x5d:
+                self.x5d = list(response_data)
+            elif frame_type == 0x61:
+                self.x61 = list(response_data)
+            elif frame_type == 0x62:
+                self.x62 = list(response_data)
+            elif frame_type == 0x63:
+                self.x63 = list(response_data)
+            elif frame_type == 0x64:
+                self.x64 = list(response_data)
+            elif frame_type == 0x65:
+                self.x65 = list(response_data)
+            elif frame_type == 0x83:
+                self.x83 = list(response_data)
+            elif frame_type == 0x90:
+                self.x90 = list(response_data)
+            elif frame_type == 0x91:
+                self.x91 = list(response_data)
+            elif frame_type == 0x92:
+                self.x92 = list(response_data)
+            elif frame_type == 0x93:
+                self.x93 = list(response_data)
+            elif frame_type == 0xb0:
+                self.xb0 = list(response_data)
+            elif frame_type == 0xb1:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb1 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.thumb_matrix[index] = d[1:]
+            elif frame_type == 0xb2:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb2 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.index_matrix[index] = d[1:]
+            elif frame_type == 0xb3:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb3 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.middle_matrix[index] = d[1:]
+            elif frame_type == 0xb4:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb4 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.ring_matrix[index] = d[1:]
+            elif frame_type == 0xb5:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb5 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.little_matrix[index] = d[1:]
+            elif frame_type == 0xb6:
+                self.xb6 = list(response_data)
+
+    def joint_map(self, pose):
+        # l21 CAN data by default receives 30 data
+        l21_pose = [0.0] * 30
+
+        mapping = {
+            0: 10,  1: 5,   2: 0,   3: 15,  4: None,  5: 20,
+            6: None, 7: 6,   8: 1,   9: 16,  10: None, 11: 21,
+            12: None, 13: 7, 14: 2,  15: 17, 16: None, 17: 22,
+            18: None, 19: 8,  20: 3,   21: 18, 22: None, 23: 23,
+            24: None, 25: 9,  26: 4,   27: 19, 28: None, 29: 24
+        }
+
+        for l21_idx, pose_idx in mapping.items():
+            if pose_idx is not None:
+                l21_pose[l21_idx] = pose[pose_idx]
+
+        return l21_pose
+
+    def state_to_cmd(self, l21_state):
+        pose = [0.0] * 25
+
+        mapping = {
+            0: 10,  1: 5,   2: 0,   3: 15,  5: 20,  7: 6,
+            8: 1,   9: 16,  11: 21, 13:7, 14: 2,  15: 17, 17: 22,
+            19: 8,  20: 3,  21: 18, 23: 23, 25: 9,   26: 4,
+            27: 19, 29: 24
+        }
+        for l21_idx, pose_idx in mapping.items():
+            pose[pose_idx] = l21_state[l21_idx]
+        return pose
+    def action_play(self):
+        self.send_command(0xA0,[])
+    def get_current_status(self, j=''):
+        self.send_command(FrameProperty.THUMB_POS, j,sleep_time=0.001)
+        self.send_command(FrameProperty.INDEX_POS,j,sleep_time=0.001)
+        self.send_command(FrameProperty.MIDDLE_POS,j,sleep_time=0.001)
+        self.send_command(FrameProperty.RING_POS,j,sleep_time=0.001)
+        self.send_command(FrameProperty.LITTLE_POS, j,sleep_time=0.001)
+        state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+        if len(state) == 30:
+            l21_state = self.state_to_cmd(l21_state=state)
+            return l21_state
+        
+    def get_current_pub_status(self):
+        state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+        if len(state) == 30:
+            l21_state = self.state_to_cmd(l21_state=state)
+            return l21_state
+        
+    def get_current_state_topic(self):
+        self.send_command(0x01,[])
+        self.send_command(0x02,[])
+        self.send_command(0x03,[])
+        self.send_command(0x04,[])
+        self.send_command(0x06,[])
+        state = self.x03+self.x02+self.x01+self.x04+self.x06
+        return state
+    
+    def get_speed(self,j=''):
+        self.send_command(FrameProperty.THUMB_SPEED, j)
+        self.send_command(FrameProperty.INDEX_SPEED, j)
+        self.send_command(FrameProperty.MIDDLE_SPEED, j)
+        self.send_command(FrameProperty.RING_SPEED, j)
+        self.send_command(FrameProperty.LITTLE_SPEED, j)
+        speed = self.x49+ self.x4a+ self.x4b+ self.x4c+ self.x4d
+        if len(speed) == 30:
+            l21_speed = self.state_to_cmd(l21_state=speed)
+            return l21_speed
+    
+    # def get_finger_torque(self):
+    #     return self.finger_torque()
+    def get_fault(self):
+        self.get_thumbn_fault()
+        self.get_index_fault()
+        self.get_middle_fault()
+        self.get_ring_fault()
+        self.get_little_fault()
+        return [self.x59]+[self.x5a]+[self.x5b]+[self.x5c]+[self.x5d]
+    def get_threshold(self):
+        self.get_thumb_threshold()
+        self.get_index_threshold()
+        self.get_middle_threshold()
+        self.get_ring_threshold()
+        self.get_little_threshold()
+        return [self.x61]+[self.x62]+[self.x63]+[self.x64]+[self.x65]
+    def get_version(self):
+        if self.xc1 == []:
+            self.send_command(FrameProperty.HAND_HARDWARE_VERSION,[])
+        return self.xc1
+    def get_normal_force(self):
+        self.send_command(FrameProperty.HAND_NORMAL_FORCE,[])
+        return self.x90
+    def get_tangential_force(self):
+        self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE,[])
+        return self.x91
+    def get_tangential_force_dir(self):
+        self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE_DIR,[])
+        return self.x92
+    def get_approach_inc(self):
+        self.send_command(FrameProperty.HAND_APPROACH_INC,[])
+        return self.x93
+    
+    def get_touch_type(self):
+        '''Get tactile sensor type data'''
+        self.send_command(FrameProperty.TOUCH_SENSOR_TYPE,[])
+        try:
+            return self.xb0[0]
+        except:
+            pass
+    def get_finger_torque(self):
+        self.send_command(FrameProperty.THUMB_TORQUE,[])
+        self.send_command(FrameProperty.INDEX_TORQUE,[])
+        self.send_command(FrameProperty.MIDDLE_TORQUE,[])
+        self.send_command(FrameProperty.RING_TORQUE,[])
+        self.send_command(FrameProperty.LITTLE_TORQUE,[])
+        return self.x51+self.x52+self.x53+self.x54+self.x55
+    
+    def get_torque(self):
+        return self.get_finger_torque()
+    
+    def get_thumb_touch(self):
+        '''Get thumb tactile sensor data'''
+        self.send_command(FrameProperty.THUMB_TOUCH,[],sleep_time=0.015)
+        return self.xb1
+    
+    def get_index_touch(self):
+        '''Get index finger tactile sensor data'''
+        self.send_command(FrameProperty.INDEX_TOUCH,[0xc6],sleep_time=0.015)
+        return self.xb2
+    
+    def get_middle_touch(self):
+        '''Get middle finger tactile sensor data'''
+        self.send_command(FrameProperty.MIDDLE_TOUCH,[],sleep_time=0.015)
+        return self.xb3
+    
+    def get_ring_touch(self):
+        '''Get ring finger tactile sensor data'''
+        self.send_command(FrameProperty.RING_TOUCH,[],sleep_time=0.015)
+        return self.xb4
+    
+    def get_little_touch(self):
+        '''Get little finger tactile sensor data'''
+        self.send_command(FrameProperty.LITTLE_TOUCH,[],sleep_time=0.015)
+        return self.xb5
+    
+    def get_palm_touch(self):
+        '''Get palm tactile sensor data'''
+        self.send_command(FrameProperty.PALM_TOUCH,[],sleep_time=0.015)
+        return self.xb6
+    
+    def get_force(self):
+        '''Get pressure sensor data'''
+        return [self.x90,self.x91 , self.x92 , self.x93]
+    
+    def get_touch(self):
+        '''Get tactile sensor data'''
+        self.get_thumb_touch()
+        self.get_index_touch()
+        self.get_middle_touch()
+        self.get_ring_touch()
+        self.get_little_touch()
+        self.get_palm_touch()
+        try:
+            return [self.xb1[1],self.xb2[1] , self.xb3[1] , self.xb4[1],self.xb5[1],self.xb6[1]]
+        except:
+            pass
+
+    def get_matrix_touch(self):
+        self.send_command(0xb1,[0xc6],sleep_time=0.04)
+        self.send_command(0xb2,[0xc6],sleep_time=0.04)
+        self.send_command(0xb3,[0xc6],sleep_time=0.04)
+        self.send_command(0xb4,[0xc6],sleep_time=0.04)
+        self.send_command(0xb5,[0xc6],sleep_time=0.04)
+        return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+    def get_current(self):
+        '''Not supported yet'''
+        return [0] * 21
+    def get_temperature(self):
+        self.get_thumb_threshold()
+        self.get_index_threshold()
+        self.get_middle_threshold()
+        self.get_ring_threshold()
+        self.get_little_threshold()
+        return self.x61+self.x62+self.x63+self.x64+self.x65
+    
+    def get_serial_number(self):
+        return [0] * 6
+
+    def get_finger_order(self):
+        return [
+            "thumb_root",
+            "index_finger_root",
+            "middle_finger_root",
+            "ring_finger_root",
+            "little_finger_root",
+            "thumb_abduction",
+            "index_finger_abduction",
+            "middle_finger_abduction",
+            "ring_finger_abduction",
+            "little_finger_abduction",
+            "thumb_roll",
+            "reserved",
+            "reserved",
+            "reserved",
+            "reserved",
+            "thumb_middle_joint",
+            "reserved",
+            "reserved",
+            "reserved",
+            "reserved",
+            "thumb_tip",
+            "index_finger_tip",
+            "middle_finger_tip",
+            "ring_finger_tip",
+            "little_finger_tip"
+        ]
+
+    def clear_faults(self):
+        '''Clear motor faults'''
+        self.send_command(0x83, [1, 1, 1, 1, 1],sleep_time=0.003)
+        return self.x83
+
+    def close_can_interface(self):
+        if self.bus:
+            self.bus.shutdown()  # Close CAN bus

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l24_can.py

@@ -0,0 +1,448 @@
+#!/usr/bin/env python3
+import can
+import time,sys,os
+import threading
+import numpy as np
+from enum import Enum
+current_dir = os.path.dirname(os.path.abspath(__file__))
+target_dir = os.path.abspath(os.path.join(current_dir, ".."))
+sys.path.append(target_dir)
+from utils.color_msg import ColorMsg
+
+class FrameProperty(Enum):
+    INVALID_FRAME_PROPERTY = 0x00  # 无效的can帧属性 | 无返回
+    # 并行指令区域
+    ROLL_POS = 0x01  # 横滚关节位置 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+    YAW_POS = 0x02  # 航向关节位置 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+    ROOT1_POS = 0x03  # 指根1关节位置 | 最接近手掌的指根关节
+    ROOT2_POS = 0x04  # 指根2关节位置 | 最接近手掌的指根关节
+    ROOT3_POS = 0x05  # 指根3关节位置 | 最接近手掌的指根关节
+    TIP_POS = 0x06  # 指尖关节位置 | 最接近手掌的指根关节
+
+    ROLL_SPEED = 0x09  # 横滚关节速度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+    YAW_SPEED = 0x0A  # 航向关节速度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+    ROOT1_SPEED = 0x0B  # 指根1关节速度 | 最接近手掌的指根关节
+    ROOT2_SPEED = 0x0C  # 指根2关节速度 | 最接近手掌的指根关节
+    ROOT3_SPEED = 0x0D  # 指根3关节速度 | 最接近手掌的指根关节
+    TIP_SPEED = 0x0E  # 指尖关节速度 | 最接近手掌的指根关节
+
+    ROLL_TORQUE = 0x11  # 横滚关节扭矩 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+    YAW_TORQUE = 0x12  # 航向关节扭矩 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+    ROOT1_TORQUE = 0x13  # 指根1关节扭矩 | 最接近手掌的指根关节
+    ROOT2_TORQUE = 0x14  # 指根2关节扭矩 | 最接近手掌的指根关节
+    ROOT3_TORQUE = 0x15  # 指根3关节扭矩 | 最接近手掌的指根关节
+    TIP_TORQUE = 0x16  # 指尖关节扭矩 | 最接近手掌的指根关节
+
+    ROLL_FAULT = 0x19  # 横滚关节故障码 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+    YAW_FAULT = 0x1A  # 航向关节故障码 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+    ROOT1_FAULT = 0x1B  # 指根1关节故障码 | 最接近手掌的指根关节
+    ROOT2_FAULT = 0x1C  # 指根2关节故障码 | 最接近手掌的指根关节
+    ROOT3_FAULT = 0x1D  # 指根3关节故障码 | 最接近手掌的指根关节
+    TIP_FAULT = 0x1E  # 指尖关节故障码 | 最接近手掌的指根关节
+
+    ROLL_TEMPERATURE = 0x21  # 横滚关节温度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+    YAW_TEMPERATURE = 0x22  # 航向关节温度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+    ROOT1_TEMPERATURE = 0x23  # 指根1关节温度 | 最接近手掌的指根关节
+    ROOT2_TEMPERATURE = 0x24  # 指根2关节温度 | 最接近手掌的指根关节
+    ROOT3_TEMPERATURE = 0x25  # 指根3关节温度 | 最接近手掌的指根关节
+    TIP_TEMPERATURE = 0x26  # 指尖关节温度 | 最接近手掌的指根关节
+    # 并行指令区域
+
+    # 串行指令区域
+    THUMB_POS = 0x41  # 大拇指指关节位置 | 返回本类型数据
+    INDEX_POS = 0x42  # 食指关节位置 | 返回本类型数据
+    MIDDLE_POS = 0x43  # 中指关节位置 | 返回本类型数据
+    RING_POS = 0x44  # 无名指关节位置 | 返回本类型数据
+    LITTLE_POS = 0x45  # 小拇指关节位置 | 返回本类型数据
+
+    THUMB_SPEED = 0x49  # 大拇指速度 | 返回本类型数据
+    INDEX_SPEED = 0x4A  # 食指速度 | 返回本类型数据
+    MIDDLE_SPEED = 0x4B  # 中指速度 | 返回本类型数据
+    RING_SPEED = 0x4C  # 无名指速度 | 返回本类型数据
+    LITTLE_SPEED = 0x4D  # 小拇指速度 | 返回本类型数据
+
+    THUMB_TORQUE = 0x51  # 大拇指扭矩 | 返回本类型数据
+    INDEX_TORQUE = 0x52  # 食指扭矩 | 返回本类型数据
+    MIDDLE_TORQUE = 0x53  # 中指扭矩 | 返回本类型数据
+    RING_TORQUE = 0x54  # 无名指扭矩 | 返回本类型数据
+    LITTLE_TORQUE = 0x55  # 小拇指扭矩 | 返回本类型数据
+
+    THUMB_FAULT = 0x59  # 大拇指故障码 | 返回本类型数据
+    INDEX_FAULT = 0x5A  # 食指故障码 | 返回本类型数据
+    MIDDLE_FAULT = 0x5B  # 中指故障码 | 返回本类型数据
+    RING_FAULT = 0x5C  # 无名指故障码 | 返回本类型数据
+    LITTLE_FAULT = 0x5D  # 小拇指故障码 | 返回本类型数据
+
+    THUMB_TEMPERATURE = 0x61  # 大拇指温度 | 返回本类型数据
+    INDEX_TEMPERATURE = 0x62  # 食指温度 | 返回本类型数据
+    MIDDLE_TEMPERATURE = 0x63  # 中指温度 | 返回本类型数据
+    RING_TEMPERATURE = 0x64  # 无名指温度 | 返回本类型数据
+    LITTLE_TEMPERATURE = 0x65  # 小拇指温度 | 返回本类型数据
+    # 串行指令区域
+
+    # 合并指令区域，同一手指非必要单控数据合并
+    FINGER_SPEED = 0x81  # 手指速度 | 返回本类型数据
+    FINGER_TORQUE = 0x82  # 转矩 | 返回本类型数据
+    FINGER_FAULT = 0x83  # 手指故障码 | 返回本类型数据
+
+    # 指尖传感器数据组
+    HAND_NORMAL_FORCE = 0x90  # 五指法向压力
+    HAND_TANGENTIAL_FORCE = 0x91  # 五指切向压力
+    HAND_TANGENTIAL_FORCE_DIR = 0x92  # 五指切向方向
+    HAND_APPROACH_INC = 0x93  # 五指接近感应
+
+    THUMB_ALL_DATA = 0x98  # 大拇指所有数据
+    INDEX_ALL_DATA = 0x99  # 食指所有数据
+    MIDDLE_ALL_DATA = 0x9A  # 中指所有数据
+    RING_ALL_DATA = 0x9B  # 无名指所有数据
+    LITTLE_ALL_DATA = 0x9C  # 小拇指所有数据
+    # 动作指令 ·ACTION
+    ACTION_PLAY = 0xA0  # 动作
+
+    # 配置命令·CONFIG
+    HAND_UID = 0xC0  # 设备唯一标识码
+    HAND_HARDWARE_VERSION = 0xC1  # 硬件版本
+    HAND_SOFTWARE_VERSION = 0xC2  # 软件版本
+    HAND_COMM_ID = 0xC3  # 设备id
+    HAND_FACTORY_RESET = 0xCE  # 恢复出厂设置
+    HAND_SAVE_PARAMETER = 0xCF  # 保存参数
+
+    WHOLE_FRAME = 0xF0  # 整帧传输 | 返回一字节帧属性+整个结构体485及网络传输专属
+
+class LinkerHandL24Can:
+    def __init__(self, config, can_channel='can0', baudrate=1000000, can_id=0x28):
+        self.config = config
+        self.can_id = can_id
+        self.running = True
+        self.x01, self.x02, self.x03, self.x04,self.x05,self.x06,self.x07, self.x08,self.x09,self.x0A,self.x0B,self.x0C,self.x0D,self.x0E,self.speed = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[]
+        # 速度
+        self.x49, self.x4a, self.x4b, self.x4c, self.x4d = [],[],[],[],[]
+        self.x41,self.x42,self.x43,self.x44,self.x45 = [],[],[],[],[]
+        # 根据操作系统初始化 CAN 总线
+        if sys.platform == "linux":
+            self.bus = can.interface.Bus(
+                channel=can_channel, interface="socketcan", bitrate=baudrate, 
+                can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+            )
+        elif sys.platform == "win32":
+            self.bus = can.interface.Bus(
+                channel=can_channel, interface='pcan', bitrate=baudrate, 
+                can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+            )
+        else:
+            raise EnvironmentError("Unsupported platform for CAN interface")
+
+        # 根据 can_id 初始化 publisher 和相关参数
+        if can_id == 0x28:  # 左手
+            self.hand_exists = config['LINKER_HAND']['LEFT_HAND']['EXISTS']
+            self.hand_joint = config['LINKER_HAND']['LEFT_HAND']['JOINT']
+            self.hand_names = config['LINKER_HAND']['LEFT_HAND']['NAME']
+        elif can_id == 0x27:  # 右手
+
+            self.hand_exists = config['LINKER_HAND']['RIGHT_HAND']['EXISTS']
+            self.hand_joint = config['LINKER_HAND']['RIGHT_HAND']['JOINT']
+            self.hand_names = config['LINKER_HAND']['RIGHT_HAND']['NAME']
+
+
+        # 启动接收线程
+        self.receive_thread = threading.Thread(target=self.receive_response)
+        self.receive_thread.daemon = True
+        self.receive_thread.start()
+
+    def send_command(self, frame_property, data_list):
+        """
+        发送命令到 CAN 总线
+        :param frame_property: 数据帧属性
+        :param data_list: 数据载荷
+        """
+        frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+        data = [frame_property_value] + [int(val) for val in data_list]
+        msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+        try:
+            self.bus.send(msg)
+            #print(f"Message sent: ID={hex(self.can_id)}, Data={data}")
+        except can.CanError as e:
+            print(f"Failed to send message: {e}")
+        time.sleep(0.002)
+
+    def receive_response(self):
+        """
+        接收并处理 CAN 总线的响应消息
+        """
+        while self.running:
+            try:
+                msg = self.bus.recv(timeout=1.0)  # 阻塞接收，1 秒超时
+                if msg:
+                    self.process_response(msg)
+            except can.CanError as e:
+                print(f"Error receiving message: {e}")
+    
+
+    def set_joint_positions(self, joint_ranges):
+        if len(joint_ranges) == 25:
+            l24_pose = self.joint_map(joint_ranges)
+            # 使用列表推导式将列表每6个元素切成一个子数组
+            chunks = [l24_pose[i:i+6] for i in range(0, 30, 6)]
+            self.send_command(FrameProperty.THUMB_POS, chunks[0])
+            self.send_command(FrameProperty.INDEX_POS, chunks[1])
+            self.send_command(FrameProperty.MIDDLE_POS, chunks[2])
+            self.send_command(FrameProperty.RING_POS, chunks[3])
+            self.send_command(FrameProperty.LITTLE_POS, chunks[4])
+        #self.set_tip_positions(joint_ranges[:5])
+        #print(l24_pose)
+    
+    # 设置所有手指横滚关节位置
+    def set_roll_positions(self, joint_ranges):
+        self.send_command(FrameProperty.ROLL_POS, joint_ranges)
+    # 设置所有手指航向关节位置
+    def set_yaw_positions(self, joint_ranges):
+        self.send_command(FrameProperty.YAW_POS, joint_ranges)
+    # 设置所有手指指根1关节位置
+    def set_root1_positions(self, joint_ranges):
+        self.send_command(FrameProperty.ROOT1_POS, joint_ranges)
+    # 设置所有手指指根2关节位置
+    def set_root2_positions(self, joint_ranges):
+        self.send_command(FrameProperty.ROOT2_POS, joint_ranges)
+    # 设置所有手指指根3关节位置
+    def set_root3_positions(self, joint_ranges):
+        self.send_command(FrameProperty.ROOT3_POS, joint_ranges)
+    # 设置所有手指指尖关节位置
+    def set_tip_positions(self, joint_ranges=[80]*5):
+        self.send_command(FrameProperty.TIP_POS, joint_ranges)
+    # 获取大拇指指关节位置
+    def get_thumb_positions(self,j=[0]):
+        self.send_command(FrameProperty.THUMB_POS, j)
+    # 获取食指关节位置
+    def get_index_positions(self, j=[0]):
+        self.send_command(FrameProperty.INDEX_POS,j)
+    # 获取中指关节位置
+    def get_middle_positions(self, j=[0]):
+        self.send_command(FrameProperty.MIDDLE_POS,j)
+    # 获取无名指关节位置
+    def get_ring_positions(self, j=[0]):
+        self.send_command(FrameProperty.RING_POS,j)
+    # 获取小拇指关节位置
+    def get_little_positions(self, j=[0]):
+        self.send_command(FrameProperty.LITTLE_POS, j)
+    # 失能01模式
+    def set_disability_mode(self, j=[1,1,1,1,1]):
+        self.send_command(0x85,j)
+    # 使能00模式
+    def set_enable_mode(self, j=[00,00,00,00,00]):
+        self.send_command(0x85,j)
+
+    
+    def set_speed(self, speed):
+        self.speed = [speed]*6
+        ColorMsg(msg=f"L24设置速度为:{self.speed}", color="yellow")
+        self.send_command(FrameProperty.THUMB_SPEED, self.speed)
+        self.send_command(FrameProperty.INDEX_SPEED, self.speed)
+        self.send_command(FrameProperty.MIDDLE_SPEED, self.speed)
+        self.send_command(FrameProperty.RING_SPEED, self.speed)
+        self.send_command(FrameProperty.LITTLE_SPEED, self.speed)
+        
+    def set_finger_torque(self, torque):
+        self.send_command(0x42, torque)
+
+    def request_device_info(self):
+        self.send_command(0xC0, [0])
+        self.send_command(0xC1, [0])
+        self.send_command(0xC2, [0])
+
+    def save_parameters(self):
+        self.send_command(0xCF, [])
+    def process_response(self, msg):
+        if msg.arbitration_id == self.can_id:
+            frame_type = msg.data[0]
+            response_data = msg.data[1:]
+            if frame_type == 0x01:
+                self.x01 = list(response_data)
+            elif frame_type == 0x02:
+                self.x02 = list(response_data)
+            elif frame_type == 0x03:
+                self.x03 = list(response_data)
+            elif frame_type == 0x04:
+                self.x04 = list(response_data)
+            elif frame_type == 0x05:
+                self.x05 = list(response_data)
+            elif frame_type == 0x06:
+                self.x06 = list(response_data)
+                print("_-"*20)
+                print(self.x06)
+            elif frame_type == 0xC0:
+                print(f"Device ID info: {response_data}")
+                if self.can_id == 0x28:
+                    self.right_hand_info = response_data
+                elif self.can_id == 0x27:
+                    self.left_hand_info = response_data
+            elif frame_type == 0x08:
+                self.x08 = list(response_data)
+            elif frame_type == 0x09:
+                self.x09 = list(response_data)
+            elif frame_type == 0x0A:
+                self.x0A = list(response_data)
+            elif frame_type == 0x0B:
+                self.x0B = list(response_data)
+            elif frame_type == 0x0C:
+                self.x0C = list(response_data)
+            elif frame_type == 0x0D:
+                self.x0D = list(response_data)
+            elif frame_type == 0x22:
+                #ColorMsg(msg=f"五指切向压力方向：{list(response_data)}")
+                d = list(response_data)
+                self.tangential_force_dir = [float(i) for i in d]
+            elif frame_type == 0x23:
+                #ColorMsg(msg=f"五指接近度：{list(response_data)}")
+                d = list(response_data)
+                self.approach_inc = [float(i) for i in d]
+            elif frame_type == 0x41: # 拇指关节位置返回值
+                self.x41 = list(response_data)
+            elif frame_type == 0x42: # 食指关节位置返回值
+                self.x42 = list(response_data)
+            elif frame_type == 0x43: # 中指关节位置返回值
+                self.x43 = list(response_data)
+            elif frame_type == 0x44: # 无名指关节位置返回值
+                self.x44 = list(response_data)
+            elif frame_type == 0x45: # 小拇指关节位置返回值
+                self.x45 = list(response_data)
+            elif frame_type == 0x49: # 拇指速度返回值
+                self.x49 = list(response_data)
+            elif frame_type == 0x4a: # 食指速度返回值
+                self.x4a = list(response_data)
+            elif frame_type == 0x4b: # 中指速度返回值
+                self.x4b = list(response_data)
+            elif frame_type == 0x4c: # 无名指速度返回值
+                self.x4c = list(response_data)
+            elif frame_type == 0x4d: # 小拇指速度返回值
+                self.x4d = list(response_data)
+
+    # topic映射L24
+    def joint_map(self, pose):
+        # L24 CAN数据默认接收30个数据
+        l24_pose = [0.0] * 30  # 初始化l24_pose为30个0.0
+
+        # 映射表，通过字典简化映射关系
+        mapping = {
+            0: 10,  1: 5,   2: 0,   3: 15,  4: None,  5: 20,
+            6: None, 7: 6,   8: 1,   9: 16,  10: None, 11: 21,
+            12: None, 13: None, 14: 2,  15: 17, 16: None, 17: 22,
+            18: None, 19: 8,  20: 3,   21: 18, 22: None, 23: 23,
+            24: None, 25: 9,  26: 4,   27: 19, 28: None, 29: 24
+        }
+
+        # 遍历映射字典，进行值的映射
+        for l24_idx, pose_idx in mapping.items():
+            if pose_idx is not None:
+                l24_pose[l24_idx] = pose[pose_idx]
+
+        return l24_pose
+
+    # 将L24的状态值转换为CMD格式的状态值
+    def state_to_cmd(self, l24_state):
+        # L24 CAN默认接收30个数据，初始化pose为25个0.0
+        pose = [0.0] * 25  # 原来控制L24的指令数据为25个
+
+        # 映射关系，字典中存储l24_state索引和pose索引之间的映射关系
+        mapping = {
+            0: 10,  1: 5,   2: 0,   3: 15,  5: 20,  7: 6,
+            8: 1,   9: 16,  11: 21, 14: 2,  15: 17, 17: 22,
+            19: 8,  20: 3,  21: 18, 23: 23, 25: 9,   26: 4,
+            27: 19, 29: 24
+        }
+        # 遍历映射字典，更新pose的值
+        for l24_idx, pose_idx in mapping.items():
+            pose[pose_idx] = l24_state[l24_idx]
+        return pose
+
+    # 获取所有关节数据
+    def get_current_status(self, j=''):
+        time.sleep(0.01)
+        self.send_command(FrameProperty.THUMB_POS, j)
+        self.send_command(FrameProperty.INDEX_POS,j)
+        self.send_command(FrameProperty.MIDDLE_POS,j)
+        self.send_command(FrameProperty.RING_POS,j)
+        self.send_command(FrameProperty.LITTLE_POS, j)
+        #return self.x41, self.x42, self.x43, self.x44, self.x45
+        time.sleep(0.1)
+        state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+        if len(state) == 30:
+            l24_state = self.state_to_cmd(l24_state=state)
+            return l24_state
+    
+    def get_speed(self,j=''):
+        time.sleep(0.1)
+        self.send_command(FrameProperty.THUMB_SPEED, j) # 大拇指速度
+        self.send_command(FrameProperty.INDEX_SPEED, j) # 食指速度
+        self.send_command(FrameProperty.MIDDLE_SPEED, j) # 中指速度
+        self.send_command(FrameProperty.RING_SPEED, j) # 无名指速度
+        self.send_command(FrameProperty.LITTLE_SPEED, j) # 小拇指速度
+        speed = self.x49+ self.x4a+ self.x4b+ self.x4c+ self.x4d
+        if len(speed) == 30:
+            l24_speed = self.state_to_cmd(l24_state=speed)
+            return l24_speed
+    
+    def get_finger_torque(self):
+        return self.finger_torque
+    # def get_current(self):
+    #     return self.x06
+    # def get_fault(self):
+    #     return self.x07
+
+    def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+        """L24 暂不支持清除故障码"""
+        pass
+    
+    def close_can_interface(self):
+        if self.bus:
+            self.bus.shutdown()  # 关闭 CAN 总线
+
+    '''
+    这个方法只用于展示数据关系映射，使用的话最好使用上面的方法
+    '''
+    def joint_map_2(self, pose):
+        l24_pose = [0.0]*30 #L24 CAN默认接收30个数据 pose控制L24发送的指令数据默认25个，这里进行映射
+        '''
+        需要进行映射
+        # L24 CAN数据格式
+        #["拇指横摆0-10", "拇指侧摆1-5", "拇指根部2-0", "拇指中部3-15", "预留4-", "拇指指尖5-20", "预留6-", "食指侧摆7-6", "食指根部8-1", "食指中部9-16", "预留10-", "食指指尖11-21", "预留12-", "预留13-", "中指根部14-2", "中指中部15-17", "预留16-", "中指指尖17-22", "预留18-", "无名指侧摆19-8", "无名指根部20-3", "无名指中部21-18", "预留22-", "无名指指尖23-23", "预留24-", "小指侧摆25-9", "小指根部26-4", "小指中部27-19", "预留28-", "小指指尖29-24"]
+        # CMD 接收到的数据格式
+        #["拇指根部0", "食指根部1", "中指根部2", "无名指根部3","小指根部4","拇指侧摆5","食指侧摆6","中指侧摆","无名指侧摆8","小指侧摆9","拇指横摆10","预留","预留","预留","预留","拇指中部15","食指中部16","中指中部17","无名指中部18","小指中部19","拇指指尖20","食指指尖21","中指指尖22","无名指指尖23","小指指尖24"]
+        '''
+        l24_pose[0] = pose[10]
+        l24_pose[1] = pose[5]
+        l24_pose[2] = pose[0]
+        l24_pose[3] = pose[15]
+        l24_pose[4] = 0.0
+        l24_pose[5] = pose[20]
+        l24_pose[6] = 0.0
+        l24_pose[7] = pose[6]
+        l24_pose[8] = pose[1]
+        l24_pose[9] = pose[16]
+        l24_pose[10] = 0.0
+        l24_pose[11] = pose[21]
+        l24_pose[12] = 0.0
+        l24_pose[13] = 0.0
+        l24_pose[14] = pose[2]
+        l24_pose[15] = pose[17]
+        l24_pose[16] = 0.0
+        l24_pose[17] = pose[22]
+        l24_pose[18] = 0.0
+        l24_pose[19] = pose[8]
+        l24_pose[20] = pose[3]
+        l24_pose[21] = pose[18]
+        l24_pose[22] = 0.0
+        l24_pose[23] = pose[23]
+        l24_pose[24] = 0.0
+        l24_pose[25] = pose[9]
+        l24_pose[26] = pose[4]
+        l24_pose[27] = pose[19]
+        l24_pose[28] = 0.0
+        l24_pose[29] = pose[24]
+        return l24_pose
+    
+    def get_finger_order(self):
+        return []
+    def get_serial_number(self):
+        return [0] * 6
+    def show_fun_table(self):
+        pass

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l25_can.py

@@ -0,0 +1,848 @@
+#!/usr/bin/env python3
+import can
+import time,sys,os
+import threading
+import numpy as np
+from enum import Enum
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+current_dir = os.path.dirname(os.path.abspath(__file__))
+target_dir = os.path.abspath(os.path.join(current_dir, ".."))
+sys.path.append(target_dir)
+
+class FrameProperty(Enum):
+    INVALID_FRAME_PROPERTY = 0x00  # Invalid CAN frame property | No response
+    # Parallel command area
+    ROLL_POS = 0x01  # Roll joint position | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger [10,11,12,13,14]
+    YAW_POS = 0x02  # Yaw joint position | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger [5,6,7,8,9]
+    ROOT1_POS = 0x03  # Root1 joint position | The root joint closest to the palm [0,1,2,3,4]
+    ROOT2_POS = 0x04  # Root2 joint position | The root joint closest to the palm [15, 16,17,18,19]
+    ROOT3_POS = 0x05  # Root3 joint position | The root joint closest to the palm Not available
+    TIP_POS = 0x06  # Fingertip joint position | The root joint closest to the palm [20,21,22,23,24]
+
+    ROLL_SPEED = 0x09  # Roll joint speed | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+    YAW_SPEED = 0x0A  # Yaw joint speed | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+    ROOT1_SPEED = 0x0B  # Root1 joint speed | The root joint closest to the palm
+    ROOT2_SPEED = 0x0C  # Root2 joint speed | The root joint closest to the palm
+    ROOT3_SPEED = 0x0D  # Root3 joint speed | The root joint closest to the palm
+    TIP_SPEED = 0x0E  # Fingertip joint speed | The root joint closest to the palm
+
+    ROLL_TORQUE = 0x11  # Roll joint torque | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+    YAW_TORQUE = 0x12  # Yaw joint torque | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+    ROOT1_TORQUE = 0x13  # Root1 joint torque | The root joint closest to the palm
+    ROOT2_TORQUE = 0x14  # Root2 joint torque | The root joint closest to the palm
+    ROOT3_TORQUE = 0x15  # Root3 joint torque | The root joint closest to the palm
+    TIP_TORQUE = 0x16  # Fingertip joint torque | The root joint closest to the palm
+
+    ROLL_FAULT = 0x19  # Roll joint fault code | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+    YAW_FAULT = 0x1A  # Yaw joint fault code | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+    ROOT1_FAULT = 0x1B  # Root1 joint fault code | The root joint closest to the palm
+    ROOT2_FAULT = 0x1C  # Root2 joint fault code | The root joint closest to the palm
+    ROOT3_FAULT = 0x1D  # Root3 joint fault code | The root joint closest to the palm
+    TIP_FAULT = 0x1E  # Fingertip joint fault code | The root joint closest to the palm
+
+    ROLL_TEMPERATURE = 0x21  # Roll joint temperature | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+    YAW_TEMPERATURE = 0x22  # Yaw joint temperature | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+    ROOT1_TEMPERATURE = 0x23  # Root1 joint temperature | The root joint closest to the palm
+    ROOT2_TEMPERATURE = 0x24  # Root2 joint temperature | The root joint closest to the palm
+    ROOT3_TEMPERATURE = 0x25  # Root3 joint temperature | The root joint closest to the palm
+    TIP_TEMPERATURE = 0x26  # Fingertip joint temperature | The root joint closest to the palm
+    # Parallel command area
+
+    # Serial command area
+    THUMB_POS = 0x41  # Thumb joint position | Returns this type of data
+    INDEX_POS = 0x42  # Index finger joint position | Returns this type of data
+    MIDDLE_POS = 0x43  # Middle finger joint position | Returns this type of data
+    RING_POS = 0x44  # Ring finger joint position | Returns this type of data
+    LITTLE_POS = 0x45  # Little finger joint position | Returns this type of data
+
+    THUMB_SPEED = 0x49  # Thumb speed | Returns this type of data
+    INDEX_SPEED = 0x4A  # Index finger speed | Returns this type of data
+    MIDDLE_SPEED = 0x4B  # Middle finger speed | Returns this type of data
+    RING_SPEED = 0x4C  # Ring finger speed | Returns this type of data
+    LITTLE_SPEED = 0x4D  # Little finger speed | Returns this type of data
+
+    THUMB_TORQUE = 0x51  # Thumb torque | Returns this type of data
+    INDEX_TORQUE = 0x52  # Index finger torque | Returns this type of data
+    MIDDLE_TORQUE = 0x53  # Middle finger torque | Returns this type of data
+    RING_TORQUE = 0x54  # Ring finger torque | Returns this type of data
+    LITTLE_TORQUE = 0x55  # Little finger torque | Returns this type of data
+
+    THUMB_FAULT = 0x59  # Thumb fault code | Returns this type of data
+    INDEX_FAULT = 0x5A  # Index finger fault code | Returns this type of data
+    MIDDLE_FAULT = 0x5B  # Middle finger fault code | Returns this type of data
+    RING_FAULT = 0x5C  # Ring finger fault code | Returns this type of data
+    LITTLE_FAULT = 0x5D  # Little finger fault code | Returns this type of data
+
+    THUMB_TEMPERATURE = 0x61  # Thumb temperature | Returns this type of data
+    INDEX_TEMPERATURE = 0x62  # Index finger temperature | Returns this type of data
+    MIDDLE_TEMPERATURE = 0x63  # Middle finger temperature | Returns this type of data
+    RING_TEMPERATURE = 0x64  # Ring finger temperature | Returns this type of data
+    LITTLE_TEMPERATURE = 0x65  # Little finger temperature | Returns this type of data
+    # Serial command area
+
+    # Merged command area, non-essential single control data of the same finger is merged
+    FINGER_SPEED = 0x81  # Finger speed | Returns this type of data
+    FINGER_TORQUE = 0x82  # Torque | Returns this type of data
+    FINGER_FAULT = 0x83  # Finger fault code | Returns this type of data
+
+    # Fingertip sensor data group
+    HAND_NORMAL_FORCE = 0x90  # Normal force of five fingers
+    HAND_TANGENTIAL_FORCE = 0x91  # Tangential force of five fingers
+    HAND_TANGENTIAL_FORCE_DIR = 0x92  # Tangential direction of five fingers
+    HAND_APPROACH_INC = 0x93  # Proximity sensing of five fingers
+
+    THUMB_ALL_DATA = 0x98  # All data of thumb
+    INDEX_ALL_DATA = 0x99  # All data of index finger
+    MIDDLE_ALL_DATA = 0x9A  # All data of middle finger
+    RING_ALL_DATA = 0x9B  # All data of ring finger
+    LITTLE_ALL_DATA = 0x9C  # All data of little finger
+    # Action command ·ACTION
+    ACTION_PLAY = 0xA0  # Action
+
+    # Configuration command ·CONFIG
+    HAND_UID = 0xC0  # Device unique identifier
+    HAND_HARDWARE_VERSION = 0xC1  # Hardware version
+    HAND_SOFTWARE_VERSION = 0xC2  # Software version
+    HAND_COMM_ID = 0xC3  # Device id
+    HAND_FACTORY_RESET = 0xCE  # Restore factory settings
+    HAND_SAVE_PARAMETER = 0xCF  # Save parameters
+
+    WHOLE_FRAME = 0xF0  # Whole frame transmission | Returns one byte frame property + the entire structure for 485 and network transmission only
+
+class LinkerHandL25Can:
+    def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28,yaml=""):
+        self.can_id = can_id
+        self.can_channel = can_channel
+        self.baudrate = baudrate
+        self.open_can = OpenCan(load_yaml=yaml)
+
+        self.running = True
+        self.last_thumb_pos, self.last_index_pos,self.last_ring_pos,self.last_middle_pos, self.last_little_pos = None,None,None,None,None
+        self.x01, self.x02, self.x03, self.x04,self.x05,self.x06,self.x07, self.x08,self.x09,self.x0A,self.x0B,self.x0C,self.x0D,self.x0E,self.speed = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[]
+        self.last_root1,self.last_yaw,self.last_roll,self.last_root2,self.last_tip = None,None,None,None,None
+        # 速度
+        self.x49, self.x4a, self.x4b, self.x4c, self.x4d,self.xc1 = [],[],[],[],[],[]
+        self.x41,self.x42,self.x43,self.x44,self.x45 = [],[],[],[],[]
+        # 扭矩
+        self.x51, self.x52, self.x53, self.x54,self.x55 = [],[],[],[],[]
+        # 故障码
+        self.x59,self.x5a,self.x5b,self.x5c,self.x5d = [],[],[],[],[]
+        # 温度阈值
+        self.x61,self.x62,self.x63,self.x64,self.x65 = [],[],[],[],[]
+        # 压感
+        self.x90,self.x91,self.x92,self.x93 = [],[],[],[]
+        # 新压感
+        self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+        self.thumb_matrix = np.full((12, 6), -1)
+        self.index_matrix = np.full((12, 6), -1)
+        self.middle_matrix = np.full((12, 6), -1)
+        self.ring_matrix = np.full((12, 6), -1)
+        self.little_matrix = np.full((12, 6), -1)
+        self.matrix_map = {
+            0: 0,
+            16: 1,
+            32: 2,
+            48: 3,
+            64: 4,
+            80: 5,
+            96: 6,
+            112: 7,
+            128: 8,
+            144: 9,
+            160: 10,
+            176: 11,
+        }
+        # 根据操作系统初始化 CAN 总线
+        # try:
+        #     if sys.platform == "linux":
+        #         self.open_can.open_can(self.can_channel)
+        #         time.sleep(0.1)
+        #         self.bus = can.interface.Bus(
+        #             channel=can_channel, interface="socketcan", bitrate=baudrate, 
+        #             can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+        #         )
+        #     elif sys.platform == "win32":
+        #         self.bus = can.interface.Bus(
+        #             channel=can_channel, interface='pcan', bitrate=baudrate, 
+        #             can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+        #         )
+        #     else:
+        #         raise EnvironmentError("Unsupported platform for CAN interface")
+        # except:
+        #     print("Please insert CAN device")
+        self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate)
+        # 启动接收线程
+        self.receive_thread = threading.Thread(target=self.receive_response)
+        self.receive_thread.daemon = True
+        self.receive_thread.start()
+
+    def init_can_bus(self, channel, baudrate):
+        """
+        尝试按优先级连接 CAN 总线，并实现回退机制。
+        """
+        # --- 统一异常处理块开始 ---
+        try:
+            if sys.platform == "linux":
+                # Linux 优先级：1. socketcan
+                try:
+                    self.open_can.open_can(self.can_channel)
+                    # 尝试 socketcan
+                    bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+                    ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+                    raise # 重新抛出异常，让外层 try 捕获
+            elif sys.platform == "win32":
+                # Windows 优先级：1. pcan
+                try:
+                    bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+                # Windows 优先级：2. candle (回退方法)
+                try:
+                    bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+                    raise # 两个接口都失败，抛出异常
+            else:
+                raise EnvironmentError("Unsupported platform for CAN interface")
+        # --- 统一异常处理块结束 ---
+        except Exception as e:
+            # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+            ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+            # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+            raise
+
+    def send_command(self, frame_property, data_list):
+        """
+        Send command to CAN bus
+        :param frame_property: Data frame properties
+        :param data_list: Data payload
+        """
+        frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+        data = [frame_property_value] + [int(val) for val in data_list]
+        msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+        try:
+            self.bus.send(msg)
+            #print(f"Message sent: ID={hex(self.can_id)}, Data={data}")
+        except can.CanError as e:
+            print(f"Failed to send message: {e}")
+            self.open_can.open_can(self.can_channel)
+            time.sleep(1)
+            self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+            time.sleep(1)
+            if self.is_can:
+                self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+            else:
+                print("Reconnecting CAN devices ....")
+        time.sleep(0.001)
+
+    def receive_response(self):
+        """
+        Receive and process response messages from CAN bus
+        """
+        while self.running:
+            try:
+                msg = self.bus.recv(timeout=1.0)  # 阻塞接收，1 秒超时
+                if msg:
+                    self.process_response(msg)
+            except can.CanError as e:
+                print(f"Error receiving message: {e}")
+    
+
+    def set_joint_positions(self, joint_ranges):
+        if len(joint_ranges) == 25:
+            l25_pose = self.joint_map(joint_ranges)
+            # 使用列表推导式将列表每6个元素切成一个子数组
+            chunks = [l25_pose[i:i+6] for i in range(0, 30, 6)]
+            self.send_command(FrameProperty.THUMB_POS, chunks[0])
+            #time.sleep(0.001)
+            self.send_command(FrameProperty.INDEX_POS, chunks[1])
+            #time.sleep(0.001)
+            self.send_command(FrameProperty.MIDDLE_POS, chunks[2])
+            #time.sleep(0.001)
+            self.send_command(FrameProperty.RING_POS, chunks[3])
+            #time.sleep(0.001)
+            self.send_command(FrameProperty.LITTLE_POS, chunks[4])
+            #time.sleep(0.001)
+
+    def set_joint_positions_by_topic(self, joint_ranges):
+        if len(joint_ranges) == 25:
+            # Finger Joint Position Constants
+            #ROLL_POS = 0x01  # Roll joint position | Coordinate system based on finger base, rotation angle defined when finger is straight [10,11,12,13,14]
+            #YAW_POS = 0x02   # Yaw joint position | Coordinate system based on finger base, rotation angle defined when finger is straight [5,6,7,8,9]
+            #ROOT1_POS = 0x03 # Root1 joint position | Joint closest to the palm [0,1,2,3,4]
+            #ROOT2_POS = 0x04 # Root2 joint position | Joint closest to the palm [15,16,17,18,19]
+            #ROOT3_POS = 0x05 # Root3 joint position | Joint closest to the palm (currently unused)
+            #TIP_POS = 0x06   # Tip joint position | Joint closest to the palm [20,21,22,23,24]
+
+            # Finger joint names mapping (Chinese to English translation):
+            # ["Thumb root", "Index root", "Middle root", "Ring root", "Pinky root",
+            #  "Thumb yaw", "Index yaw", "Middle yaw", "Ring yaw", "Pinky yaw",
+            #  "Thumb roll", "Reserved", "Reserved", "Reserved", "Reserved",
+            #  "Thumb middle", "Index middle", "Middle middle", "Ring middle", "Pinky middle",
+            #  "Thumb tip", "Index tip", "Middle tip", "Ring tip", "Pinky tip"]
+
+            
+            l25_pose = self.slice_list(joint_ranges,5)
+            if self._list_d_value(self.last_root1, l25_pose[0]):
+                self.set_root1_positions(l25_pose[0])
+                self.last_root1 = l25_pose[0]
+            if self._list_d_value(self.last_yaw, l25_pose[1]):  
+                self.set_yaw_positions(l25_pose[1])
+                self.last_yaw = l25_pose[1]
+            if self._list_d_value(self.last_roll, l25_pose[2]):
+                self.set_roll_positions(l25_pose[2])
+                self.last_roll = l25_pose[2]
+            if self._list_d_value(self.last_root2, l25_pose[3]):
+                self.set_root2_positions(l25_pose[3])
+                self.last_root2 = l25_pose[3]
+            if self._list_d_value(self.last_tip, l25_pose[4]): 
+                self.set_tip_positions(l25_pose[4])
+                self.last_tip = l25_pose[4]
+            
+
+    def slice_list(self, input_list, slice_size):
+        """
+        Split a list into chunks of specified size.
+
+        Parameters:
+            input_list (list): The list to be chunked.
+            slice_size (int): Number of elements in each chunk.
+
+        Returns:
+            list of lists: The chunked list.
+        """
+        # Implementation using list comprehension
+        sliced_list = [input_list[i:i + slice_size] for i in range(0, len(input_list), slice_size)]
+        return sliced_list
+
+    def _list_d_value(self,list1, list2):
+        if list1 == None:
+            return True
+        for a, b in zip(list1, list2):
+            if abs(b - a) > 2:
+                return True
+                break
+        return False
+    # Set roll joint positions for all fingers
+    def set_roll_positions(self, joint_ranges):
+        self.send_command(FrameProperty.ROLL_POS, joint_ranges)
+    # Set yaw joint positions for all fingers
+    def set_yaw_positions(self, joint_ranges):
+        print(joint_ranges)
+        self.send_command(FrameProperty.YAW_POS, joint_ranges)
+    # Set base joint 1 positions for all fingers
+    def set_root1_positions(self, joint_ranges):
+        self.send_command(FrameProperty.ROOT1_POS, joint_ranges)
+    # Set base joint 2 positions for all fingers
+    def set_root2_positions(self, joint_ranges):
+        self.send_command(FrameProperty.ROOT2_POS, joint_ranges)
+    # Set base joint 3 positions for all fingers
+    def set_root3_positions(self, joint_ranges):
+        self.send_command(FrameProperty.ROOT3_POS, joint_ranges)
+    # Set fingertip joint positions for all fingers
+    def set_tip_positions(self, joint_ranges=[80]*5):
+        self.send_command(FrameProperty.TIP_POS, joint_ranges)
+    # Set thumb torque parameters
+    def set_thumb_torque(self, j=[]):
+        self.send_command(FrameProperty.THUMB_TORQUE, j)
+    # Set index finger torque
+    def set_index_torque(self, j=[]):
+        self.send_command(FrameProperty.INDEX_TORQUE, j)
+    # Set middle finger torque
+    def set_middle_torque(self, j=[]):
+        self.send_command(FrameProperty.MIDDLE_TORQUE, j)
+    # Set ring finger torque
+    def set_ring_torque(self, j=[]):
+        self.send_command(FrameProperty.RING_TORQUE, j)
+    # Set little finger torque
+    def set_little_torque(self, j=[]):
+        self.send_command(FrameProperty.LITTLE_TORQUE, j)
+
+    # Get thumb joint position
+    def get_thumb_positions(self,j=[0]):
+        self.send_command(FrameProperty.THUMB_POS, j)
+    # Get index finger joint positions
+    def get_index_positions(self, j=[0]):
+        self.send_command(FrameProperty.INDEX_POS,j)
+    # Get middle finger joint position
+    def get_middle_positions(self, j=[0]):
+        self.send_command(FrameProperty.MIDDLE_POS,j)
+    # Retrieve the position of the ring finger joint
+    def get_ring_positions(self, j=[0]):
+        self.send_command(FrameProperty.RING_POS,j)
+    # Retrieve the position of the little finger joint
+    def get_little_positions(self, j=[0]):
+        self.send_command(FrameProperty.LITTLE_POS, j)
+    # All fault codes of motors in the thumb
+    def get_thumbn_fault(self,j=[]):
+        self.send_command(FrameProperty.THUMB_FAULT,j)
+    # All motor fault codes for the index finger
+    def get_index_fault(self,j=[]):
+        self.send_command(FrameProperty.INDEX_FAULT,j)
+    # All motor fault codes for the middle finger
+    def get_middle_fault(self,j=[]):
+        self.send_command(FrameProperty.MIDDLE_FAULT,j)
+    # All motor fault codes for the ring finger
+    def get_ring_fault(self,j=[]):
+        self.send_command(FrameProperty.RING_FAULT,j)
+    # All motor fault codes for the little finger
+    def get_little_fault(self,j=[]):
+        self.send_command(FrameProperty.LITTLE_FAULT,j)
+    # Temperature threshold for the thumb motors
+    def get_thumb_threshold(self,j=[]):
+        self.send_command(FrameProperty.THUMB_TEMPERATURE, '')
+    # Temperature threshold for the index finger motors
+    def get_index_threshold(self,j=[]):
+        self.send_command(FrameProperty.INDEX_TEMPERATURE, j)
+    # Temperature threshold for the middle finger motors
+    def get_middle_threshold(self,j=[]):
+        self.send_command(FrameProperty.MIDDLE_TEMPERATURE, j)
+    # Temperature threshold for the ring finger motors
+    def get_ring_threshold(self,j=[]):
+        self.send_command(FrameProperty.RING_TEMPERATURE, j)
+    # Little finger temperature threshold
+    def get_little_threshold(self,j=[]):
+        self.send_command(FrameProperty.LITTLE_TEMPERATURE, j)
+
+
+    def set_disability_mode(self, j=[1,1,1,1,1]):
+        self.send_command(0x85,j)
+
+    def set_enable_mode(self, j=[00,00,00,00,00]):
+        self.send_command(0x85,j)
+    
+    # Set torque for all fingers
+    def set_torque(self,torque=[250]*5):
+        t = torque[0]
+        i = torque[1]
+        m = torque[2]
+        r = torque[3]
+        l = torque[4]
+        self.set_thumb_torque(j=[t]*5)
+        self.set_index_torque(j=[i]*5)
+        self.set_middle_torque(j=[m]*5)
+        self.set_ring_torque(j=[r]*5)
+        self.set_little_torque(j=[l]*5)
+    
+    def set_speed(self, speed):
+        self.speed = speed
+        if len(speed) < 25:
+            thumb_speed = [self.speed[0]]*5
+            index_speed = [self.speed[1]]*5
+            middle_speed = [self.speed[2]]*5
+            ring_speed = [self.speed[3]]*5
+            little_speed = [self.speed[4]]*5
+        else:
+            thumb_speed = [self.speed[0],self.speed[1],self.speed[2],self.speed[3],self.speed[4]]
+            index_speed = [self.speed[5],self.speed[6],self.speed[7],self.speed[8],self.speed[9]]
+            middle_speed = [self.speed[10],self.speed[11],self.speed[12],self.speed[13],self.speed[14]]
+            ring_speed = [self.speed[15],self.speed[16],self.speed[17],self.speed[18],self.speed[19]]
+            little_speed = [self.speed[20],self.speed[21],self.speed[22],self.speed[23],self.speed[24]]
+        self.send_command(FrameProperty.THUMB_SPEED, thumb_speed)
+        self.send_command(FrameProperty.INDEX_SPEED, index_speed)
+        self.send_command(FrameProperty.MIDDLE_SPEED, middle_speed)
+        self.send_command(FrameProperty.RING_SPEED, ring_speed)
+        self.send_command(FrameProperty.LITTLE_SPEED, little_speed)
+        
+    def set_finger_torque(self, torque):
+        self.send_command(0x42, torque)
+
+    def request_device_info(self):
+        self.send_command(0xC0, [0])
+        self.send_command(0xC1, [0])
+        self.send_command(0xC2, [0])
+
+    def save_parameters(self):
+        self.send_command(0xCF, [])
+    def process_response(self, msg):
+        if msg.arbitration_id == self.can_id:
+            frame_type = msg.data[0]
+            response_data = msg.data[1:]
+            if len(list(response_data)) == 0:
+                return
+            if frame_type == 0x01:
+                self.x01 = list(response_data)
+            elif frame_type == 0x02:
+                self.x02 = list(response_data)
+            elif frame_type == 0x03:
+                self.x03 = list(response_data)
+            elif frame_type == 0x04:
+                self.x04 = list(response_data)
+            elif frame_type == 0x05:
+                self.x05 = list(response_data)
+            elif frame_type == 0x06:
+                self.x06 = list(response_data)
+            elif frame_type == 0xC0:
+                print(f"Device ID info: {response_data}")
+                if self.can_id == 0x28:
+                    self.right_hand_info = response_data
+                elif self.can_id == 0x27:
+                    self.left_hand_info = response_data
+            elif frame_type == 0x08:
+                self.x08 = list(response_data)
+            elif frame_type == 0x09:
+                self.x09 = list(response_data)
+            elif frame_type == 0x0A:
+                self.x0A = list(response_data)
+            elif frame_type == 0x0B:
+                self.x0B = list(response_data)
+            elif frame_type == 0x0C:
+                self.x0C = list(response_data)
+            elif frame_type == 0x0D:
+                self.x0D = list(response_data)
+            elif frame_type == 0x22:
+                d = list(response_data)
+                self.tangential_force_dir = [float(i) for i in d]
+            elif frame_type == 0x23:
+                d = list(response_data)
+                self.approach_inc = [float(i) for i in d]
+            elif frame_type == 0x41: 
+                self.x41 = list(response_data)
+            elif frame_type == 0x42:
+
+                self.x42 = list(response_data)
+            elif frame_type == 0x43: 
+                self.x43 = list(response_data)
+            elif frame_type == 0x44: 
+                
+                self.x44 = list(response_data)
+            elif frame_type == 0x45: 
+                self.x45 = list(response_data)
+            elif frame_type == 0x49: 
+                self.x49 = list(response_data)
+            elif frame_type == 0x4a:
+                self.x4a = list(response_data)
+            elif frame_type == 0x4b:
+                self.x4b = list(response_data)
+            elif frame_type == 0x4c: 
+                self.x4c = list(response_data)
+            elif frame_type == 0x4d:
+                self.x4d = list(response_data)
+            elif frame_type == 0xc1:
+                self.xc1 = list(response_data)
+            elif frame_type == 0x51: 
+                self.x51 = list(response_data)
+            elif frame_type == 0x52: 
+                self.x52 = list(response_data)
+            elif frame_type == 0x53: 
+                self.x53 = list(response_data)
+            elif frame_type == 0x54: 
+                self.x54 = list(response_data)
+            elif frame_type == 0x55: 
+                self.x55 = list(response_data)
+            elif frame_type == 0x59:
+                self.x59 = list(response_data)
+            elif frame_type == 0x5a:
+                self.x5a = list(response_data)
+            elif frame_type == 0x5b:
+                self.x5b = list(response_data)
+            elif frame_type == 0x5c:
+                self.x5c = list(response_data)
+            elif frame_type == 0x5d:
+                self.x5d = list(response_data)
+            elif frame_type == 0x61:
+                self.x61 = list(response_data)
+            elif frame_type == 0x62:
+                self.x62 = list(response_data)
+            elif frame_type == 0x63:
+                self.x63 = list(response_data)
+            elif frame_type == 0x64:
+                self.x64 = list(response_data)
+            elif frame_type == 0x65:
+                self.x65 = list(response_data)
+            elif frame_type == 0x90:
+                self.x90 = list(response_data)
+            elif frame_type == 0x91:
+                self.x91 = list(response_data)
+            elif frame_type == 0x92:
+                self.x92 = list(response_data)
+            elif frame_type == 0x93:
+                self.x93 = list(response_data)
+            elif frame_type == 0xb0:
+                self.xb0 = list(response_data)
+            elif frame_type == 0xb1:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb1 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.thumb_matrix[index] = d[1:] 
+            elif frame_type == 0xb2:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb2 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.index_matrix[index] = d[1:]  
+            elif frame_type == 0xb3:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb3 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.middle_matrix[index] = d[1:]  
+            elif frame_type == 0xb4:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb4 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.ring_matrix[index] = d[1:]
+            elif frame_type == 0xb5:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb5 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.little_matrix[index] = d[1:]
+
+
+    def joint_map(self, pose):
+        l25_pose = [0.0] * 30
+
+        # 映射表，通过字典简化映射关系
+        mapping = {
+            0: 10,  1: 5,   2: 0,   3: 15,  4: None,  5: 20,
+            6: None, 7: 6,   8: 1,   9: 16,  10: None, 11: 21,
+            12: None, 13: 7, 14: 2,  15: 17, 16: None, 17: 22,
+            18: None, 19: 8,  20: 3,   21: 18, 22: None, 23: 23,
+            24: None, 25: 9,  26: 4,   27: 19, 28: None, 29: 24
+        }
+
+        # 遍历映射字典，进行值的映射
+        for l25_idx, pose_idx in mapping.items():
+            if pose_idx is not None:
+                l25_pose[l25_idx] = pose[pose_idx]
+
+        return l25_pose
+
+
+    def state_to_cmd(self, l25_state):
+        
+        pose = [0.0] * 25
+        mapping = {
+            0: 10,  1: 5,   2: 0,   3: 15,  5: 20,  7: 6,
+            8: 1,   9: 16,  11: 21, 13:7, 14: 2,  15: 17, 17: 22,
+            19: 8,  20: 3,  21: 18, 23: 23, 25: 9,   26: 4,
+            27: 19, 29: 24
+        }
+        # 遍历映射字典，更新pose的值
+        for l25_idx, pose_idx in mapping.items():
+            pose[pose_idx] = l25_state[l25_idx]
+        return pose
+    def action_play(self):
+        self.send_command(0xA0,[])
+
+    def get_current_status(self, j=''):
+        self.send_command(FrameProperty.THUMB_POS, j)
+        #time.sleep(0.001)
+        self.send_command(FrameProperty.INDEX_POS,j)
+        #time.sleep(0.001)
+        self.send_command(FrameProperty.MIDDLE_POS,j)
+        #time.sleep(0.001)
+        self.send_command(FrameProperty.RING_POS,j)
+        #time.sleep(0.001)
+        self.send_command(FrameProperty.LITTLE_POS, j)
+        #time.sleep(0.001)
+        state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+        if len(state) == 30:
+            l25_state = self.state_to_cmd(l25_state=state)
+            return l25_state
+
+    def get_current_pub_status(self):
+        state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+        if len(state) == 30:
+            l25_state = self.state_to_cmd(l25_state=state)
+            return l25_state
+        
+    def get_current_state_topic(self):
+        self.send_command(0x01,[])
+        #time.sleep(0.001)
+        self.send_command(0x02,[])
+       # time.sleep(0.001)
+        self.send_command(0x03,[])
+        #time.sleep(0.001)
+        self.send_command(0x04,[])
+        #time.sleep(0.001)
+        self.send_command(0x06,[])
+        #time.sleep(0.001)
+        state = self.x03+self.x02+self.x01+self.x04+self.x06
+        return state
+    def get_speed(self,j=''):
+        self.send_command(FrameProperty.THUMB_SPEED, j)
+        #time.sleep(0.01)
+        self.send_command(FrameProperty.INDEX_SPEED, j)
+        #time.sleep(0.01)
+        self.send_command(FrameProperty.MIDDLE_SPEED, j)
+        #time.sleep(0.01)
+        self.send_command(FrameProperty.RING_SPEED, j)
+        #time.sleep(0.01)
+        self.send_command(FrameProperty.LITTLE_SPEED, j)
+        #time.sleep(0.01)
+        speed = self.x49+ self.x4a+ self.x4b+ self.x4c+ self.x4d
+        if len(speed) == 30:
+            l25_speed = self.state_to_cmd(l25_state=speed)
+            return l25_speed
+    
+    def get_finger_torque(self):
+        self.send_command(FrameProperty.THUMB_TORQUE,[])
+        self.send_command(FrameProperty.INDEX_TORQUE,[])
+        self.send_command(FrameProperty.MIDDLE_TORQUE,[])
+        self.send_command(FrameProperty.RING_TORQUE,[])
+        self.send_command(FrameProperty.LITTLE_TORQUE,[])
+        return self.x51+self.x52+self.x53+self.x54+self.x55
+    
+    def get_torque(self):
+        return self.get_finger_torque()
+    def get_fault(self):
+        self.get_thumbn_fault()
+        #time.sleep(0.001)
+        self.get_index_fault()
+        #time.sleep(0.001)
+        self.get_middle_fault()
+        #time.sleep(0.001)
+        self.get_ring_fault()
+        #time.sleep(0.001)
+        self.get_little_fault()
+        #time.sleep(0.001)
+        return [self.x59]+[self.x5a]+[self.x5b]+[self.x5c]+[self.x5d]
+    def get_threshold(self):
+        self.get_thumb_threshold()
+        self.get_index_threshold()
+        self.get_middle_threshold()
+        self.get_ring_threshold()
+        self.get_little_threshold()
+        return [self.x61]+[self.x62]+[self.x63]+[self.x64]+[self.x65]
+    def get_version(self):
+        if self.xc1 == []:
+            self.send_command(FrameProperty.HAND_HARDWARE_VERSION,[])
+        return self.xc1
+    def get_normal_force(self):
+        self.send_command(FrameProperty.HAND_NORMAL_FORCE,[])
+        return self.x90
+    def get_tangential_force(self):
+        self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE,[])
+        return self.x91
+    def get_tangential_force_dir(self):
+        self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE_DIR,[])
+        return self.x92
+    def get_approach_inc(self):
+        self.send_command(FrameProperty.HAND_APPROACH_INC,[])
+        return self.x93
+    def get_force(self):
+        '''获取压感数据'''
+        return [self.x90,self.x91 , self.x92 , self.x93]
+    
+    def get_matrix_touch(self):
+        self.send_command(0xb1,[0xc6])
+        time.sleep(0.03)
+        self.send_command(0xb2,[0xc6])
+        time.sleep(0.03)
+        self.send_command(0xb3,[0xc6])
+        time.sleep(0.03)
+        self.send_command(0xb4,[0xc6])
+        time.sleep(0.03)
+        self.send_command(0xb5,[0xc6])
+        time.sleep(0.03)
+        return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+    
+    def get_touch_type(self):
+        '''Get touch type'''
+        self.send_command(0xb1,[])
+        time.sleep(0.03)
+        if len(self.xb1) == 2:
+            return 2
+        else:
+            return -1
+    
+    def get_touch(self):
+        '''Get touch data (not supported yet)'''
+        return [-1] * 6
+
+
+    def get_current(self):
+        return [0] * 21
+    def get_temperature(self):
+        self.get_thumb_threshold()
+        self.get_index_threshold()
+        self.get_middle_threshold()
+        self.get_ring_threshold()
+        self.get_little_threshold()
+        return [self.x61]+[self.x62]+[self.x63]+[self.x64]+[self.x65]
+    
+    def get_finger_order(self):
+        return ["Thumb root", "Index root", "Middle root", "Ring root", "Little root",
+            "Thumb abduction", "Index abduction", "Middle abduction", "Ring abduction", "Little abduction",
+            "Thumb roll", "Reserved", "Reserved", "Reserved", "Reserved",
+            "Thumb middle", "Index middle", "Middle middle", "Ring middle", "Little middle",
+            "Thumb tip", "Index tip", "Middle tip", "Ring tip", "Little tip"]
+    
+    def close_can_interface(self):
+        if self.bus:
+            self.bus.shutdown()
+    def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+        """L25 暂不支持清除故障码"""
+        pass
+    '''
+    这个方法只用于展示数据关系映射，使用的话最好使用上面的方法
+    '''
+    def joint_map_2(self, pose):
+        l25_pose = [0.0]*30 #L25 CAN默认接收30个数据 pose控制L25发送的指令数据默认25个，这里进行映射
+        '''
+        需要进行映射
+        # L25 CAN数据格式
+        #["拇指横摆0-10", "拇指侧摆1-5", "拇指根部2-0", "拇指中部3-15", "预留4-", "拇指指尖5-20", "预留6-", "食指侧摆7-6", "食指根部8-1", "食指中部9-16", "预留10-", "食指指尖11-21", "预留12-", "预留13-", "中指根部14-2", "中指中部15-17", "预留16-", "中指指尖17-22", "预留18-", "无名指侧摆19-8", "无名指根部20-3", "无名指中部21-18", "预留22-", "无名指指尖23-23", "预留24-", "小指侧摆25-9", "小指根部26-4", "小指中部27-19", "预留28-", "小指指尖29-24"]
+        # CMD 接收到的数据格式
+        #["拇指根部0", "食指根部1", "中指根部2", "无名指根部3","小指根部4","拇指侧摆5","食指侧摆6","中指侧摆","无名指侧摆8","小指侧摆9","拇指横摆10","预留","预留","预留","预留","拇指中部15","食指中部16","中指中部17","无名指中部18","小指中部19","拇指指尖20","食指指尖21","中指指尖22","无名指指尖23","小指指尖24"]
+        '''
+        l25_pose[0] = pose[10]
+        l25_pose[1] = pose[5]
+        l25_pose[2] = pose[0]
+        l25_pose[3] = pose[15]
+        l25_pose[4] = 0.0
+        l25_pose[5] = pose[20]
+        l25_pose[6] = 0.0
+        l25_pose[7] = pose[6]
+        l25_pose[8] = pose[1]
+        l25_pose[9] = pose[16]
+        l25_pose[10] = 0.0
+        l25_pose[11] = pose[21]
+        l25_pose[12] = 0.0
+        l25_pose[13] = 0.0
+        l25_pose[14] = pose[2]
+        l25_pose[15] = pose[17]
+        l25_pose[16] = 0.0
+        l25_pose[17] = pose[22]
+        l25_pose[18] = 0.0
+        l25_pose[19] = pose[8]
+        l25_pose[20] = pose[3]
+        l25_pose[21] = pose[18]
+        l25_pose[22] = 0.0
+        l25_pose[23] = pose[23]
+        l25_pose[24] = 0.0
+        l25_pose[25] = pose[9]
+        l25_pose[26] = pose[4]
+        l25_pose[27] = pose[19]
+        l25_pose[28] = 0.0
+        l25_pose[29] = pose[24]
+        return l25_pose
+    
+    def get_serial_number(self):
+        return [0] * 6
+    def show_fun_table(self):
+        pass

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py

@@ -0,0 +1,426 @@
+import can
+import time, sys
+import threading
+import numpy as np
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+
+class LinkerHandL6Can:
+    def __init__(self, can_id, can_channel='can0', baudrate=1000000,yaml=""):
+        self.can_id = can_id
+        self.can_channel = can_channel
+        self.baudrate = baudrate
+        self.open_can = OpenCan(load_yaml=yaml)
+
+        self.x01 = [0] * 6 # 关节位置
+        self.x02 = [-1] * 6 # 转矩限制
+        self.x05 = [0] * 6 # 速度
+        self.x07 = [-1] * 6 # 加速度
+        self.x33 = [0] * 6 # 温度
+        self.x35 = [0] * 6 # 关节错误码
+        self.x36 = [-1] * 6 # 电流
+        self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+
+        self.thumb_matrix = np.full((12, 6), -1)
+        self.index_matrix = np.full((12, 6), -1)
+        self.middle_matrix = np.full((12, 6), -1)
+        self.ring_matrix = np.full((12, 6), -1)
+        self.little_matrix = np.full((12, 6), -1)
+        self.matrix_map = {
+            0: 0,
+            16: 1,
+            32: 2,
+            48: 3,
+            64: 4,
+            80: 5,
+            96: 6,
+            112: 7,
+            128: 8,
+            144: 9,
+            160: 10,
+            176: 11,
+        }
+        self.serial_number = []
+        self.serial_number_map = {
+            0: 0,
+            1: 1,
+            2: 2,
+            3: 3,
+        }
+        # Fault codes
+        
+        self.joint_angles = [0] * 6
+        self.pressures = [200] * 6  # Default torque 200
+        self.bus = self.init_can_bus(can_channel, baudrate)
+        self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)]
+        self.is_lock = False
+        self.version = None
+        # Start the receiving thread
+        self.running = True
+        self.receive_thread = threading.Thread(target=self.receive_response)
+        self.receive_thread.daemon = True
+        self.receive_thread.start()
+
+    def init_can_bus(self, channel, baudrate):
+        """
+        尝试按优先级连接 CAN 总线，并实现回退机制。
+        """
+        # --- 统一异常处理块开始 ---
+        try:
+            if sys.platform == "linux":
+                # Linux 优先级：1. socketcan
+                try:
+                    self.open_can.open_can(self.can_channel)
+                    # 尝试 socketcan
+                    bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+                    ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+                    raise # 重新抛出异常，让外层 try 捕获
+            elif sys.platform == "win32":
+                # Windows 优先级：1. pcan
+                try:
+                    bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+                # Windows 优先级：2. candle (回退方法)
+                try:
+                    bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+                    raise # 两个接口都失败，抛出异常
+            else:
+                raise EnvironmentError("Unsupported platform for CAN interface")
+        # --- 统一异常处理块结束 ---
+        except Exception as e:
+            # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+            ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+            # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+            raise
+
+    def send_frame(self, frame_property, data_list,sleep=0.003):
+        """Send a single CAN frame with specified properties and data."""
+        frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+        data = [frame_property_value] + [int(val) for val in data_list]
+        msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+        try:
+            self.bus.send(msg)
+        except can.CanError as e:
+            print(f"Failed to send message: {e}")
+            self.open_can.open_can(self.can_channel)
+            time.sleep(1)
+            self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+            time.sleep(1)
+            if self.is_can:
+                self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+            else:
+                print("Reconnecting CAN devices ....")
+        time.sleep(sleep)
+
+    def set_joint_positions(self, joint_angles):
+        """Set the positions of 10 joints (joint_angles: list of 10 values)."""
+        if len(joint_angles) > 6:
+            self.joint_angles = joint_angles[:6]
+        else:
+            self.joint_angles = joint_angles
+        # Send angle control in frames
+        self.send_frame(0x01, self.joint_angles, sleep=0.003)
+
+    def set_max_torque_limits(self, pressures, type="get"):
+        """Set maximum torque limits."""
+        if type == "get":
+            self.pressures = [0.0]
+        else:
+            self.pressures = pressures[:6]
+
+    def set_torque(self, torque=[180] * 6):
+        """Set L6 maximum torque limits."""
+        if len(torque) != 6:
+            raise ValueError("Torque list must have 6 elements.")
+            return
+        self.send_frame(0x02, torque)
+
+    def set_speed(self, speed=[180] * 6):
+        """Set L6 speed."""
+        if len(speed) != 6:
+            raise ValueError("Speed list must have 6 elements.")
+            return
+        self.x05 = speed
+        for i in range(2):
+            time.sleep(0.001)
+            self.send_frame(0x05, speed)
+
+    ''' -------------------Pressure Sensors---------------------- '''
+    def get_normal_force(self):
+        self.send_frame(0x20, [],sleep=0.01)
+
+    def get_tangential_force(self):
+        self.send_frame(0x21, [],sleep=0.01)
+
+    def get_tangential_force_dir(self):
+        self.send_frame(0x22, [],sleep=0.01)
+
+    def get_approach_inc(self):
+        self.send_frame(0x23, [],sleep=0.01)
+
+    ''' -------------------Motor Temperature---------------------- '''
+    def get_motor_temperature(self):
+        self.send_frame(0x33, [])
+
+    # Motor fault codes
+    def get_motor_fault_code(self):
+        self.send_frame(0x35, [])
+
+    def receive_response(self):
+        """Receive CAN responses and process them."""
+        while self.running:
+            try:
+                msg = self.bus.recv(timeout=1.0)
+                if msg:
+                    self.process_response(msg)
+            except can.CanError as e:
+                print(f"Error receiving CAN message: {e}")
+
+    def process_response(self, msg):
+        """Process received CAN messages."""
+        #if msg.arbitration_id == self.can_id:
+        if msg.arbitration_id in (self.can_id, self.can_id + 8):
+            try:
+                frame_type = msg.data[0]
+                response_data = msg.data[1:]
+                if len(list(response_data)) == 0:
+                    return
+            except:
+                return
+            if frame_type == 0x01:   # 0x01
+                self.x01 = list(response_data)
+            elif frame_type == 0x02:    # 0x02
+                self.x02 = list(response_data)
+            elif frame_type == 0x05: # Set speed
+                self.x05 = list(response_data)
+            elif frame_type == 0x20:
+                d = list(response_data)
+                self.normal_force = [float(i) for i in d]
+            elif frame_type == 0x21:
+                d = list(response_data)
+                self.tangential_force = [float(i) for i in d]
+            elif frame_type == 0x22:
+                d = list(response_data)
+                self.tangential_force_dir = [float(i) for i in d]
+            elif frame_type == 0x23:
+                d = list(response_data)
+                self.approach_inc = [float(i) for i in d]
+            elif frame_type == 0x33: # L6 temperature
+                self.x33 = list(response_data)
+            elif frame_type == 0x35: # L6 fault codes
+                self.x35 = list(response_data)
+            elif frame_type == 0x36: # L6 电流
+                self.x36 = list(response_data)
+            elif frame_type == 0xb0:
+                self.xb0 = list(response_data)
+            elif frame_type == 0xb1:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb1 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.thumb_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb2:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb2 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.index_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb3:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb3 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.middle_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb4:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb4 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.ring_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb5:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb5 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.little_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0x64: # L6 version number
+                self.version = list(response_data)
+            elif frame_type == 0xC2: # L6 version number
+                self.version = list(response_data)
+            elif frame_type == 0xC0:
+                d = list(response_data)
+                index = self.serial_number_map.get(d[0])
+                if index is not None:
+                    self.serial_number=self.serial_number + d[1:]
+                else:
+                    self.serial_number=self.serial_number + [-1] * 6
+                
+
+
+    def get_version(self):
+        self.send_frame(0x64, [],sleep=0.1)
+        time.sleep(0.1)
+        if self.version is None:
+            self.send_frame(0xC2, [],sleep=0.1)
+            time.sleep(0.1)
+        return self.version
+
+    def get_current_status(self):
+        self.send_frame(0x01, [],sleep=0.005)
+        return self.x01
+        
+    def get_current_pub_status(self):
+        return self.x01
+
+    def get_speed(self):
+        #self.send_frame(0x05, [],sleep=0.003)
+        #print("L6暂不支持读取实时速度")
+        return [0] * 6
+
+    def get_current(self):
+        '''Not supported yet.'''
+        self.send_frame(0x36, [],sleep=0.005)
+        return self.x36
+
+
+
+    def get_torque(self):
+        '''Not supported yet.'''
+        self.send_frame(0x2, [],sleep=0.01)
+        return self.x02
+
+    def get_touch_type(self):
+        '''Get touch type'''
+        self.send_frame(0xb1,[])
+        t = []
+        for i in range(3):
+            t = self.xb1
+            time.sleep(0.01)
+        if len(t) == 2:
+            return 2
+        else:
+            self.send_frame(0x20,[],sleep=0.03)
+            time.sleep(0.01)
+            if self.normal_force[0] == -1:
+                return -1
+            else:
+                return 1
+
+
+    def get_touch(self):
+        '''Get touch data'''
+        self.send_frame(0xb1,[],sleep=0.03)
+        self.send_frame(0xb2,[],sleep=0.03)
+        self.send_frame(0xb3,[],sleep=0.03)
+        self.send_frame(0xb4,[],sleep=0.03)
+        self.send_frame(0xb5,[],sleep=0.03)
+        return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+    
+    def get_matrix_touch(self):
+        self.send_frame(0xb1,[0xc6],sleep=0.01)
+        self.send_frame(0xb2,[0xc6],sleep=0.01)
+        self.send_frame(0xb3,[0xc6],sleep=0.01)
+        self.send_frame(0xb4,[0xc6],sleep=0.01)
+        self.send_frame(0xb5,[0xc6],sleep=0.01)
+
+        return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+    
+    def get_matrix_touch_v2(self):
+        self.send_frame(0xb1,[0xc6],sleep=0.009)
+        self.send_frame(0xb2,[0xc6],sleep=0.009)
+        self.send_frame(0xb3,[0xc6],sleep=0.009)
+        self.send_frame(0xb4,[0xc6],sleep=0.009)
+        self.send_frame(0xb5,[0xc6],sleep=0.009)
+        return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+    
+    def get_thumb_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb1,[0xc6],sleep=sleep_time)
+        return self.thumb_matrix
+    
+    def get_index_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb2,[0xc6],sleep=sleep_time)
+        return self.index_matrix
+    
+    def get_middle_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb3,[0xc6],sleep=sleep_time)
+        return self.middle_matrix
+    
+    def get_ring_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb4,[0xc6],sleep=sleep_time)
+        return self.ring_matrix
+    
+    def get_little_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb5,[0xc6],sleep=sleep_time)
+        return self.little_matrix
+
+    def get_force(self):
+        '''Get pressure.'''
+        return [self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc]
+
+    def get_temperature(self):
+        '''Get temperature.'''
+        self.get_motor_temperature()
+        return self.x33
+
+    def get_fault(self):
+        '''Get faults.'''
+        self.get_motor_fault_code()
+        return self.x35
+    
+    def get_finger_order(self):
+        return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+    def show_fun_table(self):
+        pass
+    
+    def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+        """O6 暂不支持清除故障码"""
+        pass
+
+    def get_serial_number(self):
+        try:
+            self.send_frame(0xC0,[],sleep=0.005)
+            # 1. 使用 bytes() 函数将整数列表转换为字节对象
+            #    bytes() 接收一个由 0-255 之间的整数组成的列表。
+            byte_data = bytes(self.serial_number)
+            # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+            result_string = byte_data.decode('ascii')
+            if result_string == "":
+                return "-1"
+            else:
+                # print(f"原始 ASCII 码列表: {self.serial_number}")
+                # print(f"解码后的字符串: {result_string}")
+                return result_string
+        except:
+            return "-1"
+
+    def close_can_interface(self):
+        """Stop the CAN communication."""
+        self.running = False
+        if self.receive_thread.is_alive():
+            self.receive_thread.join()
+        if self.bus:
+            self.bus.shutdown()

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l7_can.py

@@ -0,0 +1,419 @@
+import can
+import time, sys
+import threading
+import numpy as np
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+
+class LinkerHandL7Can:
+    def __init__(self, can_id, can_channel='can0', baudrate=1000000,yaml=""):
+        self.can_id = can_id
+        self.can_channel = can_channel
+        self.baudrate = baudrate
+        self.open_can = OpenCan(load_yaml=yaml)
+
+        self.x01 = [0] * 7
+        self.x02 = [-1] * 7
+        self.x05 = [0] * 7
+        self.x33 = [0] * 7
+        self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+        self.thumb_matrix = np.full((12, 6), -1)
+        self.index_matrix = np.full((12, 6), -1)
+        self.middle_matrix = np.full((12, 6), -1)
+        self.ring_matrix = np.full((12, 6), -1)
+        self.little_matrix = np.full((12, 6), -1)
+        self.matrix_map = {
+            0: 0,
+            16: 1,
+            32: 2,
+            48: 3,
+            64: 4,
+            80: 5,
+            96: 6,
+            112: 7,
+            128: 8,
+            144: 9,
+            160: 10,
+            176: 11,
+        }
+        self.serial_number = []
+        self.serial_number_map = {
+            0: 0,
+            1: 1,
+            2: 2,
+            3: 3,
+        }
+        # Fault codes
+        self.x35 = [0] * 7, [0] * 7
+        self.joint_angles = [0] * 10
+        self.pressures = [200] * 7  # Default torque 200
+        self.bus = self.init_can_bus(can_channel, baudrate)
+        self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 7 for _ in range(4)]
+        self.is_lock = False
+        self.version = None
+        # Start the receiving thread
+        self.running = True
+        self.receive_thread = threading.Thread(target=self.receive_response)
+        self.receive_thread.daemon = True
+        self.receive_thread.start()
+
+    def init_can_bus(self, channel, baudrate):
+        """
+        尝试按优先级连接 CAN 总线，并实现回退机制。
+        """
+        # --- 统一异常处理块开始 ---
+        try:
+            if sys.platform == "linux":
+                # Linux 优先级：1. socketcan
+                try:
+                    self.open_can.open_can(self.can_channel)
+                    # 尝试 socketcan
+                    bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+                    ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+                    raise # 重新抛出异常，让外层 try 捕获
+            elif sys.platform == "win32":
+                # Windows 优先级：1. pcan
+                try:
+                    bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+                # Windows 优先级：2. candle (回退方法)
+                try:
+                    bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+                    raise # 两个接口都失败，抛出异常
+            else:
+                raise EnvironmentError("Unsupported platform for CAN interface")
+        # --- 统一异常处理块结束 ---
+        except Exception as e:
+            # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+            ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+            # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+            raise
+
+    def send_frame(self, frame_property, data_list,sleep=0.005):
+        """Send a single CAN frame with specified properties and data."""
+        frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+        data = [frame_property_value] + [int(val) for val in data_list]
+        msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+        try:
+            self.bus.send(msg)
+        except can.CanError as e:
+            print(f"Failed to send message: {e}")
+            self.open_can.open_can(self.can_channel)
+            time.sleep(1)
+            self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+            time.sleep(1)
+            if self.is_can:
+                self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+            else:
+                print("Reconnecting CAN devices ....")
+        time.sleep(sleep)
+
+    def set_joint_positions(self, joint_angles):
+        """Set the positions of 10 joints (joint_angles: list of 10 values)."""
+        self.is_lock = True
+        if len(joint_angles) > 7:
+            self.joint_angles = joint_angles[:7]
+        else:
+            self.joint_angles = joint_angles
+        # Send angle control in frames
+        self.send_frame(0x01, self.joint_angles, sleep=0.003)
+        self.is_lock = False
+
+    def set_max_torque_limits(self, pressures, type="get"):
+        """Set maximum torque limits."""
+        if type == "get":
+            self.pressures = [0.0]
+        else:
+            self.pressures = pressures[:7]
+
+    def set_torque(self, torque=[180] * 7):
+        """Set L7 maximum torque limits."""
+        if len(torque) != 7:
+            raise ValueError("Torque list must have 7 elements.")
+            return
+        self.send_frame(0x02, torque)
+
+    def set_speed(self, speed=[180] * 7):
+        """Set L7 speed."""
+        if len(speed) != 7:
+            raise ValueError("Speed list must have 7 elements.")
+            return
+        self.x05 = speed
+        for i in range(2):
+            time.sleep(0.001)
+            self.send_frame(0x05, speed)
+
+    ''' -------------------Pressure Sensors---------------------- '''
+    def get_normal_force(self):
+        self.send_frame(0x20, [],sleep=0.004)
+
+    def get_tangential_force(self):
+        self.send_frame(0x21, [],sleep=0.004)
+
+    def get_tangential_force_dir(self):
+        self.send_frame(0x22, [],sleep=0.004)
+
+    def get_approach_inc(self):
+        self.send_frame(0x23, [],sleep=0.004)
+
+    ''' -------------------Motor Temperature---------------------- '''
+    def get_motor_temperature(self):
+        self.send_frame(0x33, [])
+
+    # Motor fault codes
+    def get_motor_fault_code(self):
+        self.send_frame(0x35, [])
+
+    def receive_response(self):
+        """Receive CAN responses and process them."""
+        while self.running:
+            try:
+                msg = self.bus.recv(timeout=1.0)
+                if msg:
+                    self.process_response(msg)
+            except can.CanError as e:
+                print(f"Error receiving CAN message: {e}")
+
+    def process_response(self, msg):
+        """Process received CAN messages."""
+        if msg.arbitration_id == self.can_id:
+            frame_type = msg.data[0]
+            response_data = msg.data[1:]
+            if len(list(response_data)) == 0:
+                return
+            if frame_type == 0x01:   # 0x01
+                self.x01 = list(response_data)
+            elif frame_type == 0x02:    # 0x02
+                self.x02 = list(response_data)
+            elif frame_type == 0x05: # Set speed
+                self.x05 = list(response_data)
+            elif frame_type == 0x20:
+                d = list(response_data)
+                self.normal_force = [float(i) for i in d]
+            elif frame_type == 0x21:
+                d = list(response_data)
+                self.tangential_force = [float(i) for i in d]
+            elif frame_type == 0x22:
+                d = list(response_data)
+                self.tangential_force_dir = [float(i) for i in d]
+            elif frame_type == 0x23:
+                d = list(response_data)
+                self.approach_inc = [float(i) for i in d]
+            elif frame_type == 0x33: # L7 temperature
+                self.x33 = list(response_data)
+            elif frame_type == 0x35: # L7 fault codes
+                self.x35 = list(response_data)
+            elif frame_type == 0xb0:
+                self.xb0 = list(response_data)
+            elif frame_type == 0xb1:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb1 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.thumb_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb2:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb2 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.index_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb3:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb3 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.middle_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb4:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb4 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.ring_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb5:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb5 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.little_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0x64: # L7 version number
+                self.version = list(response_data)
+            elif frame_type == 0xC2: # O6 version number
+                self.version = list(response_data)
+            elif frame_type == 0xC0:
+                d = list(response_data)
+                index = self.serial_number_map.get(d[0])
+                if index is not None:
+                    self.serial_number=self.serial_number + d[1:]
+                else:
+                    self.serial_number=self.serial_number + [-1] * 6
+
+    def get_version(self):
+        self.send_frame(0x64, [], sleep=0.1)
+        time.sleep(0.1)
+        if self.version is None:
+            self.send_frame(0xC2, [], sleep=0.1)
+            time.sleep(0.1)
+        return self.version
+
+    def get_current_status(self):
+        if self.is_lock:
+            return self.x01
+        elif self.is_lock == False:
+            self.send_frame(0x01, [],sleep=0.003)
+            return self.x01
+        
+    def get_current_pub_status(self):
+        return self.x01
+
+    def get_speed(self):
+        self.send_frame(0x05, [],sleep=0.003)
+        return self.x05
+
+    def get_current(self):
+        '''Not supported yet.'''
+        self.send_frame(0x2, [],sleep=0.1)
+        return self.x02
+
+
+
+    def get_torque(self):
+        '''Not supported yet.'''
+        self.send_frame(0x2, [],sleep=0.01)
+        return self.x02
+
+    def get_touch_type(self):
+        '''Get touch type'''
+        self.send_frame(0xb1,[])
+        t = []
+        for i in range(3):
+            t = self.xb1
+            time.sleep(0.01)
+        if len(t) == 2:
+            return 2
+        else:
+            self.send_frame(0x20,[],sleep=0.03)
+            time.sleep(0.01)
+            if self.normal_force[0] == -1:
+                return -1
+            else:
+                return 1
+
+
+    def get_touch(self):
+        '''Get touch data'''
+        self.send_frame(0xb1,[],sleep=0.03)
+        self.send_frame(0xb2,[],sleep=0.03)
+        self.send_frame(0xb3,[],sleep=0.03)
+        self.send_frame(0xb4,[],sleep=0.03)
+        self.send_frame(0xb5,[],sleep=0.03)
+        return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+    
+    def get_matrix_touch(self):
+        self.send_frame(0xb1,[0xc6],sleep=0.01)
+        self.send_frame(0xb2,[0xc6],sleep=0.01)
+        self.send_frame(0xb3,[0xc6],sleep=0.01)
+        self.send_frame(0xb4,[0xc6],sleep=0.01)
+        self.send_frame(0xb5,[0xc6],sleep=0.01)
+
+        return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+    
+    def get_matrix_touch_v2(self):
+        self.send_frame(0xb1,[0xc6],sleep=0.005)
+        self.send_frame(0xb2,[0xc6],sleep=0.005)
+        self.send_frame(0xb3,[0xc6],sleep=0.005)
+        self.send_frame(0xb4,[0xc6],sleep=0.005)
+        self.send_frame(0xb5,[0xc6],sleep=0.005)
+        return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+
+    def get_thumb_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb1,[0xc6],sleep=sleep_time)
+        return self.thumb_matrix
+    
+    def get_index_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb2,[0xc6],sleep=sleep_time)
+        return self.index_matrix
+    
+    def get_middle_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb3,[0xc6],sleep=sleep_time)
+        return self.middle_matrix
+    
+    def get_ring_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb4,[0xc6],sleep=sleep_time)
+        return self.ring_matrix
+    
+    def get_little_matrix_touch(self,sleep_time=0.005):
+        self.send_frame(0xb5,[0xc6],sleep=sleep_time)
+        return self.little_matrix
+
+    def get_force(self):
+        '''Get pressure.'''
+        return [self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc]
+
+    def get_temperature(self):
+        '''Get temperature.'''
+        self.get_motor_temperature()
+        return self.x33
+
+    def get_fault(self):
+        '''Get faults.'''
+        self.get_motor_fault_code()
+        return self.x35
+    
+    def get_serial_number(self):
+        try:
+            self.send_frame(0xC0,[],sleep=0.005)
+            # 1. 使用 bytes() 函数将整数列表转换为字节对象
+            #    bytes() 接收一个由 0-255 之间的整数组成的列表。
+            byte_data = bytes(self.serial_number)
+            # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+            result_string = byte_data.decode('ascii')
+            if result_string == "":
+                return "-1"
+            else:
+                # print(f"原始 ASCII 码列表: {self.serial_number}")
+                # print(f"解码后的字符串: {result_string}")
+                return result_string
+        except:
+            return "-1"
+    
+    def get_finger_order(self):
+        return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", "thumb_cmc_roll"]
+    
+    def show_fun_table(self):
+        pass
+
+    def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+        """L7 暂不支持清除故障码"""
+        pass
+
+    def close_can_interface(self):
+        """Stop the CAN communication."""
+        self.running = False
+        if self.receive_thread.is_alive():
+            self.receive_thread.join()
+        if self.bus:
+            self.bus.shutdown()

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py

@@ -0,0 +1,447 @@
+import can
+import time, sys
+import threading
+import numpy as np
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+
+class LinkerHandO6Can:
+    def __init__(self, can_id, can_channel='can0', baudrate=1000000,yaml=""):
+        self.can_id = can_id
+        self.can_channel = can_channel
+        self.baudrate = baudrate
+        self.open_can = OpenCan(load_yaml=yaml)
+
+        self.x01 = [0] * 6 # 关节位置
+        self.x02 = [-1] * 6 # 转矩限制
+        self.x05 = [0] * 6 # 速度
+        self.x07 = [-1] * 6 # 加速度
+        self.x33 = [0] * 6 # 温度
+        self.x35 = [0] * 6 # 关节错误码
+        self.x36 = [-1] * 6 # 电流
+        self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+
+        self.thumb_matrix = np.full((10, 4), -1)
+        self.index_matrix = np.full((10, 4), -1)
+        self.middle_matrix = np.full((10, 4), -1)
+        self.ring_matrix = np.full((10, 4), -1)
+        self.little_matrix = np.full((10, 4), -1)
+        self.matrix_map = {
+            0: 0,
+            16: 1,
+            32: 2,
+            48: 3,
+            64: 4,
+            80: 5,
+            96: 6,
+            112: 7,
+            128: 8,
+            144: 9,
+            160: 10,
+            176: 11,
+        }
+        self.serial_number = []
+        self.serial_number_map = {
+            0: 0,
+            1: 1,
+            2: 2,
+            3: 3,
+        }
+        # Fault codes
+        
+        self.joint_angles = [0] * 6
+        self.pressures = [200] * 6  # Default torque 200
+        self.bus = self.init_can_bus(can_channel, baudrate)
+        self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)]
+        self.is_lock = False
+        self.version = None
+        # Start the receiving thread
+        self.running = True
+        
+        self.receive_thread = threading.Thread(target=self.receive_response)
+        self.receive_thread.daemon = True
+        self.receive_thread.start()
+        time.sleep(0.1)
+        self._check_touch_type()
+
+    def _check_touch_type(self):
+        '''根据SN编码判断压感类型'''
+        self.sn = self.get_serial_number()
+        time.sleep(0.1)
+        if self.sn != "-1":
+            parts = self.sn.split("-")
+            if parts[4] == "A":
+                self.touch_type = 1
+            elif parts[4] == "B":
+                self.touch_type = 2
+                self.touch_code = 0xA4  # 6*12 O6 一律0XA4
+            elif parts[4] == "J":
+                self.touch_type = 3
+            elif parts[4] == "F":
+                self.touch_type = 4
+                self.touch_code = 0xA4 # 4*10
+            elif parts[4] == "Z":
+                self.touch_type = -1
+        else:
+            # 如果没有SN编码则根据返回数据进行判断
+            self.touch_type = self.get_touch_type()
+
+
+    def init_can_bus(self, channel, baudrate):
+        """
+        尝试按优先级连接 CAN 总线，并实现回退机制。
+        """
+        # --- 统一异常处理块开始 ---
+        try:
+            if sys.platform == "linux":
+                # Linux 优先级：1. socketcan
+                try:
+                    self.open_can.open_can(self.can_channel)
+                    # 尝试 socketcan
+                    bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+                    ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+                    raise # 重新抛出异常，让外层 try 捕获
+            elif sys.platform == "win32":
+                # Windows 优先级：1. pcan
+                try:
+                    bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+                # Windows 优先级：2. candle (回退方法)
+                try:
+                    bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+                    ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+                    return bus
+                except CanError as e:
+                    ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+                    raise # 两个接口都失败，抛出异常
+            else:
+                raise EnvironmentError("Unsupported platform for CAN interface")
+        # --- 统一异常处理块结束 ---
+        except Exception as e:
+            # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+            ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+            # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+            raise
+
+    def send_frame(self, frame_property, data_list,sleep=0.005):
+        """Send a single CAN frame with specified properties and data."""
+        frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+        data = [frame_property_value] + [int(val) for val in data_list]
+        msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+        try:
+            self.bus.send(msg)
+        except can.CanError as e:
+            print(f"Failed to send message: {e}")
+            self.open_can.open_can(self.can_channel)
+            time.sleep(1)
+            self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+            time.sleep(1)
+            if self.is_can:
+                self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+            else:
+                print("Reconnecting CAN devices ....")
+        time.sleep(sleep)
+
+    def set_joint_positions(self, joint_angles):
+        """Set the positions of 10 joints (joint_angles: list of 10 values)."""
+        if len(joint_angles) > 6:
+            self.joint_angles = joint_angles[:6]
+        else:
+            self.joint_angles = joint_angles
+        # Send angle control in frames
+        self.send_frame(0x01, self.joint_angles, sleep=0.003)
+
+    def set_max_torque_limits(self, pressures, type="get"):
+        """Set maximum torque limits."""
+        if type == "get":
+            self.pressures = [0.0]
+        else:
+            self.pressures = pressures[:6]
+
+    def set_torque(self, torque=[180] * 6):
+        """Set L6 maximum torque limits."""
+        if len(torque) != 6:
+            raise ValueError("Torque list must have 6 elements.")
+            return
+        self.send_frame(0x02, torque)
+
+    def set_speed(self, speed=[180] * 6):
+        """Set L6 speed."""
+        if len(speed) != 6:
+            raise ValueError("Speed list must have 6 elements.")
+            return
+        self.x05 = speed
+        for i in range(2):
+            time.sleep(0.001)
+            self.send_frame(0x05, speed)
+
+    ''' -------------------Pressure Sensors---------------------- '''
+    def get_normal_force(self):
+        self.send_frame(0x20, [],sleep=0.01)
+
+    def get_tangential_force(self):
+        self.send_frame(0x21, [],sleep=0.01)
+
+    def get_tangential_force_dir(self):
+        self.send_frame(0x22, [],sleep=0.01)
+
+    def get_approach_inc(self):
+        self.send_frame(0x23, [],sleep=0.01)
+
+    ''' -------------------Motor Temperature---------------------- '''
+    def get_motor_temperature(self):
+        self.send_frame(0x33, [])
+
+    # Motor fault codes
+    def get_motor_fault_code(self):
+        self.send_frame(0x35, [])
+
+    def receive_response(self):
+        """Receive CAN responses and process them."""
+        while self.running:
+            try:
+                msg = self.bus.recv(timeout=1.0)
+                if msg:
+                    self.process_response(msg)
+            except can.CanError as e:
+                print(f"Error receiving CAN message: {e}")
+
+    def process_response(self, msg):
+        """Process received CAN messages."""
+        if msg.arbitration_id == self.can_id:
+            frame_type = msg.data[0]
+            response_data = msg.data[1:]
+            if len(list(response_data)) == 0:
+                return
+            if frame_type == 0x01:   # 0x01
+                self.x01 = list(response_data)
+            elif frame_type == 0x02:    # 0x02
+                self.x02 = list(response_data)
+            elif frame_type == 0x05: # Set speed
+                self.x05 = list(response_data)
+            elif frame_type == 0x20:
+                d = list(response_data)
+                self.normal_force = [float(i) for i in d]
+            elif frame_type == 0x21:
+                d = list(response_data)
+                self.tangential_force = [float(i) for i in d]
+            elif frame_type == 0x22:
+                d = list(response_data)
+                self.tangential_force_dir = [float(i) for i in d]
+            elif frame_type == 0x23:
+                d = list(response_data)
+                self.approach_inc = [float(i) for i in d]
+            elif frame_type == 0x33: # O6 temperature
+                self.x33 = list(response_data)
+            elif frame_type == 0x35: # O6 fault codes
+                self.x35 = list(response_data)
+            elif frame_type == 0x36: # O6 电流
+                self.x36 = list(response_data)
+            elif frame_type == 0xb0:
+                self.xb0 = list(response_data)
+            elif frame_type == 0xb1:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb1 = d
+                elif len(d) == 5:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.thumb_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb2:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb2 = d
+                elif len(d) == 5:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.index_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb3:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb3 = d
+                elif len(d) == 5:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.middle_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb4:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb4 = d
+                elif len(d) == 5:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.ring_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb5:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb5 = d
+                elif len(d) == 5:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.little_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0x64: # O6 version number
+                self.version = list(response_data)
+            elif frame_type == 0xC2: # O6 version number
+                self.version = list(response_data)
+            elif frame_type == 0xC0:
+                d = list(response_data)
+                index = self.serial_number_map.get(d[0])
+                if index is not None:
+                    self.serial_number += d[1:]
+                else:
+                    self.serial_number=self.serial_number + [-1] * 6
+                
+
+
+    def get_version(self):
+        self.send_frame(0x64, [],sleep=0.1)
+        time.sleep(0.1)
+        if self.version is None:
+            self.send_frame(0xC2, [],sleep=0.1)
+            time.sleep(0.1)
+        return self.version
+
+    def get_current_status(self):
+        self.send_frame(0x01, [],sleep=0.005)
+        return self.x01
+        
+    def get_current_pub_status(self):
+        return self.x01
+
+    def get_speed(self):
+        self.send_frame(0x05, [],sleep=0.002)
+        #print("L6暂不支持读取实时速度")
+        return self.x05
+
+    def get_current(self):
+        '''Not supported yet.'''
+        self.send_frame(0x36, [],sleep=0.005)
+        return self.x36
+
+
+
+    def get_torque(self):
+        '''Not supported yet.'''
+        self.send_frame(0x2, [],sleep=0.01)
+        return self.x02
+
+    def get_touch_type(self):
+        '''Get touch type'''
+        self.send_frame(0xb1,[])
+        t = []
+        for i in range(3):
+            t = self.xb1
+            time.sleep(0.01)
+        if len(t) == 2:
+            return 2
+        else:
+            self.send_frame(0x20,[],sleep=0.03)
+            time.sleep(0.01)
+            if self.normal_force[0] == -1:
+                return -1
+            else:
+                return 1
+
+
+    def get_touch(self):
+        '''Get touch data'''
+        self.send_frame(0xb1,[],sleep=0.03)
+        self.send_frame(0xb2,[],sleep=0.03)
+        self.send_frame(0xb3,[],sleep=0.03)
+        self.send_frame(0xb4,[],sleep=0.03)
+        self.send_frame(0xb5,[],sleep=0.03)
+        return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+    
+    def get_matrix_touch(self):
+        self.send_frame(0xb1,[self.touch_code],sleep=0.01)
+        self.send_frame(0xb2,[self.touch_code],sleep=0.01)
+        self.send_frame(0xb3,[self.touch_code],sleep=0.01)
+        self.send_frame(0xb4,[self.touch_code],sleep=0.01)
+        self.send_frame(0xb5,[self.touch_code],sleep=0.01)
+
+        return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+    
+    def get_matrix_touch_v2(self):
+        self.send_frame(0xb1,[self.touch_code],sleep=0.009)
+        self.send_frame(0xb2,[self.touch_code],sleep=0.009)
+        self.send_frame(0xb3,[self.touch_code],sleep=0.009)
+        self.send_frame(0xb4,[self.touch_code],sleep=0.009)
+        self.send_frame(0xb5,[self.touch_code],sleep=0.009)
+        return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+    
+    def get_thumb_matrix_touch(self,sleep_time=0.002):
+        self.send_frame(0xb1,[self.touch_code],sleep=sleep_time)
+        return self.thumb_matrix
+    
+    def get_index_matrix_touch(self,sleep_time=0.002):
+        self.send_frame(0xb2,[self.touch_code],sleep=sleep_time)
+        return self.index_matrix
+    
+    def get_middle_matrix_touch(self,sleep_time=0.002):
+        self.send_frame(0xb3,[self.touch_code],sleep=sleep_time)
+        return self.middle_matrix
+    
+    def get_ring_matrix_touch(self,sleep_time=0.002):
+        self.send_frame(0xb4,[self.touch_code],sleep=sleep_time)
+        return self.ring_matrix
+    
+    def get_little_matrix_touch(self,sleep_time=0.002):
+        self.send_frame(0xb5,[self.touch_code],sleep=sleep_time)
+        return self.little_matrix
+
+    def get_force(self):
+        '''Get pressure.'''
+        return [self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc]
+
+    def get_temperature(self):
+        '''Get temperature.'''
+        self.get_motor_temperature()
+        return self.x33
+
+    def get_fault(self):
+        '''Get faults.'''
+        self.get_motor_fault_code()
+        return self.x35
+    
+    def get_finger_order(self):
+        return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+        
+    def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+        """O6 暂不支持清除故障码"""
+        pass
+    def show_fun_table(self):
+        pass
+
+    def get_serial_number(self):
+        try:
+            self.send_frame(0xC0,[],sleep=0.005)
+            # 1. 使用 bytes() 函数将整数列表转换为字节对象
+            #    bytes() 接收一个由 0-255 之间的整数组成的列表。
+            byte_data = bytes(self.serial_number)
+            # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+            result_string = byte_data.decode('ascii')
+            if result_string == "":
+                return "-1"
+            else:
+                # print(f"原始 ASCII 码列表: {self.serial_number}")
+                # print(f"解码后的字符串: {result_string}")
+                return result_string
+        except:
+            return "-1"
+
+    def close_can_interface(self):
+        """Stop the CAN communication."""
+        self.running = False
+        if self.receive_thread.is_alive():
+            self.receive_thread.join()
+        if self.bus:
+            self.bus.shutdown()

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l10_rs485.py

@@ -0,0 +1,345 @@
+#!/usr/bin/env python3
+import os
+import time
+import struct
+from typing import Dict, List
+import numpy as np
+from pymodbus.client import ModbusSerialClient
+_INTERVAL = 0.005  # 8 ms
+class LinkerHandL10RS485:
+    KEYS = ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch",
+            "ring_mcp_pitch", "pinky_mcp_pitch", "index_mcp_roll", "ring_mcp_roll",
+            "pinky_mcp_roll", "thumb_cmc_yaw"]
+
+    def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+        self.slave = hand_id
+        self.cli = ModbusSerialClient(
+            port=modbus_port,
+            baudrate=baudrate,
+            bytesize=8,
+            parity="N",
+            stopbits=1,
+            timeout=0.05,          # 50 ms 超时
+            retries=3,             # 重试次数
+            retry_on_empty=True,
+            handle_local_echo=False
+        )
+        # 在 pymodbus 3.5.1 中，连接需要显式调用 connect()
+        self.connected = self.cli.connect()
+        if not self.connected:
+            raise ConnectionError(f"RS485 connect fail to {modbus_port}")
+
+    # --------------------------------------------------
+    # 批量读取接口
+    # --------------------------------------------------
+    def read_angles(self) -> List[int]:
+        time.sleep(_INTERVAL)
+        rsp = self.cli.read_input_registers(address=0, count=10, slave=self.slave)
+        if rsp.isError():
+            raise RuntimeError(f"read_angles failed: {rsp}")
+        return rsp.registers
+
+    def read_torques(self) -> List[int]:
+        time.sleep(_INTERVAL)
+        rsp = self.cli.read_input_registers(address=10, count=10, slave=self.slave)
+        if rsp.isError():
+            raise RuntimeError(f"read_torques failed: {rsp}")
+        return rsp.registers
+
+    def read_speeds(self) -> List[int]:
+        time.sleep(_INTERVAL)
+        rsp = self.cli.read_input_registers(address=20, count=10, slave=self.slave)
+        if rsp.isError():
+            raise RuntimeError(f"read_speeds failed: {rsp}")
+        return rsp.registers
+
+    def read_temperatures(self) -> List[int]:
+        time.sleep(_INTERVAL)
+        rsp = self.cli.read_input_registers(address=40, count=10, slave=self.slave)
+        if rsp.isError():
+            raise RuntimeError(f"read_temperatures failed: {rsp}")
+        return rsp.registers
+
+    def read_error_codes(self) -> List[int]:
+        time.sleep(_INTERVAL)
+        rsp = self.cli.read_input_registers(address=50, count=10, slave=self.slave)
+        if rsp.isError():
+            raise RuntimeError(f"read_error_codes failed: {rsp}")
+        return rsp.registers
+
+    def read_versions(self) -> dict:
+        time.sleep(_INTERVAL)
+        rsp = self.cli.read_input_registers(address=158, count=6, slave=self.slave)
+        if rsp.isError():
+            raise RuntimeError(f"read_versions failed: {rsp}")
+        keys = ["hand_freedom", "hand_version", "hand_number",
+                "hand_direction", "software_version", "hardware_version"]
+        #return dict(zip(keys, rsp.registers))
+        return rsp.registers
+
+    # --------------------------------------------------
+    # 5 个压力传感器
+    # --------------------------------------------------
+    def read_pressure_thumb(self) -> np.ndarray:
+        return np.array(self._pressure(1), dtype=np.uint8)
+
+    def read_pressure_index(self) -> np.ndarray:
+        return np.array(self._pressure(2), dtype=np.uint8)
+
+    def read_pressure_middle(self) -> np.ndarray:
+        return np.array(self._pressure(3), dtype=np.uint8)
+
+    def read_pressure_ring(self) -> np.ndarray:
+        return np.array(self._pressure(4), dtype=np.uint8)
+
+    def read_pressure_pinky(self) -> np.ndarray:
+        return np.array(self._pressure(5), dtype=np.uint8)
+
+    # def _pressure(self, finger: int) -> List[int]:
+    #     time.sleep(_INTERVAL)
+    #     # 先选择手指
+    #     wrsp = self.cli.write_register(address=60, value=finger, slave=self.slave)
+    #     if wrsp.isError():
+    #         raise RuntimeError(f"write finger select {finger} failed: {wrsp}")
+        
+    #     time.sleep(_INTERVAL)
+    #     # 读取压力传感器数据 (96个寄存器)
+    #     rrsp = self.cli.read_input_registers(address=62, count=96, slave=self.slave)
+    #     if rrsp.isError():
+    #         raise RuntimeError(f"read pressure finger={finger} failed: {rrsp}")
+    #     return np.array(rrsp.registers, dtype=np.uint8)
+    def _pressure(self, finger: int) -> np.ndarray:
+        """
+        6x12 (72点) 矩阵尺寸。
+        Modbus 地址 60/62。
+        """
+        rows = 12  # 12 行
+        cols = 6   # 6 列
+        finger_size = rows * cols  # 72 个数据点
+        
+        # modbus 地址和计数
+        write_address = 70  # 写入手指选择
+        read_address = 72   # 读取压力数据
+        read_count = 96     # 读取 96 个寄存器
+        skip_count = 10     # 跳过前 10 个校验点
+        
+        # 0. 参数校验和手指写入值确定
+        if finger < 1 or finger > 5:
+            raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
+            
+        finger_write_value = finger 
+        
+        # 1. 写入手指选择寄存器 (地址 60)
+        time.sleep(0.008)
+        wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
+        if wrsp.isError():
+            raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
+
+        # 写入后等待片刻
+        time.sleep(0.008) 
+        
+        # 2. 读取地址 62 的数据
+        rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
+        
+        if rrsp.isError():
+            raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
+            
+        registers_16bit: List[int] = rrsp.registers 
+        
+        # 3. 核心数据处理
+        # a. 提取低 8 位数据 (得到 96 个 8 位数据点)
+        final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
+        
+        # b. 跳过前 10 个校验/头部数据点 (得到 86 个有效数据点)
+        effective_data = np.array(final_data_96[skip_count:], dtype=np.uint8)
+        # c. 截取当前手指的矩阵数据 (从 86 个有效点中截取 72 个点)
+        start_idx = 0 
+        end_idx = finger_size  # 72
+        
+        finger_data_flat = effective_data[start_idx:end_idx]
+        
+        # d. 验证数据长度
+        if finger_data_flat.size != finger_size:
+            raise ValueError(
+                f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
+                f"但仅截取到 {finger_data_flat.size} 点。请检查协议，确认地址 62 是否一次性返回了所有手指数据。"
+            )
+            
+        # e. 重塑为二维矩阵 (12 行 6 列)
+        finger_matrix = finger_data_flat.reshape((rows, cols))
+                
+        return finger_matrix
+
+    
+    
+
+    # --------------------------------------------------
+    # 批量写入接口
+    # --------------------------------------------------
+    def write_angles(self, vals: List[int]):
+        vals = [int(x) for x in vals]
+        if not self.is_valid_10xuint8(vals):
+            raise ValueError("需要 10 个 0-255 整数")
+        
+        time.sleep(_INTERVAL)
+        rsp = self.cli.write_registers(address=0, values=vals, slave=self.slave)
+        if rsp.isError():
+            raise RuntimeError(f"write_angles failed: {rsp}")
+
+    def write_speeds(self, vals: List[int]):
+        vals = [int(x) for x in vals]
+        if not self.is_valid_10xuint8(vals):
+            raise ValueError("需要 10 个 0-255 整数")
+            
+        time.sleep(_INTERVAL)
+        rsp = self.cli.write_registers(address=20, values=vals, slave=self.slave)
+        if rsp.isError():
+            raise RuntimeError(f"write_speeds failed: {rsp}")
+
+    def write_torques(self, vals: List[int]):
+        vals = [int(x) for x in vals]
+        if not self.is_valid_10xuint8(vals):
+            raise ValueError("需要 10 个 0-255 整数")
+            
+        time.sleep(_INTERVAL)
+        rsp = self.cli.write_registers(address=10, values=vals, slave=self.slave)
+        if rsp.isError():
+            raise RuntimeError(f"write_torques failed: {rsp}")
+
+    # --------------------------------------------------
+    # 上下文管理
+    # --------------------------------------------------
+    def close(self):
+        if self.connected:
+            self.cli.close()
+            self.connected = False
+
+    def __enter__(self):
+        return self
+
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        self.close()
+
+    # --------------------------------------------------
+    # 工具函数
+    # --------------------------------------------------
+    def is_valid_10xuint8(self, lst) -> bool:
+        if len(lst) != 10:
+            return False
+        return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
+
+    # --------------------------------------------------
+    # 固定 API 接口
+    # --------------------------------------------------
+    def set_joint_positions(self, joint_angles=None):
+        joint_angles = joint_angles or [0] * 10
+        self.write_angles(joint_angles)
+
+    def set_speed(self, speed=None):
+        speed = speed or [200] * 10
+        self.write_speeds(speed)
+
+    def set_torque(self, torque=None):
+        torque = torque or [200] * 10
+        self.write_torques(torque)
+
+    def set_current(self, current=None):
+        print("当前L10不支持设置电流", flush=True)
+
+    def get_version(self) -> dict:
+        return self.read_versions()
+
+    def get_current(self):
+        print("当前L10不支持获取电流", flush=True)
+
+    def get_state(self) -> List[int]:
+        return self.read_angles()
+
+    def get_state_for_pub(self) -> List[int]:
+        return self.get_state()
+
+    def get_current_status(self) -> List[int]:
+        return self.get_state()
+
+    def get_speed(self) -> List[int]:
+        return self.read_speeds()
+
+    def get_joint_speed(self) -> List[int]:
+        return self.get_speed()
+
+    def get_touch_type(self) -> int:
+        return 2
+
+    def get_normal_force(self) -> List[int]:
+        return [-1] * 5
+
+    def get_tangential_force(self) -> List[int]:
+        return [-1] * 5
+
+    def get_approach_inc(self) -> List[int]:
+        return [-1] * 5
+
+    def get_touch(self) -> List[int]:
+        return [-1] * 5
+
+    def get_thumb_matrix_touch(self,sleep_time=0):
+        return self._pressure(1)
+
+    def get_index_matrix_touch(self,sleep_time=0):
+        return self._pressure(2)
+
+    def get_middle_matrix_touch(self,sleep_time=0):
+        return self._pressure(3)
+
+    def get_ring_matrix_touch(self,sleep_time=0):
+        return self._pressure(4)
+
+    def get_little_matrix_touch(self,sleep_time=0):
+        return self._pressure(5)
+
+    def get_matrix_touch(self) -> List[List[int]]:
+        return self.get_thumb_matrix_touch(),self.get_index_matrix_touch(), self.get_middle_matrix_touch(), self.get_ring_matrix_touch(), self.get_little_matrix_touch()
+
+    def get_matrix_touch_v2(self) -> List[List[int]]:
+        return self.get_matrix_touch()
+
+    def get_torque(self) -> List[int]:
+        return self.read_torques()
+
+    def get_temperature(self) -> List[int]:
+        return self.read_temperatures()
+
+    def get_fault(self) -> List[int]:
+        return self.read_error_codes()
+    
+    def get_serial_number(self):
+        return [0] * 6
+
+    def clear_faults(self):
+        pass
+
+# ------------------- demo -------------------
+if __name__ == "__main__":
+    try:
+        with LinkerHandL10RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
+            print("连接成功!")
+            
+            # 测试读取角度
+            angles = hand.read_angles()
+            print("角度:", dict(zip(LinkerHandL10RS485.KEYS, angles)))
+            
+            # 测试读取版本信息
+            ver = hand.get_version()
+            print("版本信息:", ver)
+            
+            # 测试压力传感器
+            print("拇指压力传感器数据长度:", len(hand.read_pressure_thumb()))
+            
+            # 测试其他读取功能
+            print("电流:", hand.read_torques())
+            print("速度:", hand.read_speeds())
+            print("温度:", hand.read_temperatures())
+            print("错误码:", hand.read_error_codes())
+            
+    except Exception as e:
+        print(f"错误: {e}")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py

@@ -0,0 +1,460 @@
+#!/usr/bin/env python3
+import os
+import time
+from pymodbus.client import ModbusSerialClient
+from typing import List, Dict
+import numpy as np
+
+_INTERVAL = 0.006  # 8 ms
+
+class LinkerHandL6RS485:
+    """L6机械手 Modbus-RTU 控制类"""
+    
+    # 6个关节名称
+    JOINT_NAMES = ["thumb_pitch", "thumb_yaw", "index_pitch", 
+                   "middle_pitch", "ring_pitch", "little_pitch"]
+    
+    # 手指名称
+    FINGER_NAMES = ["thumb", "index", "middle", "ring", "little"]
+    
+    def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+        """
+        初始化L6机械手
+        hand_id: 右手0x27(39), 左手0x28(40)
+        modbus_port: 串口设备路径
+        baudrate: 波特率，固定115200
+        """
+        self.slave = hand_id
+        self.cli = ModbusSerialClient(
+            port=modbus_port, 
+            baudrate=baudrate,
+            bytesize=8, 
+            parity="N", 
+            stopbits=1,
+            timeout=0.05
+        )
+        # pymodbus 3.5.1 需要显式连接
+        self.connected = self.cli.connect()
+        if not self.connected:
+            raise ConnectionError(f"RS485连接失败，端口: {modbus_port}")
+
+    def _read_input_registers(self, address: int, count: int) -> List[int]:
+        """读取输入寄存器"""
+        time.sleep(_INTERVAL)
+        result = self.cli.read_input_registers(address=address, count=count, slave=self.slave)
+        if result.isError():
+            raise RuntimeError(f"读取输入寄存器失败: address={address}, count={count}")
+        return result.registers
+
+    def _write_register(self, address: int, value: int):
+        """写入单个寄存器"""
+        time.sleep(_INTERVAL)
+        result = self.cli.write_register(address=address, value=value, slave=self.slave)
+        if result.isError():
+            raise RuntimeError(f"写入寄存器失败: address={address}, value={value}")
+
+    def _write_registers(self, address: int, values: List[int]):
+        """写入多个寄存器"""
+        time.sleep(_INTERVAL)
+        result = self.cli.write_registers(address=address, values=values, slave=self.slave)
+        if result.isError():
+            raise RuntimeError(f"写入多个寄存器失败: address={address}, values={values}")
+
+    # --------------------------------------------------
+    # 基础读取接口
+    # --------------------------------------------------
+    
+    def read_angles(self) -> List[int]:
+        """读取6个关节角度 (输入寄存器 0-5)"""
+        return self._read_input_registers(0, 6)
+
+    def read_torques(self) -> List[int]:
+        """读取6个关节转矩 (输入寄存器 6-11)"""
+        return self._read_input_registers(6, 6)
+
+    def read_speeds(self) -> List[int]:
+        """读取6个关节速度 (输入寄存器 12-17)"""
+        return self._read_input_registers(12, 6)
+
+    def read_temperatures(self) -> List[int]:
+        """读取6个关节温度 (输入寄存器 18-23)"""
+        return self._read_input_registers(18, 6)
+
+    def read_error_codes(self) -> List[int]:
+        """读取6个关节错误码 (输入寄存器 24-29)"""
+        return self._read_input_registers(24, 6)
+
+    # --------------------------------------------------
+    # 压力传感器接口
+    # --------------------------------------------------
+    
+    # def _pressure(self, finger: int) -> List[int]:
+    #     """内部：选手指 → 读压力数据"""
+    #     # 选择手指 (保持寄存器 36)
+    #     self._write_register(36, finger)
+    #     time.sleep(_INTERVAL)
+    #     # 读取压力数据 (输入寄存器 52-122)
+    #     return np.array(self._read_input_registers(52, 71))
+    def _pressure(self, finger: int) -> np.ndarray:
+        """
+        6x12 (72点) 矩阵尺寸。
+        Modbus 地址 60/62。
+        """
+        rows = 12  # 12 行
+        cols = 6   # 6 列
+        finger_size = rows * cols  # 72 个数据点
+        
+        # modbus 地址和计数 (按协议文档)
+        write_address = 36  # 写入手指选择 (保持寄存器)
+        read_address = 52   # 读取压力数据 (输入寄存器)
+        read_count = 71     # 读取 71 个寄存器
+        skip_count = 0      # 不跳过数据点
+        
+        # 0. 参数校验和手指写入值确定
+        if finger < 1 or finger > 5:
+            raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
+            
+        finger_write_value = finger 
+        
+        # 1. 写入手指选择寄存器 (地址 36)
+        time.sleep(0.08)
+        wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
+        if wrsp.isError():
+            raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
+
+        # 写入后等待片刻
+        time.sleep(0.08) 
+        
+        # 2. 读取地址 52 的数据
+        rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
+        
+        if rrsp.isError():
+            raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
+            
+        registers_16bit: List[int] = rrsp.registers 
+        
+        # 3. 核心数据处理
+        # a. 提取低 8 位数据 (得到 71 个 8 位数据点)
+        final_data_71 = [reg_value & 255 for reg_value in registers_16bit]
+        
+        # b. 不跳过数据点 (按协议)
+        effective_data = np.array(final_data_71, dtype=np.uint8)
+        
+        # c. 截取当前手指的矩阵数据 (71 个有效点中截取 72 个点，可能需要多读)
+        # 注: 协议返回 71 个点，手指数 1-5，每个手指需要 72 点
+        # 这里取全部数据
+        start_idx = 0 
+        end_idx = min(len(effective_data), finger_size)  # 取较小值
+
+        finger_data_flat = effective_data[start_idx:end_idx]
+        
+        # d. 验证数据长度
+        if finger_data_flat.size < finger_size:
+            # 如果数据不足，尝试多读一些
+            rrsp2 = self.cli.read_input_registers(address=read_address + read_count, count=10, slave=self.slave)
+            if not rrsp2.isError():
+                extra_data = [reg_value & 255 for reg_value in rrsp2.registers]
+                finger_data_flat = np.concatenate([finger_data_flat, np.array(extra_data, dtype=np.uint8)])
+        
+        # 最终确保有足够数据
+        if finger_data_flat.size >= finger_size:
+            finger_data_flat = finger_data_flat[:finger_size]
+        else:
+            raise ValueError(
+                f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
+                f"但仅获取到 {finger_data_flat.size} 点。请检查协议。"
+            )
+            
+        # e. 重塑为二维矩阵 (12 行 6 列)
+        finger_matrix = finger_data_flat.reshape((rows, cols))
+                
+        return finger_matrix
+
+    def read_pressure_thumb(self) -> np.ndarray:
+        """读取大拇指压力数据"""
+        return np.array(self._pressure(1), dtype=np.uint8)
+
+    def read_pressure_index(self) -> np.ndarray:
+        """读取食指压力数据"""
+        return np.array(self._pressure(2), dtype=np.uint8)
+
+    def read_pressure_middle(self) -> np.ndarray:
+        """读取中指压力数据"""
+        return np.array(self._pressure(3), dtype=np.uint8)
+
+    def read_pressure_ring(self) -> np.ndarray:
+        """读取无名指压力数据"""
+        return np.array(self._pressure(4), dtype=np.uint8)
+
+    def read_pressure_little(self) -> np.ndarray:
+        """读取小拇指压力数据"""
+        return np.array(self._pressure(5), dtype=np.uint8)
+
+    # --------------------------------------------------
+    # 版本信息接口
+    # --------------------------------------------------
+    
+    def read_versions(self) -> Dict[str, int]:
+        """读取版本信息 (输入寄存器 148-155)"""
+        result = self._read_input_registers(148, 8)
+        
+        return {
+            "hand_freedom": result[0],
+            "hand_version": result[1],
+            "hand_number": result[2],
+            "hand_direction": result[3],
+            "software_version_major": result[4],
+            "software_version_minor": result[5] if len(result) > 5 else 0,
+            "software_version_revision": result[6] if len(result) > 6 else 0,
+            "hardware_version": result[7] if len(result) > 7 else 0
+        }
+
+    # --------------------------------------------------
+    # 写入接口
+    # --------------------------------------------------
+    
+    def write_angles(self, vals: List[int]):
+        """设置6个关节角度 (保持寄存器 0-5)"""
+        vals = [int(x) for x in vals]
+        if not self.is_valid_6xuint8(vals):
+            raise ValueError("需要6个0-255的整数")
+        self._write_registers(0, vals)
+
+    def write_torques(self, vals: List[int]):
+        """设置6个关节转矩 (保持寄存器 6-11)"""
+        vals = [int(x) for x in vals]
+        if not self.is_valid_6xuint8(vals):
+            raise ValueError("需要6个0-255的整数")
+        self._write_registers(6, vals)
+
+    def write_speeds(self, vals: List[int]):
+        """设置6个关节速度 (保持寄存器 12-17)"""
+        vals = [int(x) for x in vals]
+        if not self.is_valid_6xuint8(vals):
+            raise ValueError("需要6个0-255的整数")
+        self._write_registers(12, vals)
+
+    # --------------------------------------------------
+    # 上下文管理
+    # --------------------------------------------------
+    
+    def close(self):
+        """关闭连接"""
+        if self.connected:
+            self.cli.close()
+            self.connected = False
+    
+    def __enter__(self):
+        return self
+    
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        self.close()
+
+    # --------------------------------------------------
+    # API固定接口函数
+    # --------------------------------------------------
+    
+    def is_valid_6xuint8(self, lst) -> bool:
+        """验证6个0-255的整数列表"""
+        if len(lst) != 6:
+            return False
+        return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
+    
+    def set_joint_positions(self, joint_angles=None):
+        """设置关节位置"""
+        joint_angles = joint_angles or [0] * 6
+        self.write_angles(joint_angles)
+
+    def set_speed(self, speed=None):
+        """设置速度"""
+        speed = speed or [200] * 6
+        self.write_speeds(speed)
+    
+    def set_torque(self, torque=None):
+        """设置扭矩"""
+        torque = torque or [200] * 6
+        self.write_torques(torque)
+
+    def set_current(self, current=None):
+        """设置电流 (L6不支持)"""
+        print("当前L6不支持设置电流", flush=True)
+
+    def get_version(self) -> list:
+        """获取版本信息"""
+        versions = self.read_versions()
+        return [
+            versions.get("hand_freedom", 0),
+            versions.get("hand_version", 0),
+            versions.get("hand_number", 0),
+            versions.get("hand_direction", 0),
+            versions.get("software_version_major", 0),
+            versions.get("hardware_version", 0)
+        ]
+
+    def get_current(self):
+        """获取电流 (L6不支持)"""
+        print("当前L6不支持获取电流", flush=True)
+        return []
+
+    def get_state(self) -> list:
+        """获取关节状态"""
+        return self.read_angles()
+    
+    def get_state_for_pub(self) -> list:
+        return self.get_state()
+
+    def get_current_status(self) -> list:
+        return self.get_state()
+    
+    def get_speed(self) -> list:
+        """获取当前速度"""
+        return self.read_speeds()
+    
+    def get_joint_speed(self) -> list:
+        return self.get_speed()
+    
+    def get_touch_type(self) -> int:
+        """获取压感类型 (2=矩阵式)"""
+        return 2
+    
+    def get_normal_force(self) -> list:
+        """获取压感数据：点式"""
+        return [-1] * 5
+    
+    def get_tangential_force(self) -> list:
+        """获取压感数据：点式"""
+        return [-1] * 5
+    
+    def get_approach_inc(self) -> list:
+        """获取压感数据：点式"""
+        return [-1] * 5
+    
+    def get_touch(self) -> list:
+        return [-1] * 5
+
+    def get_thumb_matrix_touch(self,sleep_time=0):
+        return self._pressure(1)
+
+    def get_index_matrix_touch(self,sleep_time=0):
+        return self._pressure(2)
+
+    def get_middle_matrix_touch(self,sleep_time=0):
+        return self._pressure(3)
+
+    def get_ring_matrix_touch(self,sleep_time=0):
+        return self._pressure(4)
+
+    def get_little_matrix_touch(self,sleep_time=0):
+        return self._pressure(5)
+        
+    def get_matrix_touch(self) -> list:
+        """获取压感数据：矩阵式"""
+        return [self._pressure(1), self._pressure(2), self._pressure(3), 
+                self._pressure(4), self._pressure(5)]
+    
+    def get_matrix_touch_v2(self) -> list:
+        """获取压感数据：矩阵式"""
+        return self.get_matrix_touch()
+    
+    def get_torque(self) -> list:
+        """获取当前扭矩"""
+        return self.read_torques()
+    
+    def get_temperature(self) -> list:
+        """获取当前电机温度"""
+        return self.read_temperatures()
+    
+    def get_fault(self) -> list:
+        """获取当前电机故障码"""
+        return self.read_error_codes()
+    
+    def get_serial_number(self):
+        return [0] * 6
+
+    def get_finger_order(self):
+        return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+    # --------------------------------------------------
+    # 便捷方法
+    # --------------------------------------------------
+    
+    def relax(self):
+        """所有手指伸直"""
+        self.set_joint_positions([255] * 6)
+    
+    def fist(self):
+        """所有手指握拳"""
+        self.set_joint_positions([0] * 6)
+    
+    def dump_status(self):
+        """打印状态信息"""
+        print("=" * 50)
+        print("L6机械手状态信息")
+        print("=" * 50)
+        
+        try:
+            # 关节状态
+            angles = self.read_angles()
+            torques = self.read_torques()
+            speeds = self.read_speeds()
+            temps = self.read_temperatures()
+            errors = self.read_error_codes()
+            
+            print("关节状态:")
+            for i, name in enumerate(self.JOINT_NAMES):
+                print(f"  {name:15s}: 角度={angles[i]:3d}, 扭矩={torques[i]:3d}, "
+                      f"速度={speeds[i]:3d}, 温度={temps[i]:2d}℃, 错误={errors[i]:2d}")
+            
+            # 版本信息
+            versions = self.read_versions()
+            print("\n版本信息:")
+            for key, value in versions.items():
+                print(f"  {key:20s}: {value}")
+            
+            # 压力传感器测试
+            print("\n压力传感器测试:")
+            thumb_pressure = self.read_pressure_thumb()
+            print(f"大拇指压力数据长度: {len(thumb_pressure)}")
+            
+        except Exception as e:
+            print(f"读取状态时出错: {e}")
+        
+        print("=" * 50)
+
+
+# ------------------- 演示程序 -------------------
+if __name__ == "__main__":
+    # 使用示例
+    try:
+        with LinkerHandL6RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
+            print("连接成功!")
+            
+            # 打印状态信息
+            hand.dump_status()
+            
+            # 测试基本控制
+            print("\n测试控制功能...")
+            print("伸直手指...")
+            hand.relax()
+            time.sleep(2)
+            
+            print("握拳...")
+            hand.fist()
+            time.sleep(2)
+            
+            print("恢复伸直...")
+            hand.relax()
+            
+            # 测试压力传感器
+            print("\n测试压力传感器...")
+            thumb_matrix = hand.get_thumb_matrix_touch()
+            print(f"大拇指压力数据: {len(thumb_matrix)}个点")
+            
+            # 获取所有手指压力数据
+            all_matrices = hand.get_matrix_touch()
+            for i, name in enumerate(hand.FINGER_NAMES):
+                matrix = all_matrices[i]
+                print(f"{name}手指压力数据长度: {len(matrix)}")
+            
+    except Exception as e:
+        print(f"错误: {e}")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak

@@ -0,0 +1,444 @@
+#!/usr/bin/env python3
+import os
+import time
+from pymodbus.client import ModbusSerialClient
+from typing import List, Dict
+import numpy as np
+
+_INTERVAL = 0.006  # 8 ms
+
+class LinkerHandL6RS485:
+    """L6机械手 Modbus-RTU 控制类"""
+    
+    # 6个关节名称
+    JOINT_NAMES = ["thumb_pitch", "thumb_yaw", "index_pitch", 
+                   "middle_pitch", "ring_pitch", "little_pitch"]
+    
+    # 手指名称
+    FINGER_NAMES = ["thumb", "index", "middle", "ring", "little"]
+    
+    def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+        """
+        初始化L6机械手
+        hand_id: 右手0x27(39), 左手0x28(40)
+        modbus_port: 串口设备路径
+        baudrate: 波特率，固定115200
+        """
+        self.slave = hand_id
+        self.cli = ModbusSerialClient(
+            port=modbus_port, 
+            baudrate=baudrate,
+            bytesize=8, 
+            parity="N", 
+            stopbits=1,
+            timeout=0.05
+        )
+        # pymodbus 3.5.1 需要显式连接
+        self.connected = self.cli.connect()
+        if not self.connected:
+            raise ConnectionError(f"RS485连接失败，端口: {modbus_port}")
+
+    def _read_input_registers(self, address: int, count: int) -> List[int]:
+        """读取输入寄存器"""
+        time.sleep(_INTERVAL)
+        result = self.cli.read_input_registers(address=address, count=count, slave=self.slave)
+        if result.isError():
+            raise RuntimeError(f"读取输入寄存器失败: address={address}, count={count}")
+        return result.registers
+
+    def _write_register(self, address: int, value: int):
+        """写入单个寄存器"""
+        time.sleep(_INTERVAL)
+        result = self.cli.write_register(address=address, value=value, slave=self.slave)
+        if result.isError():
+            raise RuntimeError(f"写入寄存器失败: address={address}, value={value}")
+
+    def _write_registers(self, address: int, values: List[int]):
+        """写入多个寄存器"""
+        time.sleep(_INTERVAL)
+        result = self.cli.write_registers(address=address, values=values, slave=self.slave)
+        if result.isError():
+            raise RuntimeError(f"写入多个寄存器失败: address={address}, values={values}")
+
+    # --------------------------------------------------
+    # 基础读取接口
+    # --------------------------------------------------
+    
+    def read_angles(self) -> List[int]:
+        """读取6个关节角度 (输入寄存器 0-5)"""
+        return self._read_input_registers(0, 6)
+
+    def read_torques(self) -> List[int]:
+        """读取6个关节转矩 (输入寄存器 6-11)"""
+        return self._read_input_registers(6, 6)
+
+    def read_speeds(self) -> List[int]:
+        """读取6个关节速度 (输入寄存器 12-17)"""
+        return self._read_input_registers(12, 6)
+
+    def read_temperatures(self) -> List[int]:
+        """读取6个关节温度 (输入寄存器 18-23)"""
+        return self._read_input_registers(18, 6)
+
+    def read_error_codes(self) -> List[int]:
+        """读取6个关节错误码 (输入寄存器 24-29)"""
+        return self._read_input_registers(24, 6)
+
+    # --------------------------------------------------
+    # 压力传感器接口
+    # --------------------------------------------------
+    
+    # def _pressure(self, finger: int) -> List[int]:
+    #     """内部：选手指 → 读压力数据"""
+    #     # 选择手指 (保持寄存器 36)
+    #     self._write_register(36, finger)
+    #     time.sleep(_INTERVAL)
+    #     # 读取压力数据 (输入寄存器 52-122)
+    #     return np.array(self._read_input_registers(52, 71))
+    def _pressure(self, finger: int) -> np.ndarray:
+        """
+        6x12 (72点) 矩阵尺寸。
+        Modbus 地址 60/62。
+        """
+        rows = 12  # 12 行
+        cols = 6   # 6 列
+        finger_size = rows * cols  # 72 个数据点
+        
+        # modbus 地址和计数
+        write_address = 60  # 写入手指选择
+        read_address = 62   # 读取压力数据
+        read_count = 96     # 读取 96 个寄存器
+        skip_count = 10     # 跳过前 10 个校验点
+        
+        # 0. 参数校验和手指写入值确定
+        if finger < 1 or finger > 5:
+            raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
+            
+        finger_write_value = finger 
+        
+        # 1. 写入手指选择寄存器 (地址 60)
+        time.sleep(0.008)
+        wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
+        if wrsp.isError():
+            raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
+
+        # 写入后等待片刻
+        time.sleep(0.008) 
+        
+        # 2. 读取地址 62 的数据
+        rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
+        
+        if rrsp.isError():
+            raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
+            
+        registers_16bit: List[int] = rrsp.registers 
+        
+        # 3. 核心数据处理
+        # a. 提取低 8 位数据 (得到 96 个 8 位数据点)
+        final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
+        
+        # b. 跳过前 10 个校验/头部数据点 (得到 86 个有效数据点)
+        effective_data = np.array(final_data_96[skip_count:], dtype=np.uint8)
+        # c. 截取当前手指的矩阵数据 (从 86 个有效点中截取 72 个点)
+        start_idx = 0 
+        end_idx = finger_size  # 72
+        
+        finger_data_flat = effective_data[start_idx:end_idx]
+        
+        # d. 验证数据长度
+        if finger_data_flat.size != finger_size:
+            raise ValueError(
+                f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
+                f"但仅截取到 {finger_data_flat.size} 点。请检查协议，确认地址 62 是否一次性返回了所有手指数据。"
+            )
+            
+        # e. 重塑为二维矩阵 (12 行 6 列)
+        finger_matrix = finger_data_flat.reshape((rows, cols))
+                
+        return finger_matrix
+
+    def read_pressure_thumb(self) -> np.ndarray:
+        """读取大拇指压力数据"""
+        return np.array(self._pressure(1), dtype=np.uint8)
+
+    def read_pressure_index(self) -> np.ndarray:
+        """读取食指压力数据"""
+        return np.array(self._pressure(2), dtype=np.uint8)
+
+    def read_pressure_middle(self) -> np.ndarray:
+        """读取中指压力数据"""
+        return np.array(self._pressure(3), dtype=np.uint8)
+
+    def read_pressure_ring(self) -> np.ndarray:
+        """读取无名指压力数据"""
+        return np.array(self._pressure(4), dtype=np.uint8)
+
+    def read_pressure_little(self) -> np.ndarray:
+        """读取小拇指压力数据"""
+        return np.array(self._pressure(5), dtype=np.uint8)
+
+    # --------------------------------------------------
+    # 版本信息接口
+    # --------------------------------------------------
+    
+    def read_versions(self) -> Dict[str, int]:
+        """读取版本信息 (输入寄存器 148-155)"""
+        result = self._read_input_registers(148, 8)
+        
+        return {
+            "hand_freedom": result[0],
+            "hand_version": result[1],
+            "hand_number": result[2],
+            "hand_direction": result[3],
+            "software_version_major": result[4],
+            "software_version_minor": result[5] if len(result) > 5 else 0,
+            "software_version_revision": result[6] if len(result) > 6 else 0,
+            "hardware_version": result[7] if len(result) > 7 else 0
+        }
+
+    # --------------------------------------------------
+    # 写入接口
+    # --------------------------------------------------
+    
+    def write_angles(self, vals: List[int]):
+        """设置6个关节角度 (保持寄存器 0-5)"""
+        vals = [int(x) for x in vals]
+        if not self.is_valid_6xuint8(vals):
+            raise ValueError("需要6个0-255的整数")
+        self._write_registers(0, vals)
+
+    def write_torques(self, vals: List[int]):
+        """设置6个关节转矩 (保持寄存器 6-11)"""
+        vals = [int(x) for x in vals]
+        if not self.is_valid_6xuint8(vals):
+            raise ValueError("需要6个0-255的整数")
+        self._write_registers(6, vals)
+
+    def write_speeds(self, vals: List[int]):
+        """设置6个关节速度 (保持寄存器 12-17)"""
+        vals = [int(x) for x in vals]
+        if not self.is_valid_6xuint8(vals):
+            raise ValueError("需要6个0-255的整数")
+        self._write_registers(12, vals)
+
+    # --------------------------------------------------
+    # 上下文管理
+    # --------------------------------------------------
+    
+    def close(self):
+        """关闭连接"""
+        if self.connected:
+            self.cli.close()
+            self.connected = False
+    
+    def __enter__(self):
+        return self
+    
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        self.close()
+
+    # --------------------------------------------------
+    # API固定接口函数
+    # --------------------------------------------------
+    
+    def is_valid_6xuint8(self, lst) -> bool:
+        """验证6个0-255的整数列表"""
+        if len(lst) != 6:
+            return False
+        return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
+    
+    def set_joint_positions(self, joint_angles=None):
+        """设置关节位置"""
+        joint_angles = joint_angles or [0] * 6
+        self.write_angles(joint_angles)
+
+    def set_speed(self, speed=None):
+        """设置速度"""
+        speed = speed or [200] * 6
+        self.write_speeds(speed)
+    
+    def set_torque(self, torque=None):
+        """设置扭矩"""
+        torque = torque or [200] * 6
+        self.write_torques(torque)
+
+    def set_current(self, current=None):
+        """设置电流 (L6不支持)"""
+        print("当前L6不支持设置电流", flush=True)
+
+    def get_version(self) -> list:
+        """获取版本信息"""
+        versions = self.read_versions()
+        return [
+            versions.get("hand_freedom", 0),
+            versions.get("hand_version", 0),
+            versions.get("hand_number", 0),
+            versions.get("hand_direction", 0),
+            versions.get("software_version_major", 0),
+            versions.get("hardware_version", 0)
+        ]
+
+    def get_current(self):
+        """获取电流 (L6不支持)"""
+        print("当前L6不支持获取电流", flush=True)
+        return []
+
+    def get_state(self) -> list:
+        """获取关节状态"""
+        return self.read_angles()
+    
+    def get_state_for_pub(self) -> list:
+        return self.get_state()
+
+    def get_current_status(self) -> list:
+        return self.get_state()
+    
+    def get_speed(self) -> list:
+        """获取当前速度"""
+        return self.read_speeds()
+    
+    def get_joint_speed(self) -> list:
+        return self.get_speed()
+    
+    def get_touch_type(self) -> int:
+        """获取压感类型 (2=矩阵式)"""
+        return 2
+    
+    def get_normal_force(self) -> list:
+        """获取压感数据：点式"""
+        return [-1] * 5
+    
+    def get_tangential_force(self) -> list:
+        """获取压感数据：点式"""
+        return [-1] * 5
+    
+    def get_approach_inc(self) -> list:
+        """获取压感数据：点式"""
+        return [-1] * 5
+    
+    def get_touch(self) -> list:
+        return [-1] * 5
+
+    def get_thumb_matrix_touch(self,sleep_time=0):
+        return self._pressure(1)
+
+    def get_index_matrix_touch(self,sleep_time=0):
+        return self._pressure(2)
+
+    def get_middle_matrix_touch(self,sleep_time=0):
+        return self._pressure(3)
+
+    def get_ring_matrix_touch(self,sleep_time=0):
+        return self._pressure(4)
+
+    def get_little_matrix_touch(self,sleep_time=0):
+        return self._pressure(5)
+        
+    def get_matrix_touch(self) -> list:
+        """获取压感数据：矩阵式"""
+        return [self._pressure(1), self._pressure(2), self._pressure(3), 
+                self._pressure(4), self._pressure(5)]
+    
+    def get_matrix_touch_v2(self) -> list:
+        """获取压感数据：矩阵式"""
+        return self.get_matrix_touch()
+    
+    def get_torque(self) -> list:
+        """获取当前扭矩"""
+        return self.read_torques()
+    
+    def get_temperature(self) -> list:
+        """获取当前电机温度"""
+        return self.read_temperatures()
+    
+    def get_fault(self) -> list:
+        """获取当前电机故障码"""
+        return self.read_error_codes()
+    
+    def get_serial_number(self):
+        return [0] * 6
+
+    # --------------------------------------------------
+    # 便捷方法
+    # --------------------------------------------------
+    
+    def relax(self):
+        """所有手指伸直"""
+        self.set_joint_positions([255] * 6)
+    
+    def fist(self):
+        """所有手指握拳"""
+        self.set_joint_positions([0] * 6)
+    
+    def dump_status(self):
+        """打印状态信息"""
+        print("=" * 50)
+        print("L6机械手状态信息")
+        print("=" * 50)
+        
+        try:
+            # 关节状态
+            angles = self.read_angles()
+            torques = self.read_torques()
+            speeds = self.read_speeds()
+            temps = self.read_temperatures()
+            errors = self.read_error_codes()
+            
+            print("关节状态:")
+            for i, name in enumerate(self.JOINT_NAMES):
+                print(f"  {name:15s}: 角度={angles[i]:3d}, 扭矩={torques[i]:3d}, "
+                      f"速度={speeds[i]:3d}, 温度={temps[i]:2d}℃, 错误={errors[i]:2d}")
+            
+            # 版本信息
+            versions = self.read_versions()
+            print("\n版本信息:")
+            for key, value in versions.items():
+                print(f"  {key:20s}: {value}")
+            
+            # 压力传感器测试
+            print("\n压力传感器测试:")
+            thumb_pressure = self.read_pressure_thumb()
+            print(f"大拇指压力数据长度: {len(thumb_pressure)}")
+            
+        except Exception as e:
+            print(f"读取状态时出错: {e}")
+        
+        print("=" * 50)
+
+
+# ------------------- 演示程序 -------------------
+if __name__ == "__main__":
+    # 使用示例
+    try:
+        with LinkerHandL6RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
+            print("连接成功!")
+            
+            # 打印状态信息
+            hand.dump_status()
+            
+            # 测试基本控制
+            print("\n测试控制功能...")
+            print("伸直手指...")
+            hand.relax()
+            time.sleep(2)
+            
+            print("握拳...")
+            hand.fist()
+            time.sleep(2)
+            
+            print("恢复伸直...")
+            hand.relax()
+            
+            # 测试压力传感器
+            print("\n测试压力传感器...")
+            thumb_matrix = hand.get_thumb_matrix_touch()
+            print(f"大拇指压力数据: {len(thumb_matrix)}个点")
+            
+            # 获取所有手指压力数据
+            all_matrices = hand.get_matrix_touch()
+            for i, name in enumerate(hand.FINGER_NAMES):
+                matrix = all_matrices[i]
+                print(f"{name}手指压力数据长度: {len(matrix)}")
+            
+    except Exception as e:
+        print(f"错误: {e}")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l7_rs485.py

@@ -0,0 +1,423 @@
+#!/usr/bin/env python3
+import time
+from typing import List, Dict, Union
+import numpy as np
+from pymodbus.client import ModbusSerialClient
+from pymodbus.exceptions import ModbusException
+
+# --- 协议常量和寄存器地址定义 (根据 O7 协议文件) ---
+
+# RS485 通信设置
+DEFAULT_BAUDRATE = 115200
+
+# O7机械手七个可控关节的键名 (根据保持寄存器和输入寄存器地址 0-6)
+O7_JOINT_KEYS = [
+    "Thumb_Pitch", "Thumb_Yaw", "Index_Pitch", "Middle_Pitch",
+    "Ring_Pitch", "Little_Pitch", "Thumb_Roll"
+]
+
+# 保持寄存器地址 (写操作 FC 16)
+HR_ADDR = {
+    "Position_Start": 0,    # 关节目标位置 (7 个寄存器: 0-6)
+    "Torque_Start": 7,      # 关节目标转矩 (7 个寄存器: 7-13)
+    "Speed_Start": 14,      # 关节目标速度 (7 个寄存器: 14-20)
+    "Pressure_Select": 42   # 压力传感器数据选择 (1 个寄存器)
+    # 21-41 为堵转保护阈值、时间和扭矩，暂未实现
+}
+
+# 输入寄存器地址 (读操作 FC 04)
+IR_ADDR = {
+    "Current_Position_Start": 0,    # 当前关节位置 (7 个寄存器: 0-6)
+    "Current_Torque_Start": 7,      # 当前关节转矩 (7 个寄存器: 7-13)
+    "Current_Speed_Start": 14,      # 当前关节速度 (7 个寄存器: 14-20)
+    "Current_Temperature_Start": 21, # 当前关节温度 (7 个寄存器: 21-27)
+    "Error_Code_Start": 28,         # 当前关节错误码 (7 个寄存器: 28-34)
+    "Tip_Force_Start": 35,          # 指尖力数据 (20 个寄存器: 35-54)
+    "Pressure_Data_Start": 57,      # 压力传感器数据起始 (96 个寄存器: 57-152)
+    "Version_Start": 153            # 版本信息 (6 个寄存器: 153-158)
+}
+
+# 辅助常量
+_JOINT_COUNT = 7
+_VERSION_COUNT = 6
+_TIP_FORCE_COUNT = 20
+_PRESSURE_REG_COUNT = 96 
+_PRESSURE_ROWS = 12 # 从 IR 56 (0xC6) 推断
+_PRESSURE_COLS = 6  # 从 IR 56 (0xC6) 推断
+_PRESSURE_DATA_SIZE = _PRESSURE_ROWS * _PRESSURE_COLS # 72
+_PRESSURE_HEADER_SKIP = 10 # 假设跳过 10 个头部/校验字节
+
+# 通信间隔时间 (使用 L10 参考中的 5ms)
+_INTERVAL = 0.005 
+
+
+class LinkerHandL7RS485:
+    """
+    O7机械手 Modbus RTU (RS485) 控制类。
+    使用 pymodbus 3.5.1 版本和 O7 机械手协议。
+    """
+    def __init__(self, 
+                 hand_id: int = 0x27, 
+                 modbus_port: str = "/dev/ttyUSB0", 
+                 baudrate: int = DEFAULT_BAUDRATE,
+                 timeout: float = 0.05):
+        """
+        初始化 Modbus 客户端。
+
+        :param hand_id: Modbus 从站地址 (0x27: 右手, 0x28: 左手)
+        :param modbus_port: 串口名称
+        :param baudrate: 波特率 (默认为 115200)
+        :param timeout: 通信超时时间 (秒)
+        """
+        self.slave = hand_id
+        self.cli = ModbusSerialClient(
+            port=modbus_port,
+            baudrate=baudrate,
+            bytesize=8,
+            parity="N",
+            stopbits=1,           # 确保与 pymodbus 3.x 兼容的写法
+            timeout=timeout,
+            retries=3,            # 重试次数
+            retry_on_empty=True,
+            handle_local_echo=False,
+            method='rtu'
+        )
+
+        # 尝试连接
+        self.connected = self.cli.connect()
+        if not self.connected:
+            raise ConnectionError(f"RS485 connect fail to {modbus_port} with ID {hex(hand_id)}.")
+        
+        print(f"O7机械手 Modbus ID {hex(hand_id)} 连接成功到 {modbus_port}。")
+
+
+    # --------------------------------------------------
+    # 核心读写函数 (基于 pymodbus 3.5.1)
+    # --------------------------------------------------
+
+    def _read_input_registers(self, address: int, count: int) -> List[int]:
+        """封装 Modbus 读取输入寄存器 (FC 04) 操作。"""
+        time.sleep(_INTERVAL)
+        try:
+            rsp = self.cli.read_input_registers(
+                address=address, 
+                count=count, 
+                slave=self.slave
+            )
+            
+            # 使用 L10 参考中验证过的 3.x 兼容错误检查
+            if rsp.isError():
+                raise RuntimeError(f"Modbus FC04 读取失败。地址: {address}, 错误: {rsp}")
+                
+            return rsp.registers
+            
+        except ModbusException as e:
+            # 捕获通信超时、CRC 错误等 Modbus 异常
+            raise RuntimeError(f"Modbus 通信异常。地址: {address}, 错误: {e}")
+        except Exception as e:
+            raise RuntimeError(f"未知读取异常。地址: {address}, 错误: {e}")
+
+    def _write_holding_registers(self, address: int, values: List[int]):
+        """封装 Modbus 写入保持寄存器 (FC 16) 操作。"""
+        time.sleep(_INTERVAL)
+        
+        # 批量写入 (FC 16)
+        if len(values) > 1:
+            write_func = self.cli.write_registers
+        # 单个写入 (FC 06)
+        elif len(values) == 1:
+            write_func = lambda address, values, slave: self.cli.write_register(address, values[0], slave)
+        else:
+             raise ValueError("写入值列表不能为空。")
+
+        try:
+            rsp = write_func(
+                address=address, 
+                values=values, 
+                slave=self.slave
+            )
+            
+            if rsp.isError():
+                raise RuntimeError(f"Modbus FC16 写入失败。地址: {address}, 错误: {rsp}")
+                
+        except ModbusException as e:
+            raise RuntimeError(f"Modbus 通信异常。地址: {address}, 错误: {e}")
+        except Exception as e:
+            raise RuntimeError(f"未知写入异常。地址: {address}, 错误: {e}")
+
+    # --------------------------------------------------
+    # 读操作 (Read API)
+    # --------------------------------------------------
+    
+    def get_joint_positions(self) -> Dict[str, int]:
+        """读取当前关节位置 (地址 0-6)。"""
+        registers = self._read_input_registers(IR_ADDR["Current_Position_Start"], _JOINT_COUNT)
+        #return dict(zip(O7_JOINT_KEYS, registers))
+        return registers
+
+    def get_current_torques(self) -> Dict[str, int]:
+        """读取当前关节转矩 (地址 7-13)。"""
+        registers = self._read_input_registers(IR_ADDR["Current_Torque_Start"], _JOINT_COUNT)
+        #return dict(zip(O7_JOINT_KEYS, registers))
+        return registers
+
+    def get_current_speeds(self) -> Dict[str, int]:
+        """读取当前关节速度 (地址 14-20)。"""
+        registers = self._read_input_registers(IR_ADDR["Current_Speed_Start"], _JOINT_COUNT)
+        #return dict(zip(O7_JOINT_KEYS, registers))
+        return registers
+
+    def get_temperatures(self) -> Dict[str, int]:
+        """读取当前关节温度 (地址 21-27)。"""
+        registers = self._read_input_registers(IR_ADDR["Current_Temperature_Start"], _JOINT_COUNT)
+        #return dict(zip(O7_JOINT_KEYS, registers))
+        return registers
+
+    def get_error_codes(self) -> Dict[str, int]:
+        """读取当前关节错误码 (地址 28-34)。"""
+        registers = self._read_input_registers(IR_ADDR["Error_Code_Start"], _JOINT_COUNT)
+        #return dict(zip(O7_JOINT_KEYS, registers))
+        return registers
+
+    def get_tip_forces(self) -> List[int]:
+        """读取指尖法向力、切向力等数据 (地址 35-54)。"""
+        return self._read_input_registers(IR_ADDR["Tip_Force_Start"], _TIP_FORCE_COUNT)
+        
+    def get_version(self) -> List[int]:
+        """读取版本信息 (地址 153-158)。"""
+        return self._read_input_registers(IR_ADDR["Version_Start"], _VERSION_COUNT)
+
+    def get_pressure_matrix(self, finger_id: int) -> np.ndarray:
+        """
+        读取特定手指的压力传感器数据矩阵。
+        
+        :param finger_id: 手指编号 (1: 大拇指, 2: 食指, 3: 中指, 4: 无名指, 5: 小拇指)
+        :return: 12x6 的压力数据矩阵 (np.ndarray)
+        """
+        if not (1 <= finger_id <= 5):
+            raise ValueError(f"无效的手指编号: {finger_id}。应在 1 到 5 之间。")
+            
+        # 1. 写入手指选择寄存器 (HR 42)
+        # 使用单个写入 (FC 06)
+        self._write_holding_registers(HR_ADDR["Pressure_Select"], [finger_id])
+
+        # 2. 读取压力传感器数据 (IR 57, 96 个寄存器)
+        time.sleep(_INTERVAL) # 等待数据更新
+        registers_16bit: List[int] = self._read_input_registers(
+            IR_ADDR["Pressure_Data_Start"], 
+            _PRESSURE_REG_COUNT
+        )
+        
+        # 3. 数据解析 (假设与 L10 类似的数据格式: 低 8 位有效，有头部数据)
+        
+        # a. 提取低 8 位数据 (得到 96 个 8 位数据点)
+        final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
+        
+        # b. 跳过头部数据点
+        effective_data = np.array(final_data_96[_PRESSURE_HEADER_SKIP:], dtype=np.uint8)
+        
+        # c. 截取当前手指的矩阵数据 (72 个点)
+        finger_data_flat = effective_data[:_PRESSURE_DATA_SIZE]
+        
+        if finger_data_flat.size != _PRESSURE_DATA_SIZE:
+             raise ValueError(
+                f"压力数据提取失败。期望 {_PRESSURE_DATA_SIZE} 点，"
+                f"但仅截取到 {finger_data_flat.size} 点。请检查协议解析逻辑。"
+            )
+            
+        # d. 重塑为二维矩阵 (12 行 6 列)
+        finger_matrix = finger_data_flat.reshape((_PRESSURE_ROWS, _PRESSURE_COLS))
+        return finger_matrix
+
+
+    # --------------------------------------------------
+    # 写操作 (Write API)
+    # --------------------------------------------------
+
+    def set_joint_positions(self, joint_angles: List[int]):
+        """
+        设置所有 7 个关节的目标位置 (地址 0-6)。
+        :param joint_angles: 7 个 0-255 的整数值列表
+        """
+        if len(joint_angles) != _JOINT_COUNT:
+            raise ValueError(f"需要 {_JOINT_COUNT} 个关节位置值，提供了 {len(joint_angles)} 个。")
+        self._write_holding_registers(HR_ADDR["Position_Start"], joint_angles)
+
+    def set_torques(self, torques: List[int]):
+        """
+        设置所有 7 个关节的目标转矩 (地址 7-13)。
+        :param torques: 7 个 0-255 的整数值列表
+        """
+        if len(torques) != _JOINT_COUNT:
+            raise ValueError(f"需要 {_JOINT_COUNT} 个关节转矩值，提供了 {len(torques)} 个。")
+        self._write_holding_registers(HR_ADDR["Torque_Start"], torques)
+
+
+    def set_speeds(self, speeds: List[int]):
+        """
+        设置所有 7 个关节的目标速度 (地址 14-20)。
+        :param speeds: 7 个 0-255 的整数值列表
+        """
+        if len(speeds) != _JOINT_COUNT:
+            raise ValueError(f"需要 {_JOINT_COUNT} 个关节速度值，提供了 {len(speeds)} 个。")
+        self._write_holding_registers(HR_ADDR["Speed_Start"], speeds)
+    
+    def set_speed(self, speed:List[int] = [200] * 7):
+        self.set_speeds(speed)
+
+    def set_torque(self, torque: List[int] = [250] * 7):
+        self.set_torques(torque)
+
+    def set_current(self, current=None):
+        print("当前L7不支持设置电流", flush=True)
+
+    def get_current(self):
+        #print("当前L7不支持获取电流", flush=True)
+        return [-1] * 7
+
+    def get_state(self) -> List[int]:
+        return self.get_joint_positions()
+    
+
+    def get_state_for_pub(self) -> List[int]:
+        return self.get_joint_positions()
+
+    def get_current_status(self) -> List[int]:
+        return self.get_joint_positions()
+
+    def get_speed(self) -> List[int]:
+        return self.get_current_speeds()
+
+    def get_joint_speed(self) -> List[int]:
+        return self.get_speed()
+
+    def get_touch_type(self) -> int:
+        return 2
+
+    def get_normal_force(self) -> List[int]:
+        return [-1] * 5
+
+    def get_tangential_force(self) -> List[int]:
+        return [-1] * 5
+
+    def get_approach_inc(self) -> List[int]:
+        return [-1] * 5
+
+    def get_touch(self) -> List[int]:
+        return [-1] * 5
+
+    def get_thumb_matrix_touch(self,sleep_time=0):
+        return self.get_pressure_matrix(finger_id=1)
+
+    def get_index_matrix_touch(self,sleep_time=0):
+        return self.get_pressure_matrix(finger_id=2)
+
+    def get_middle_matrix_touch(self,sleep_time=0):
+        return self.get_pressure_matrix(finger_id=3)
+
+    def get_ring_matrix_touch(self,sleep_time=0):
+        return self.get_pressure_matrix(finger_id=4)
+
+    def get_little_matrix_touch(self,sleep_time=0):
+        return self.get_pressure_matrix(finger_id=5)
+
+    def get_matrix_touch(self) -> List[List[int]]:
+        return self.get_thumb_matrix_touch(),self.get_index_matrix_touch(), self.get_middle_matrix_touch(), self.get_ring_matrix_touch(), self.get_little_matrix_touch()
+
+    def get_matrix_touch_v2(self) -> List[List[int]]:
+        return self.get_matrix_touch()
+
+    def get_torque(self) -> List[int]:
+        return self.get_current_torques()
+
+    def get_temperature(self) -> List[int]:
+        return self.get_temperatures()
+
+    def get_fault(self) -> List[int]:
+        return self.get_error_codes()
+
+    def get_serial_number(self):
+        return [0] * 6
+    
+    def get_finger_order(self):
+        return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", "thumb_cmc_roll"]
+    
+    def show_fun_table(self):
+        pass
+
+    def clear_faults(self):
+        pass
+    # --------------------------------------------------
+    # 上下文管理
+    # --------------------------------------------------
+
+    def close(self):
+        """断开 Modbus 连接。"""
+        if self.connected:
+            self.cli.close()
+            self.connected = False
+            print("Modbus 连接已断开。")
+
+    def __enter__(self):
+        return self
+
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        self.close()
+
+
+# ------------------- Demo/使用示例 -------------------
+if __name__ == "__main__":
+    # --- 配置区域 ---
+    # 右手 Modbus ID: 0x27 (39)
+    # 左手 Modbus ID: 0x28 (40)
+    TARGET_HAND_ID = 0x28  # <--- 请根据需要修改为 0x27 或 0x28
+    PORT = "/dev/ttyUSB0"   # <--- 请修改为您的实际串口，例如 'COM3'
+    
+    try:
+        # 使用上下文管理器，确保连接自动关闭
+        with LinkerHandL7RS485(hand_id=TARGET_HAND_ID, modbus_port=PORT) as hand:
+            print("\n--- 1. 读取当前状态 ---")
+            
+            # 读取当前关节位置、速度、转矩
+            angles = hand.get_joint_positions()
+            print(f"当前关节位置 (7DOF): {angles}")
+            
+            speeds = hand.get_current_speeds()
+            print(f"当前关节速度: {speeds}")
+            
+            # 读取传感器和错误信息
+            temps = hand.get_temperatures()
+            print(f"关节温度: {temps}")
+            
+            errors = hand.get_error_codes()
+            print(f"关节错误码: {errors}")
+            
+            # 读取版本
+            version_info = hand.get_version()
+            print(f"版本信息 (Hand_freedom, ..., hardware_version): {version_info}")
+            
+            # --- 2. 写入指令示例 ---
+            print("\n--- 2. 写入指令示例 (设置所有关节到 128) ---")
+            
+            # 假设要将所有关节位置设置到中间值 128
+            target_angles = [128] * _JOINT_COUNT
+            hand.set_joint_positions(target_angles)
+            print(f"写入目标角度: {target_angles}")
+            
+            # 假设要设置所有关节的速度到 100
+            target_speeds = [100] * _JOINT_COUNT
+            hand.set_speeds(target_speeds)
+            print(f"写入目标速度: {target_speeds}")
+
+            # --- 3. 压力传感器读取示例 ---
+            print("\n--- 3. 压力传感器读取 (大拇指 1) ---")
+            thumb_matrix = hand.get_pressure_matrix(finger_id=1)
+            print(f"大拇指压力矩阵 (12x6):")
+            print(thumb_matrix)
+            
+    except ConnectionError as e:
+        print(f"致命错误: 连接失败。{e}")
+    except RuntimeError as e:
+        print(f"致命错误: Modbus 操作失败。{e}")
+    except Exception as e:
+        print(f"捕获到未知异常: {e}")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py

@@ -0,0 +1,671 @@
+#!/usr/bin/env python3
+"""
+O6 机械手 Modbus-RTU 控制类 (基于 pymodbus 3.5.1)
+"""
+
+import os
+import time
+from typing import List, Dict, Any # 引入 Any 来表示灵活的输入类型
+import numpy as np
+import logging
+from threading import Lock # 用于线程安全和总线仲裁
+
+# 导入 pymodbus 客户端
+from pymodbus.client import ModbusSerialClient
+from struct import error as StructError 
+
+logging.basicConfig(
+    level=logging.INFO,
+    format="[%(asctime)s] %(levelname)-8s %(message)s",
+    datefmt="%H:%M:%S"
+)
+
+# ------------------------------------------------------------------
+# 读输入寄存器地址枚举（功能码 04，只读）- 按照 O6 协议文档定义
+# ------------------------------------------------------------------
+REG_RD_CURRENT_THUMB_PITCH      = 0   # 大拇指弯曲角度（0-255，小=弯，大=伸）
+REG_RD_CURRENT_THUMB_YAW        = 1   # 大拇指横摆角度（0-255，小=靠掌心，大=远离）
+REG_RD_CURRENT_INDEX_PITCH      = 2   # 食指弯曲角度
+REG_RD_CURRENT_MIDDLE_PITCH     = 3   # 中指弯曲角度
+REG_RD_CURRENT_RING_PITCH       = 4   # 无名指弯曲角度
+REG_RD_CURRENT_LITTLE_PITCH     = 5   # 小拇指弯曲角度
+REG_RD_CURRENT_THUMB_TORQUE     = 6   # 大拇指弯曲转矩（0-255）
+REG_RD_CURRENT_THUMB_YAW_TORQUE = 7   # 大拇指横摆转矩
+REG_RD_CURRENT_INDEX_TORQUE     = 8   # 食指转矩
+REG_RD_CURRENT_MIDDLE_TORQUE    = 9   # 中指转矩
+REG_RD_CURRENT_RING_TORQUE      = 10  # 无名指转矩
+REG_RD_CURRENT_LITTLE_TORQUE    = 11  # 小拇指转矩
+REG_RD_CURRENT_THUMB_SPEED      = 12  # 大拇指弯曲速度（0-255）
+REG_RD_CURRENT_THUMB_YAW_SPEED  = 13  # 大拇指横摆速度
+REG_RD_CURRENT_INDEX_SPEED      = 14  # 食指速度
+REG_RD_CURRENT_MIDDLE_SPEED     = 15  # 中指速度
+REG_RD_CURRENT_RING_SPEED       = 16  # 无名指速度
+REG_RD_CURRENT_LITTLE_SPEED     = 17  # 小拇指速度
+REG_RD_THUMB_TEMP               = 18  # 大拇指弯曲温度（0-70℃）
+REG_RD_THUMB_YAW_TEMP           = 19  # 大拇指横摆温度
+REG_RD_INDEX_TEMP               = 20  # 食指温度
+REG_RD_MIDDLE_TEMP              = 21  # 中指温度
+REG_RD_RING_TEMP                = 22  # 无名指温度
+REG_RD_LITTLE_TEMP              = 23  # 小拇指温度
+REG_RD_THUMB_ERROR              = 24  # 大拇指错误码
+REG_RD_THUMB_YAW_ERROR          = 25  # 大拇指横摆错误码
+REG_RD_INDEX_ERROR              = 26  # 食指错误码
+REG_RD_MIDDLE_ERROR             = 27  # 中指错误码
+REG_RD_RING_ERROR               = 28  # 无名指错误码
+REG_RD_LITTLE_ERROR             = 29  # 小拇指错误码
+
+# 版本号/设备编号寄存器（地址 30-44，共15个寄存器）
+REG_RD_HAND_FREEDOM             = 30  # Hand_freedom - 设备编号 / 自由度（与机械手上标签相同）
+REG_RD_HAND_VERSION             = 31  # hand_version - 手版本
+REG_RD_HAND_NUMBER_HIGH         = 32  # hand_number_高位 - 设备编号（高字节）
+REG_RD_HAND_NUMBER_MID          = 33  # hand_number_中位 - 设备编号（中字节）
+REG_RD_HAND_NUMBER_LOW          = 34  # hand_number_低位 - 设备编号（低字节）
+REG_RD_HAND_DIRECTION           = 35  # hand_direction - 手方向（左/右）
+REG_RD_HARDWARE_VERSION_HIGH    = 36  # hardware_version_高位 - 硬件版本（高字节）
+REG_RD_HARDWARE_VERSION_MID     = 37  # hardware_version_中位 - 硬件版本（中字节）
+REG_RD_HARDWARE_VERSION_LOW     = 38  # hardware_version_低位 - 硬件版本（低字节）
+REG_RD_SOFTWARE_VERSION_HIGH    = 39  # software_version_高位 - 软件版本（高字节）
+REG_RD_SOFTWARE_VERSION_MID     = 40  # software_version_中位 - 软件版本（中字节）
+REG_RD_SOFTWARE_VERSION_LOW     = 41  # software_version_低位 - 软件版本（低字节）
+REG_RD_MECHANICAL_VERSION_HIGH  = 42  # mechanical_version_高位 - 机械版本（高字节）
+REG_RD_MECHANICAL_VERSION_MID   = 43  # mechanical_version_中位 - 机械版本（中字节）
+REG_RD_MECHANICAL_VERSION_LOW   = 44  # mechanical_version_低位 - 机械版本（低字节）
+
+# 力传感器寄存器（地址 45-87+，动态范围）
+REG_RD_PRESSURE_SENSING_ID      = 45  # Pressure_Sensing_ID - 压力传感器ID (0-5)
+REG_RD_PRESSURE_SENSING_SPEC    = 46  # Pressure_Sensing_Specifications - 传感器数据规格
+
+
+# ------------------------------------------------------------------
+# 写保持寄存器地址枚举（功能码 16，读写）- 保持原样
+# ------------------------------------------------------------------
+REG_WR_THUMB_PITCH      = 0   # 大拇指弯曲角度（0-255）
+REG_WR_THUMB_YAW        = 1   # 大拇指横摆角度
+REG_WR_INDEX_PITCH      = 2   # 食指弯曲角度
+REG_WR_MIDDLE_PITCH     = 3   # 中指弯曲角度
+REG_WR_RING_PITCH       = 4   # 无名指弯曲角度
+REG_WR_LITTLE_PITCH     = 5   # 小拇指弯曲角度
+REG_WR_THUMB_TORQUE     = 6   # 大拇指弯曲转矩
+REG_WR_THUMB_YAW_TORQUE = 7   # 大拇指横摆转矩
+REG_WR_INDEX_TORQUE     = 8   # 食指转矩
+REG_WR_MIDDLE_TORQUE    = 9   # 中指转矩
+REG_WR_RING_TORQUE      = 10  # 无名指转矩
+REG_WR_LITTLE_TORQUE    = 11  # 小拇指转矩
+REG_WR_THUMB_SPEED      = 12  # 大拇指弯曲速度
+REG_WR_THUMB_YAW_SPEED  = 13  # 大拇指横摆速度
+REG_WR_INDEX_SPEED      = 14  # 食指速度
+REG_WR_MIDDLE_SPEED     = 15  # 中指速度
+REG_WR_RING_SPEED       = 16  # 无名指速度
+REG_WR_LITTLE_SPEED     = 17  # 小拇指速度
+
+
+class LinkerHandO6RS485:
+    """O6 机械手 Modbus-RTU 控制类，使用 pymodbus 3.5.1"""
+
+    TTL_TIMEOUT = 0.15     # 串口超时
+    FRAME_GAP = 0.030      # 30 ms
+    
+    # KEYS for easy indexing
+    JOINT_KEYS = ["thumb_pitch", "thumb_yaw", "index_pitch", 
+                  "middle_pitch", "ring_pitch", "little_pitch"]
+
+    def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+        self._id = hand_id
+        self._last_ts = 0.0  # 上一次帧结束时间
+        self._lock = Lock()  # 总线访问锁
+
+        # 使用 pymodbus 3.x 客户端
+        self.cli = ModbusSerialClient(
+            port=modbus_port,
+            baudrate=baudrate,
+            bytesize=8,
+            parity="N",
+            stopbits=1,
+            timeout=self.TTL_TIMEOUT,
+            handle_local_echo=False
+        )
+
+        try:
+            logging.info(f"Connecting to Modbus RTU on {modbus_port}...")
+            self.connected = self.cli.connect()
+            if not self.connected:
+                raise ConnectionError(f"RS485 connect fail to {modbus_port}")
+            logging.info("Connection successful.")
+        except Exception as e:
+            logging.error(f"Initialization failed: {e}")
+            raise
+
+    # ----------------------------------------------------------
+    # 辅助方法
+    # ----------------------------------------------------------
+    def _bus_free(self):
+        """保证距离上一帧 ≥ 30 ms"""
+        with self._lock:
+            elapse = time.perf_counter() - self._last_ts
+            if elapse < self.FRAME_GAP:
+                time.sleep(self.FRAME_GAP - elapse)
+
+    def _execute_read(self, address: int, count: int) -> List[int]:
+        """执行 Modbus 读取操作 (功能码 04), 带总线仲裁。"""
+        self._bus_free()
+        
+        rsp = self.cli.read_input_registers(
+            address=address, 
+            count=count, 
+            slave=self._id
+        )
+        
+        self._last_ts = time.perf_counter()
+        
+        if rsp.isError():
+            raise RuntimeError(f"Modbus Read Failed (Addr={address}, Count={count}): {rsp}")
+        
+        # 确保返回的值是 Python 原生整数
+        return [int(reg) for reg in rsp.registers]
+
+    def _execute_write(self, address: int, values: List[int]):
+        """执行 Modbus 批量写入操作 (功能码 16), 带总线仲裁。"""
+        self._bus_free()
+        
+        # values 必须是 Python 原生整数列表
+        rsp = self.cli.write_registers(
+            address=address, 
+            values=values, 
+            slave=self._id
+        )
+        
+        self._last_ts = time.perf_counter()
+        
+        if rsp.isError():
+            raise RuntimeError(f"Modbus Write Failed (Addr={address}, Values={values}): {rsp}")
+
+    # ----------------------------------------------------------
+    # 批量读取和数据封装（优化通信效率）
+    # ----------------------------------------------------------
+    def read_all_angles(self) -> List[int]:
+        return self._execute_read(REG_RD_CURRENT_THUMB_PITCH, 6)
+    
+    def read_all_torques(self) -> List[int]:
+        return self._execute_read(REG_RD_CURRENT_THUMB_TORQUE, 6)
+
+    def read_all_speeds(self) -> List[int]:
+        return self._execute_read(REG_RD_CURRENT_THUMB_SPEED, 6)
+    
+    def read_all_temperatures(self) -> List[int]:
+        return self._execute_read(REG_RD_THUMB_TEMP, 6)
+    
+    def read_all_errors(self) -> List[int]:
+        return self._execute_read(REG_RD_THUMB_ERROR, 6)
+
+    # ----------------------------------------------------------
+    # 版本号/设备编号读取（按照协议文档：地址30-44，共15个寄存器）
+    # ----------------------------------------------------------
+    def read_all_versions(self) -> str:
+        """一次性读取全部15个寄存器 (地址30-44)，返回以 '.' 连接的字符串。
+        
+        按 O6 协议文档的版本号格式返回：
+            hand_freedom.hand_version.hand_number_high.hand_number_mid.hand_number_low
+            .hand_direction.hardware_ver_hardware_ver_m.hardware_ver_l
+            .software_ver_h.software_ver_m.software_ver_l
+            .mechanical_ver_h.mechanical_ver_m.mechanical_ver_l
+        
+        例如: "6.1.001.002.003.0.1.2.3.4.5.6.7.8.9"
+        """
+        raw = self._execute_read(REG_RD_HAND_FREEDOM, 15)
+        return ".".join(str(v) for v in raw)
+
+    # ----------------------------------------------------------
+    # 基于 read_all_versions() 的设备编号解析方法
+    # ----------------------------------------------------------
+    def _parse_versions(self):
+        """解析 read_all_versions() 返回的字符串为字典"""
+        parts = self.read_all_versions().split(".")
+        if len(parts) < 15:
+            return {}
+        return {
+            "hand_freedom":      int(parts[0]),
+            "hand_version":      int(parts[1]),
+            "hand_number_high":  int(parts[2]),
+            "hand_number_mid":   int(parts[3]),
+            "hand_number_low":   int(parts[4]),
+            "hand_direction":    int(parts[5]),
+            "hw_ver_high":       int(parts[6]),
+            "hw_ver_mid":        int(parts[7]),
+            "hw_ver_low":        int(parts[8]),
+            "sw_ver_high":       int(parts[9]),
+            "sw_ver_mid":        int(parts[10]),
+            "sw_ver_low":        int(parts[11]),
+            "mech_ver_high":     int(parts[12]),
+            "mech_ver_mid":      int(parts[13]),
+            "mech_ver_low":      int(parts[14]),
+        }
+
+    def get_device_number(self) -> str:
+        """获取设备编号（与机械手上标签相同）。格式：高位+中位+低位 拼接的字符串。"""
+        v = self._parse_versions()
+        if not v:
+            return "0"
+        high, mid, low = v["hand_number_high"], v["hand_number_mid"], v["hand_number_low"]
+        # 将每个字节格式化为无前导零的整数（与标签显示一致）
+        return f"{high}{mid}{low}"
+
+    def get_device_number_value(self) -> int:
+        """获取设备编号数值"""
+        v = self._parse_versions()
+        if not v:
+            return 0
+        high, mid, low = v["hand_number_high"], v["hand_number_mid"], v["hand_number_low"]
+        return high * 65536 + mid * 256 + low
+
+    def get_hardware_version(self) -> str:
+        """获取硬件版本号。格式：高.中.低"""
+        v = self._parse_versions()
+        if not v:
+            return "0.0.0"
+        return f"{v['hw_ver_high']}.{v['hw_ver_mid']}.{v['hw_ver_low']}"
+
+    def get_software_version(self) -> str:
+        """获取软件版本号。格式：高.中.低"""
+        v = self._parse_versions()
+        if not v:
+            return "0.0.0"
+        return f"{v['sw_ver_high']}.{v['sw_ver_mid']}.{v['sw_ver_low']}"
+
+    def get_mechanical_version(self) -> str:
+        """获取机械版本号。格式：高.中.低"""
+        v = self._parse_versions()
+        if not v:
+            return "0.0.0"
+        return f"{v['mech_ver_high']}.{v['mech_ver_mid']}.{v['mech_ver_low']}"
+
+    def get_hand_freedom(self) -> int:
+        """获取自由度（与机械手上标签相同）"""
+        v = self._parse_versions()
+        return v.get("hand_freedom", 0)
+
+    def get_hand_version_raw(self) -> int:
+        """获取手版本原始值"""
+        v = self._parse_versions()
+        return v.get("hand_version", 0)
+
+    def get_hand_direction(self) -> str:
+        """获取手方向，转换为字符：76→'L', 82→'R'"""
+        v = self._parse_versions()
+        val = v.get("hand_direction", 0)
+        if val in (76, 82):
+            return chr(val)
+        # fallback: 直接转为字符（如果值在可打印范围内）
+        return chr(val) if 32 < val < 128 else f'Unknown({val})'
+
+
+    # ----------------------------------------------------------
+    # 只读属性（单个寄存器读取）
+    # ----------------------------------------------------------
+    def _read_reg(self, addr: int) -> int:
+        """读单个输入寄存器（功能码 04），带 30 ms 帧间隔"""
+        return self._execute_read(addr, 1)[0]
+    
+    def get_thumb_pitch(self) -> int:       return self._read_reg(REG_RD_CURRENT_THUMB_PITCH)
+    def get_thumb_yaw(self) -> int:         return self._read_reg(REG_RD_CURRENT_THUMB_YAW)
+    def get_index_pitch(self) -> int:       return self._read_reg(REG_RD_CURRENT_INDEX_PITCH)
+    def get_middle_pitch(self) -> int:      return self._read_reg(REG_RD_CURRENT_MIDDLE_PITCH)
+    def get_ring_pitch(self) -> int:        return self._read_reg(REG_RD_CURRENT_RING_PITCH)
+    def get_little_pitch(self) -> int:      return self._read_reg(REG_RD_CURRENT_LITTLE_PITCH)
+
+    def get_thumb_torque(self) -> int:      return self._read_reg(REG_RD_CURRENT_THUMB_TORQUE)
+    def get_thumb_yaw_torque(self) -> int:  return self._read_reg(REG_RD_CURRENT_THUMB_YAW_TORQUE)
+    def get_index_torque(self) -> int:      return self._read_reg(REG_RD_CURRENT_INDEX_TORQUE)
+    def get_middle_torque(self) -> int:     return self._read_reg(REG_RD_CURRENT_MIDDLE_TORQUE)
+    def get_ring_torque(self) -> int:       return self._read_reg(REG_RD_CURRENT_RING_TORQUE)
+    def get_little_torque(self) -> int:     return self._read_reg(REG_RD_CURRENT_LITTLE_TORQUE)
+
+    def get_thumb_speed(self) -> int:       return self._read_reg(REG_RD_CURRENT_THUMB_SPEED)
+    def get_thumb_yaw_speed(self) -> int:   return self._read_reg(REG_RD_CURRENT_THUMB_YAW_SPEED)
+    def get_index_speed(self) -> int:       return self._read_reg(REG_RD_CURRENT_INDEX_SPEED)
+    def get_middle_speed(self) -> int:      return self._read_reg(REG_RD_CURRENT_MIDDLE_SPEED)
+    def get_ring_speed(self) -> int:        return self._read_reg(REG_RD_CURRENT_RING_SPEED)
+    def get_little_speed(self) -> int:      return self._read_reg(REG_RD_CURRENT_LITTLE_SPEED)
+
+    def get_thumb_temp(self) -> int:        return self._read_reg(REG_RD_THUMB_TEMP)
+    def get_thumb_yaw_temp(self) -> int:    return self._read_reg(REG_RD_THUMB_YAW_TEMP)
+    def get_index_temp(self) -> int:        return self._read_reg(REG_RD_INDEX_TEMP)
+    def get_middle_temp(self) -> int:       return self._read_reg(REG_RD_MIDDLE_TEMP)
+    def get_ring_temp(self) -> int:         return self._read_reg(REG_RD_RING_TEMP)
+    def get_little_temp(self) -> int:       return self._read_reg(REG_RD_LITTLE_TEMP)
+
+    def get_thumb_error(self) -> int:       return self._read_reg(REG_RD_THUMB_ERROR)
+    def get_thumb_yaw_error(self) -> int:   return self._read_reg(REG_RD_THUMB_YAW_ERROR)
+    def get_index_error(self) -> int:       return self._read_reg(REG_RD_INDEX_ERROR)
+    def get_middle_error(self) -> int:      return self._read_reg(REG_RD_MIDDLE_ERROR)
+    def get_ring_error(self) -> int:        return self._read_reg(REG_RD_RING_ERROR)
+    def get_little_error(self) -> int:      return self._read_reg(REG_RD_LITTLE_ERROR)
+
+    
+    # ----------------------------------------------------------
+    # 批量 Getter (使用 read_all_... 方法)
+    # ----------------------------------------------------------
+    def get_state(self) -> List[int]:
+        """获取手指电机状态 (角度)"""
+        return self.read_all_angles()
+
+    def get_torque(self) -> List[int]:
+        """获取当前扭矩"""
+        return self.read_all_torques()
+
+    def get_speed(self) -> List[int]:
+        """获取当前速度"""
+        return self.read_all_speeds()
+
+    def get_temperature(self) -> List[int]:
+        """获取当前电机温度"""
+        return self.read_all_temperatures()
+    
+    def get_fault(self) -> List[int]:
+        """获取当前电机故障码"""
+        return self.read_all_errors()
+    
+    def get_version(self) -> str:
+        """获取当前固件版本号（已转换为字符串格式）"""
+        return self.read_all_versions()
+
+
+    # ----------------------------------------------------------
+    # 写保持寄存器 (单个寄存器写入)
+    # ----------------------------------------------------------
+    def _write_reg(self, addr: int, value: int):
+        """写单个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+        if not 0 <= value <= 255:
+            raise ValueError("value must be 0-255")
+        
+        # 确保 value 是 Python 原生 int
+        self._execute_write(addr, [int(value)])
+
+    def _write_regs(self, addr: int, values: List[int]):
+        """写多个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+        # 此时 values 应该已经是经过 is_valid_6xuint8 验证并转换的 Python int 列表
+        if not all(0 <= v <= 255 for v in values):
+            # 这行理论上不应触发，因为上层调用已校验
+            raise ValueError("All values must be 0-255")
+        self._execute_write(addr, values)
+
+
+    def set_thumb_pitch(self, v: int):           self._write_reg(REG_WR_THUMB_PITCH, v)
+    def set_thumb_yaw(self, v: int):             self._write_reg(REG_WR_THUMB_YAW, v)
+    def set_index_pitch(self, v: int):           self._write_reg(REG_WR_INDEX_PITCH, v)
+    def set_middle_pitch(self, v: int):          self._write_reg(REG_WR_MIDDLE_PITCH, v)
+    def set_ring_pitch(self, v: int):            self._write_reg(REG_WR_RING_PITCH, v)
+    def set_little_pitch(self, v: int):          self._write_reg(REG_WR_LITTLE_PITCH, v)
+
+    def set_thumb_torque(self, v: int):          self._write_reg(REG_WR_THUMB_TORQUE, v)
+    def set_thumb_yaw_torque(self, v: int):      self._write_reg(REG_WR_THUMB_YAW_TORQUE, v)
+    def set_index_torque(self, v: int):          self._write_reg(REG_WR_INDEX_TORQUE, v)
+    def set_middle_torque(self, v: int):         self._write_reg(REG_WR_MIDDLE_TORQUE, v)
+    def set_ring_torque(self, v: int):           self._write_reg(REG_WR_RING_TORQUE, v)
+    def set_little_torque(self, v: int):         self._write_reg(REG_WR_LITTLE_TORQUE, v)
+
+    def set_thumb_speed(self, v: int):           self._write_reg(REG_WR_THUMB_SPEED, v)
+    def set_thumb_yaw_speed(self, v: int):       self._write_reg(REG_WR_THUMB_YAW_SPEED, v)
+    def set_index_speed(self, v: int):           self._write_reg(REG_WR_INDEX_SPEED, v)
+    def set_middle_speed(self, v: int):          self._write_reg(REG_WR_MIDDLE_SPEED, v)
+    def set_ring_speed(self, v: int):            self._write_reg(REG_WR_RING_SPEED, v)
+    def set_little_speed(self, v: int):          self._write_reg(REG_WR_LITTLE_SPEED, v)
+
+    # ----------------------------------------------------------
+    # 固定函数 (采用批量写入优化)
+    # ----------------------------------------------------------
+    def is_valid_6xuint8(self, lst: List[Any]) -> bool:
+        """
+        验证6个0-255的整数列表。
+        允许输入包含浮点数、NumPy整数等可转换为 int 的类型，并进行范围校验。
+        """
+        if not (isinstance(lst, list) and len(lst) == 6):
+            return False
+            
+        try:
+            # 关键：尝试将所有元素转换为 Python 原生 int
+            int_values = [int(v) for v in lst]
+        except (ValueError, TypeError):
+            # 转换失败，列表中包含不可转换的元素
+            return False
+
+        # 校验转换后的整数列表是否在 0-255 范围内
+        return all(0 <= x <= 255 for x in int_values)
+    
+    def set_joint_positions(self, joint_angles: List[Any] = None):
+        joint_angles = joint_angles or [0] * 6
+        
+        if not self.is_valid_6xuint8(joint_angles):
+            logging.error(f"Invalid joint angles received: {joint_angles}")
+            raise ValueError("Joint angles must be a list of 6 values between 0 and 255 (convertible to int).")
+
+        # 强制转换为 Modbus 兼容的 Python 原生 int 列表
+        int_angles = [int(v) for v in joint_angles] 
+        
+        # 批量写入 6 个角度寄存器 (从 REG_WR_THUMB_PITCH 地址 0 开始, count=6)
+        self._write_regs(REG_WR_THUMB_PITCH, int_angles)
+
+    def set_speed(self, speed: List[Any] = None):
+        speed = speed or [200] * 6
+        if not self.is_valid_6xuint8(speed):
+            logging.error(f"Invalid speed values received: {speed}")
+            raise ValueError("Speed values must be a list of 6 values between 0 and 255 (convertible to int).")
+        
+        int_speed = [int(v) for v in speed]
+        self._write_regs(REG_WR_THUMB_SPEED, int_speed)
+    
+    def set_torque(self, torque: List[Any] = None):
+        torque = torque or [200] * 6
+        if not self.is_valid_6xuint8(torque):
+            logging.error(f"Invalid torque values received: {torque}")
+            raise ValueError("Torque values must be a list of 6 values between 0 and 255 (convertible to int).")
+            
+        int_torque = [int(v) for v in torque]
+        self._write_regs(REG_WR_THUMB_TORQUE, int_torque)
+
+    # ... (其他固定函数保持不变) ...
+
+    def set_current(self, current: List[int] = None):
+        print("当前O6不支持设置电流", flush=True)
+        pass
+
+    def get_state_for_pub(self) -> list:
+        return self.get_state()
+
+    def get_current_status(self) -> list:
+        return self.get_state()
+    
+    def get_joint_speed(self) -> list:
+        return self.get_speed()
+    
+    def get_touch_type(self) -> list:
+        return -1
+    
+    def get_normal_force(self) -> list:
+        return [-1] * 5
+    
+    def get_tangential_force(self) -> list:
+        return [-1] * 5
+    
+    def get_approach_inc(self) -> list:
+        return [-1] * 5
+    
+    def get_touch(self) -> list:
+        return [-1] * 5
+    
+    def _pressure(self, finger: int) -> np.ndarray:
+        """
+        读取压力传感器数据 (10x4矩阵)
+        """
+        rows = 10      # 10行
+        cols = 4       # 4列
+        finger_size = rows * cols  # 40个数据点
+        
+        # modbus 地址 (按O6协议文档)
+        write_address = 18   # 写入手指选择 (保持寄存器)
+        read_address = 47    # 读取压力数据 (输入寄存器)
+        read_count = 40      # 读取40个寄存器
+        
+        # 0. 参数校验
+        if finger < 1 or finger > 5:
+            raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
+        
+        # 1. 写入手指选择寄存器 (地址18)
+        time.sleep(0.01)
+        self._write_reg(write_address, finger)
+        
+        # 2. 读取压力数据
+        data = self._execute_read(read_address, read_count)
+        
+        # 3. 转换为numpy数组并重塑为10x4矩阵
+        finger_matrix = np.array(data, dtype=np.uint8).reshape((rows, cols))
+        
+        return finger_matrix
+    
+    def get_thumb_matrix_touch(self,sleep_time=0):
+        return np.array(self._pressure(1), dtype=np.uint8)
+
+
+    def get_index_matrix_touch(self,sleep_time=0):
+        return np.array(self._pressure(2), dtype=np.uint8)
+
+    def get_middle_matrix_touch(self,sleep_time=0):
+        return np.array(self._pressure(3), dtype=np.uint8)
+
+    def get_ring_matrix_touch(self,sleep_time=0):
+        return np.array(self._pressure(4), dtype=np.uint8)
+
+    def get_little_matrix_touch(self,sleep_time=0):
+        return np.array(self._pressure(5), dtype=np.uint8)
+
+    def get_matrix_touch(self) -> list:
+        thumb_matrix = np.full((12, 6), -1)
+        index_matrix = np.full((12, 6), -1)
+        middle_matrix = np.full((12, 6), -1)
+        ring_matrix = np.full((12, 6), -1)
+        little_matrix = np.full((12, 6), -1)
+        return thumb_matrix , index_matrix , middle_matrix , ring_matrix , little_matrix
+    
+    def get_serial_number(self):
+
+        return "["+str(self.get_hand_freedom())+"-"+str(self.get_mechanical_version())+"-"+str(self.get_device_number())+"-"+str(self.get_hand_direction())+"]"
+
+    def get_matrix_touch_v2(self) -> list:
+        return self.get_matrix_touch()
+
+    def get_finger_order(self):
+        return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+    
+    def clear_faults(self):
+        pass
+    
+    def close(self):
+        if hasattr(self, 'connected') and self.connected:
+            self.cli.close()
+            self.connected = False
+            logging.info("Modbus connection closed.")
+
+    def __enter__(self):
+        return self
+
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        self.close()
+
+    # ----------------------------------------------------------
+    # 便捷函数
+    # ----------------------------------------------------------
+    def set_all_fingers(self, pitch: int):
+        """同时设置五指弯曲角度（0-255），使用批量写入"""
+        # 允许传入 float/numpy int 等可转换为 int 的类型
+        try:
+            pitch_int = int(pitch)
+        except (ValueError, TypeError):
+             raise ValueError("Pitch value must be a number convertible to int (0-255)")
+
+        if not 0 <= pitch_int <= 255:
+            raise ValueError("Pitch value must be 0-255")
+        
+        # 批量设置所有 6 个关节的角度
+        self.set_joint_positions([pitch_int] * 6)
+
+    def relax(self):
+        """全部手指伸直（255）"""
+        self.set_all_fingers(255)
+
+    def fist(self):
+        """全部手指弯曲（0）"""
+        self.set_all_fingers(0)
+
+    def dump_status(self):
+        """打印当前所有可读状态 (使用批量读取优化)"""
+        print("--------- O6 Hand Status ---------")
+        
+        angles = self.get_state()
+        temps = self.get_temperature()
+        errors = self.get_fault()
+        
+        # 解析版本号字符串
+        v = self._parse_versions()
+
+        if v:
+            device_num_str = f"{v['hand_number_high']}{v['hand_number_mid']}{v['hand_number_low']}"
+            hw_ver = f"{v['hw_ver_high']}.{v['hw_ver_mid']}.{v['hw_ver_low']}"
+            sw_ver = f"{v['sw_ver_high']}.{v['sw_ver_mid']}.{v['sw_ver_low']}"
+            mech_ver = f"{v['mech_ver_high']}.{v['mech_ver_mid']}.{v['mech_ver_low']}"
+            
+            print(f"Device Number:   {device_num_str}")
+            print(f"HWSW Version:    HW={hw_ver} SW={sw_ver}")
+            print(f"Mechanical Ver:  {mech_ver}")
+            print(f"Hand Freedom:    {v['hand_freedom']}")
+            print(f"Full Version:    {self.read_all_versions()}")
+
+        print(f"Joint Angles:    {angles}")
+        print(f"Temperature:     {temps}℃")
+        print(f"Error Codes:     {errors}")
+        print("----------------------------------")
+
+
+# ------------------------------------------------------------------
+# 命令行快速测试
+# ------------------------------------------------------------------
+if __name__ == "__main__":
+    import argparse
+    
+    # 假设默认站号是 0x27 (39)
+    DEFAULT_HAND_ID = 0x27
+
+    parser = argparse.ArgumentParser(description="O6 Hand Modbus tester (using pymodbus 3.5.1)")
+    parser.add_argument("-p", "--port", required=True, help="串口, 如 /dev/ttyUSB0")
+    parser.add_argument("-l", "--left", action="store_const", const=0x28, default=DEFAULT_HAND_ID, dest='hand_id', help="左手 (0x28)，默认右手 (0x27)")
+    
+    args = parser.parse_args()
+
+    try:
+        # 使用 with 语句确保连接关闭，这是 pymodbus 的推荐用法
+        with LinkerHandO6RS485(hand_id=args.hand_id, modbus_port=args.port, baudrate=115200) as hand:
+            hand.dump_status()
+
+            # 测试新增的设备编号读取方法
+            print("\n--- 设备信息 ---")
+            print(f"设备编号（字符串）: {hand.get_device_number()}")
+            print(f"设备编号（数值）:   {hand.get_device_number_value()}")
+            print(f"硬件版本号:         {hand.get_hardware_version()}")
+            print(f"软件版本号:         {hand.get_software_version()}")
+            print(f"机械版本号:         {hand.get_mechanical_version()}")
+
+            print("\n执行 relax → 伸直")
+            hand.relax()
+            time.sleep(1)
+            print("执行 fist → 握拳")
+            hand.fist()
+            time.sleep(1)
+            hand.relax()
+            print("演示完成")
+            
+    except ConnectionError as e:
+        print(f"连接错误: {e}")
+    except RuntimeError as e:
+        print(f"Modbus 运行时错误: {e}")
+    except StructError as e:
+        print(f"数据结构错误 (请检查输入数据类型是否为原生int): {e}")
+    except Exception as e:
+        print(f"发生其他错误: {e}")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py

@@ -0,0 +1,355 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+import sys, os, time,threading
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+from utils.mapping import *
+from utils.color_msg import ColorMsg
+from utils.load_write_yaml import LoadWriteYaml
+from utils.open_can import OpenCan
+
+class LinkerHandApi:
+    def __init__(self, hand_type="left", hand_joint="L10", modbus = "None",can="can0"):  # Ubuntu:can0   win:PCAN_USBBUS1
+        self.last_position = []
+        self.yaml = LoadWriteYaml()
+        self.config = self.yaml.load_setting_yaml()
+        self.version = self.config["VERSION"]
+        self.can = can
+        ColorMsg(msg=f"Current SDK version: {self.version}", color="green")
+        self.hand_joint = hand_joint
+        self.hand_type = hand_type
+        self.is_palm_touch = -1 # 是否为全掌压力传感器
+        if self.hand_type == "left":
+            self.hand_id = 0x28  # Left hand
+        if self.hand_type == "right":
+            self.hand_id = 0x27  # Right hand
+        if self.hand_joint.upper() == "O6":
+            if modbus != "None":
+                from core.rs485.linker_hand_o6_rs485 import LinkerHandO6RS485
+                self.hand = LinkerHandO6RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200)
+            else:
+                from core.can.linker_hand_o6_can import LinkerHandO6Can
+                self.hand = LinkerHandO6Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+        if self.hand_joint == "L6":
+            if modbus != "None":
+                from core.rs485.linker_hand_l6_rs485 import LinkerHandL6RS485
+                self.hand = LinkerHandL6RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200)
+            else:
+                from core.can.linker_hand_l6_can import LinkerHandL6Can
+                self.hand = LinkerHandL6Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+        if self.hand_joint == "L7":
+            if modbus != "None":
+                from core.rs485.linker_hand_l7_rs485 import LinkerHandL7RS485
+                self.hand = LinkerHandL7RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200)
+            else:
+                from core.can.linker_hand_l7_can import LinkerHandL7Can
+                self.hand = LinkerHandL7Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+        if self.hand_joint == "L10":
+            if modbus != "None":
+                from core.rs485.linker_hand_l10_rs485 import LinkerHandL10RS485
+                self.hand = LinkerHandL10RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200)
+            else:
+                from core.can.linker_hand_l10_can import LinkerHandL10Can
+                self.hand = LinkerHandL10Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+        if self.hand_joint == "L20":
+            from core.can.linker_hand_l20_can import LinkerHandL20Can
+            self.hand = LinkerHandL20Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+        if self.hand_joint == "G20":
+            from core.can.linker_hand_g20_can import LinkerHandG20Can
+            self.hand = LinkerHandG20Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+            time.sleep(0.01)
+            self.is_palm_touch = self.hand.get_touch_sensor_type()
+            ColorMsg(msg=f"传感器类型:{self.is_palm_touch}")
+        if self.hand_joint == "L21":
+            from core.can.linker_hand_l21_can import LinkerHandL21Can
+            self.hand = LinkerHandL21Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+        if self.hand_joint == "L25":
+            from core.can.linker_hand_l25_can import LinkerHandL25Can
+            self.hand = LinkerHandL25Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+        # Open can0
+        if sys.platform == "linux" and modbus=="None":
+            self.open_can = OpenCan(load_yaml=self.yaml)
+            self.open_can.open_can(self.can)
+            self.is_can = self.open_can.is_can_up_sysfs(interface=self.can)
+            if not self.is_can:
+                ColorMsg(msg=f"{self.can} interface is not open", color="red")
+                sys.exit(1)
+        version = self.get_embedded_version()
+        self.serial_number = self.get_serial_number()
+        if version == None or len(version) == 0:
+            ColorMsg(msg="Warning: Hardware version number not recognized, it is recommended to terminate the program and re insert USB to CAN conversion", color="yellow")
+        else:
+            ColorMsg(msg=f"Embedded:{version}", color="green")
+        ColorMsg(msg=f"Linker Hand Serial Number: {self.serial_number}", color="green")
+        
+    
+    # Five-finger movement
+    def finger_move(self, pose=[]):
+        '''
+        Five-finger movement
+        @params: pose list L7 len(7) | L10 len(10) | L20 len(20) | L25 len(25) 0~255
+        '''
+        
+        if len(pose) == 0:
+            return
+        pose = [int(v) for v in pose]
+        if any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in pose):
+            ColorMsg(msg=f"The numerical range cannot be less than 0 or greater than 255",color="red")
+            return
+        if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6") and len(pose) == 6:
+            self.hand.set_joint_positions(pose)
+        elif self.hand_joint == "L7" and len(pose) == 7:
+            self.hand.set_joint_positions(pose)
+        elif self.hand_joint == "L10" and len(pose) == 10:
+            self.hand.set_joint_positions(pose)
+        elif self.hand_joint == "L20" and len(pose) == 20:
+            self.hand.set_joint_positions(pose)
+        elif self.hand_joint == "G20" and len(pose) == 20:
+            self.hand.set_joint_positions(pose)
+        elif self.hand_joint == "L21" and len(pose) == 25:
+            self.hand.set_joint_positions(pose)
+        elif self.hand_joint == "L25" and len(pose) == 25:
+            self.hand.set_joint_positions(pose)
+        else:
+            ColorMsg(msg=f"Current LinkerHand is {self.hand_type}{self.hand_joint}, action sequence is {pose}, does not match", color="red")
+        self.last_position = pose
+
+    def _get_normal_force(self):
+        '''# Get normal force'''
+        self.hand.get_normal_force()
+    
+    def _get_tangential_force(self):
+        '''# Get tangential force'''
+        self.hand.get_tangential_force()
+    
+    def _get_tangential_force_dir(self):
+        '''# Get tangential force direction'''
+        self.hand.get_tangential_force_dir()
+    
+    def _get_approach_inc(self):
+        '''# Get approach increment'''
+        self.hand.get_approach_inc()
+    
+
+    def set_speed(self, speed=[100]*5):
+        '''# Set speed'''
+        has_non_int = any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in speed)
+        if has_non_int:
+            print("Set Speed The numerical range can only be positive integers or floating-point numbers between 0 and 255", flush=True)
+            return
+        if len(speed) < 5:
+            print("数据长度不够,至少5个元素", flush=True)
+            return
+        if self.hand_joint == "L7" and len(speed) < 7:
+            print("数据长度不够,至少7个元素", flush=True)
+            return
+        ColorMsg(msg=f"{self.hand_type} {self.hand_joint} set speed to {speed}", color="green")
+        self.hand.set_speed(speed=speed)
+    
+    def set_joint_speed(self, speed=[100]*5):
+        '''Set speed by topic'''
+        if len(speed) == 0:
+            return
+        if any(not isinstance(x, (int, float)) or x < 10 or x > 255 for x in speed):
+            ColorMsg(msg=f"The numerical range cannot be less than 10 or greater than 255",color="red")
+            return
+        self.hand.set_speed(speed=speed)
+    
+    def set_torque(self, torque=[180] * 5):
+        '''Set maximum torque'''
+        has_non_int = any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in torque)
+        if has_non_int:
+            print("Set Torque The numerical range can only be positive integers or floating-point numbers between 0 and 255", flush=True)
+            return
+        if len(torque) < 5:
+            print("数据长度不够,至少5个元素", flush=True)
+            return
+        if self.hand_joint == "L7" and len(torque) < 7:
+            print("数据长度不够,至少7个元素", flush=True)
+            return
+        if (self.hand_joint == "L6" or self.hand_joint == "O6") and len(torque) != 6:
+            print("L6 or O6数据长度错误,至少6个元素", flush=True)
+            return
+        ColorMsg(msg=f"{self.hand_type} {self.hand_joint} set maximum torque to {torque}", color="green")
+        return self.hand.set_torque(torque=torque)
+    
+    
+    def set_current(self, current=[250] * 5):
+        '''Set current L7/L10/L25 not supported'''
+        if any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in current):
+            print("Set Current The numerical range can only be positive integers or floating-point numbers between 0 and 255", flush=True)
+            return
+        if self.hand_joint == "L20":
+            return self.hand.set_current(current=current)
+        else:
+            pass
+
+    def get_embedded_version(self):
+        '''Get embedded version'''
+        return self.hand.get_version()
+    
+    def get_serial_number(self):
+        '''Get serial number'''
+        try:
+            return self.hand.sn
+        except:
+            return self.hand.get_serial_number()
+
+    def get_current(self):
+        '''Get current'''
+        return self.hand.get_current()
+    
+    def get_state(self):
+        '''Get current joint state'''
+        return self.hand.get_current_status()
+
+    
+    def get_state_for_pub(self):
+        return self.hand.get_current_pub_status()
+    
+    def get_speed(self):
+        '''Get speed'''
+        return self.hand.get_speed()
+
+    
+    def get_joint_speed(self):
+        speed = []
+        if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6":
+            return self.hand.get_speed()
+        elif self.hand_joint == "L7":
+            return self.hand.get_speed()
+        elif self.hand_joint == "L10":
+            speed = self.hand.get_speed()
+            return speed
+        elif self.hand_joint == "G20":
+            return self.hand.get_speed()
+        elif self.hand_joint == "L20":
+            speed = self.hand.get_speed()
+            return [255, speed[1], speed[2], speed[3], speed[4], 255, 255, 255, 255, 255, speed[0], 255, 255, 255, 255, 255, 255, 255, 255, 255]
+        elif self.hand_joint == "L21":
+            return self.hand.get_speed()
+        elif self.hand_joint == "L25":
+            return self.hand.get_speed()
+
+    def get_touch_type(self):
+        '''Get touch type'''
+        try:
+            return self.hand.touch_type
+        except:
+            return self.hand.get_touch_type()
+    
+    def get_force(self):
+        '''Get normal force, tangential force, tangential force direction, approach sensing data'''
+        self._get_normal_force()
+        self._get_tangential_force()
+        self._get_tangential_force_dir()
+        self._get_approach_inc()
+        return self.hand.get_force()
+
+    def get_touch(self):
+        '''Get touch data'''
+        return self.hand.get_touch()
+    
+    def get_matrix_touch(self):
+        return self.hand.get_matrix_touch()
+    
+    def get_matrix_touch_v2(self):
+        return self.hand.get_matrix_touch_v2()
+    
+
+    def get_thumb_matrix_touch(self,sleep_time=0):
+        if sleep_time > 0:
+            return self.hand.get_thumb_matrix_touch(sleep_time=sleep_time)
+        else:
+            return self.hand.get_thumb_matrix_touch()
+    
+    def get_index_matrix_touch(self,sleep_time=0):
+        if sleep_time > 0:
+            return self.hand.get_index_matrix_touch(sleep_time=sleep_time)
+        else:
+            return self.hand.get_index_matrix_touch()
+    
+    def get_middle_matrix_touch(self,sleep_time=0):
+        if sleep_time > 0:
+            return self.hand.get_middle_matrix_touch(sleep_time=sleep_time)
+        else:
+            return self.hand.get_middle_matrix_touch()
+    
+    def get_ring_matrix_touch(self,sleep_time=0):
+        if sleep_time > 0:
+            return self.hand.get_ring_matrix_touch(sleep_time=sleep_time)
+        else:
+            return self.hand.get_ring_matrix_touch()
+    
+    def get_little_matrix_touch(self,sleep_time=0):
+        if sleep_time > 0:
+            return self.hand.get_little_matrix_touch(sleep_time=sleep_time)
+        else:
+            return self.hand.get_little_matrix_touch()
+        
+    def get_palm_matrix_touch(self,sleep_time=0):
+        if self.is_palm_touch == 5:
+            if sleep_time > 0:
+                return self.hand.get_palm_matrix_touch(sleep_time=sleep_time)
+            else:
+                return self.hand.get_palm_matrix_touch()
+
+    def get_torque(self):
+        '''Get current maximum torque'''
+        return self.hand.get_torque()
+    
+    def get_temperature(self):
+        '''Get current motor temperature'''
+        return self.hand.get_temperature()
+    
+    def get_fault(self):
+        '''Get motor fault code'''
+        return self.hand.get_fault()
+    
+    def clear_faults(self):
+        '''Clear motor fault codes Not supported yet, currently only supports L20'''
+        self.hand.clear_faults()
+        return [0] * 5
+
+    def set_enable(self):
+        '''Set motor enable Only supports L25'''
+        if self.hand_joint == "L25":
+            self.hand.set_enable_mode()
+        else:
+            pass
+
+    def set_disable(self):
+        '''Set motor disable Only supports L25'''
+        if self.hand_joint == "L25":
+            self.hand.set_disability_mode()
+        else:
+            pass
+
+    def get_finger_order(self):
+        '''Get finger motor order'''
+        # if self.hand_joint == "L21" or self.hand_joint == "L25" or self.hand_joint == "G20":
+        #     return self.hand.get_finger_order()
+        # else:
+        #     return []
+        return self.hand.get_finger_order()
+        
+    def range_to_arc_left(self, state, hand_joint):
+        return range_to_arc_left(left_range=state, hand_joint=hand_joint)
+    
+    def range_to_arc_right(self, state, hand_joint):
+        return range_to_arc_right(right_range=state, hand_joint=hand_joint)
+    
+    def arc_to_range_left(self,state,hand_joint):
+        return arc_to_range_left(hand_arc_l=state,hand_joint=hand_joint)
+    
+    def arc_to_range_right(self,state,hand_joint):
+        return arc_to_range_right(right_arc=state,hand_joint=hand_joint)
+    
+    def show_fun_table(self):
+        self.hand.show_fun_table()
+        
+    def close_can(self):
+        if sys.platform == "linux" and modbus=="None":
+            self.open_can.close_can(can=self.can)                         
+
+if __name__ == "__main__":
+    hand = LinkerHandApi(hand_type="right", hand_joint="L10")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/color_msg.py

@@ -0,0 +1,27 @@
+#! /usr/bin/env python3
+
+import time
+
+class ColorMsg():
+    def __init__(self,msg: str,color: str = '', timestamp: bool = True) -> None:
+        self.msg = msg
+        self.color = color
+        self.timestamp = timestamp
+        self.colorMsg(msg=self.msg, color=self.color, timestamp=self.timestamp)
+
+    def colorMsg(self,msg: str, color: str = '', timestamp: bool = True):
+        str = ""
+        if timestamp:
+            str += time.strftime('%Y-%m-%d %H:%M:%S',
+                                time.localtime(time.time())) + "  "
+        if color == "red":
+            str += "\033[1;31;40m"
+        elif color == "green":
+            str += "\033[1;32;40m"
+        elif color == "yellow":
+            str += "\033[1;33;40m"
+        else:
+            print(str + msg, flush=True)
+            return
+        str += msg + "\033[0m"
+        print(str, flush=True)

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/init_linker_hand.py

@@ -0,0 +1,81 @@
+'''
+Author: HJX
+Date: 2025-04-01 14:09:21
+LastEditors: Please set LastEditors
+LastEditTime: 2025-04-08 11:18:23
+FilePath: /Linker_Hand_SDK_ROS/src/linker_hand_sdk_ros/scripts/LinkerHand/utils/init_linker_hand.py
+Description: 
+symbol_custom_string_obkorol_copyright: 
+'''
+import yaml, os, sys
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+from load_write_yaml import LoadWriteYaml
+
+class InitLinkerHand():
+    def __init__(self):
+        self.yaml = LoadWriteYaml()
+        self.setting = self.yaml.load_setting_yaml()
+
+    def current_hand(self):
+        '''
+        初始化灵巧手
+        return: hand_joint str L7/L10/L20/L21/L25, hand_type str left or right
+        '''
+        # 左手是否配置
+        self.left_hand = None
+        self.left_hand_joint = None
+        self.left_hand_type = None
+        self.left_hand_force = None
+        self.left_hand_pose = None
+        self.left_hand_torque = [200, 200, 200, 200, 200]
+        self.left_hand_speed = [80, 200, 200, 200, 200]
+        # 右手是否配置
+        self.right_hand = None
+        self.right_hand_joint = None
+        self.right_hand_type = None
+        self.right_hand_force = None
+        self.right_hand_pose = None
+        self.right_hand_torque = [200, 200, 200, 200, 200]
+        self.right_hand_speed = [80, 200, 200, 200, 200]
+        if self.setting['LINKER_HAND']['LEFT_HAND']['EXISTS'] == True:
+            self.left_hand = True
+            self.left_hand_joint = self.setting['LINKER_HAND']['LEFT_HAND']['JOINT']
+            self.left_hand_type = "left"
+            self.left_hand_force = self.setting['LINKER_HAND']['LEFT_HAND']['TOUCH']
+            if self.left_hand_joint == "L7":
+                # The data length of L7 is 7, reinitialize here
+                self.left_hand_pose = [255, 200, 255, 255, 255, 255, 180]
+                self.left_hand_torque = [250, 250, 250, 250, 250, 250, 250]
+                self.left_hand_speed = [120, 180, 180, 180, 180, 180, 180]
+            elif self.left_hand_joint == "L10":
+                self.left_hand_pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+            elif self.left_hand_joint == "L20":
+                self.left_hand_pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+            elif self.left_hand_joint == "L21":
+                self.left_hand_pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+            elif self.left_hand_joint == "L25":
+                self.left_hand_pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+        # 判断右手是否配置 
+        if self.setting['LINKER_HAND']['RIGHT_HAND']['EXISTS'] == True:
+            self.right_hand = True
+            self.right_hand_joint = self.setting['LINKER_HAND']['RIGHT_HAND']['JOINT']
+            self.right_hand_type = "right"
+            self.right_hand_force = self.setting['LINKER_HAND']['RIGHT_HAND']['TOUCH']
+            if self.right_hand_joint == "L7":
+                # The data length of L7 is 7, reinitialize here
+                self.right_hand_pose = [255, 200, 255, 255, 255, 255, 180]
+                self.right_hand_torque = [250, 250, 250, 250, 250, 250, 250]
+                self.right_hand_speed = [120, 250, 250, 250, 250, 250, 250]
+            elif self.right_hand_joint == "L10":
+                self.right_hand_pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+            elif self.right_hand_joint == "L20":
+                self.right_hand_pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+            elif self.right_hand_joint == "L21":
+                self.right_hand_pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+            elif self.right_hand_joint == "L25":
+                self.right_hand_pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+
+
+        return self.left_hand ,self.left_hand_joint ,self.left_hand_type ,self.left_hand_force,self.left_hand_pose, self.left_hand_torque, self.left_hand_speed ,self.right_hand ,self.right_hand_joint ,self.right_hand_type ,self.right_hand_force,self.right_hand_pose, self.right_hand_torque, self.right_hand_speed,self.setting
+
+        

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/load_write_yaml.py

@@ -0,0 +1,101 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+'''
+Author: HJX
+Date: 2025-04-01 14:09:21
+LastEditors: Please set LastEditors
+LastEditTime: 2025-04-11 10:19:01
+FilePath: /LinkerHand_Python_SDK/LinkerHand/utils/load_write_yaml.py
+Description: 
+symbol_custom_string_obkorol_copyright: 
+'''
+import yaml, os, sys
+class LoadWriteYaml():
+    def __init__(self):
+        # 由于是API形式，这里要给配置文件目录绝对路径
+        #yaml_path = "/home/linkerhand/ROS2/linker_hand_ros2_sdk/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand"
+        yaml_path = os.path.dirname(os.path.abspath(__file__)) + "/../../LinkerHand"
+        self.setting_path = yaml_path+"/config/setting.yaml"
+        self.l7_positions = yaml_path+"/config/L7_positions.yaml"
+        self.l10_positions = yaml_path+"/config/L10_positions.yaml"
+        self.l20_positions = yaml_path+"/config/L20_positions.yaml"
+        self.l21_positions = yaml_path+"/config/L21_positions.yaml"
+        self.l25_positions = yaml_path+"/config/L25_positions.yaml"
+        
+
+    def load_setting_yaml(self):
+        try:
+            with open(self.setting_path, 'r', encoding='utf-8') as file:
+                setting = yaml.safe_load(file)
+                self.sdk_version = setting["VERSION"]
+                self.left_hand_exists = setting['LINKER_HAND']['LEFT_HAND']['EXISTS']
+                self.left_hand_names = setting['LINKER_HAND']['LEFT_HAND']['NAME']
+                self.left_hand_joint = setting['LINKER_HAND']['LEFT_HAND']['JOINT']
+                self.left_hand_force = setting['LINKER_HAND']['LEFT_HAND']['TOUCH']
+                self.right_hand_exists = setting['LINKER_HAND']['RIGHT_HAND']['EXISTS']
+                self.right_hand_names = setting['LINKER_HAND']['RIGHT_HAND']['NAME']
+                self.right_hand_joint = setting['LINKER_HAND']['RIGHT_HAND']['JOINT']
+                self.right_hand_force = setting['LINKER_HAND']['RIGHT_HAND']['TOUCH']
+                self.password = setting['PASSWORD']
+        except Exception as e:
+            setting = None
+            print(f"Error reading setting.yaml: {e}")
+        self.setting = setting
+        return self.setting
+    
+    def load_action_yaml(self,hand_joint="",hand_type=""):
+        if hand_joint == "L20":
+            action_path = self.l20_positions
+        elif hand_joint == "L10":
+            action_path = self.l10_positions
+        elif hand_joint == "L25":
+            action_path = self.l25_positions
+        elif hand_joint == "L21":
+            action_path = self.l21_positions
+        elif hand_joint == "L7":
+            action_path = self.l7_positions
+            print(action_path)
+        try:
+            with open(action_path, 'r', encoding='utf-8') as file:
+                yaml_data = yaml.safe_load(file)
+                if hand_type == "left":
+                    self.action_yaml = yaml_data["LEFT_HAND"]
+                else:
+                    self.action_yaml = yaml_data["RIGHT_HAND"]
+        except Exception as e:
+            self.action_yaml = None
+            print(f"yaml配置文件不存在: {e}")
+        return self.action_yaml 
+
+    def write_to_yaml(self, action_name, action_pos,hand_joint="",hand_type=""):
+        a = False
+        if hand_joint == "L20":
+            action_path = self.l20_positions
+        elif hand_joint == "L10":
+            action_path = self.l10_positions
+        elif hand_joint == "L7":
+            action_path = self.l7_positions
+        elif hand_joint == "L21":
+            action_path = self.l21_positions
+        elif hand_joint == "L25":
+            action_path = self.l25_positions
+        try:
+            with open(action_path, 'r', encoding='utf-8') as file:
+                yaml_data = yaml.safe_load(file)
+                print(yaml_data)
+            if hand_type == "left":
+                if yaml_data["LEFT_HAND"] == None:
+                    yaml_data["LEFT_HAND"] = []
+                yaml_data["LEFT_HAND"].append({"ACTION_NAME": action_name, "POSITION": action_pos})
+            elif hand_type == "right":
+                if yaml_data["RIGHT_HAND"] == None:
+                    yaml_data["RIGHT_HAND"] = []
+                yaml_data["RIGHT_HAND"].append({"ACTION_NAME": action_name, "POSITION": action_pos})
+            with open(action_path, 'w', encoding='utf-8') as file:
+                yaml.safe_dump(yaml_data, file, allow_unicode=True)
+            a = True
+        except Exception as e:
+            a = False
+            print(f"Error writing to yaml file: {e}")
+        return a
+        

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/mapping.py

@@ -0,0 +1,383 @@
+#---------------------------------------------------------------------------------------------------
+# L6 L
+l6_l_min = [0, 0, 0, 0, 0, 0]
+l6_l_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26]
+l6_l_derict = [-1, -1, -1, -1, -1, -1]
+# L6 R
+l6_r_min = [0, 0, 0, 0, 0, 0]
+l6_r_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26]
+l6_r_derict = [-1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# O6 L
+o6_l_min = [0, 0, 0, 0, 0, 0]
+o6_l_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
+o6_l_derict = [-1, -1, -1, -1, -1, -1]
+# O6 R
+o6_r_min = [0, 0, 0, 0, 0, 0]
+o6_r_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
+o6_r_derict = [-1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L7 L OK
+l7_l_min = [0, 0, 0, 0, 0, 0, 0]
+l7_l_max = [0.44, 1.43, 1.62, 1.62, 1.62, 1.62, 1.01]
+l7_l_derict = [-1, -1, -1, -1, -1, -1, -1]
+# L7 R OK (urdf后续会更改！！！)
+l7_r_min = [0, -1.43, 0, 0, 0, 0, 0]
+l7_r_max = [0.75, 0, 1.62, 1.62, 1.62, 1.62, 1.54]
+l7_r_derict = [-1, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L10 L OK
+l10_l_min = [0, 0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.52]
+l10_l_max = [1.45, 1.43, 1.62, 1.62, 1.62, 1.62, 0.26, 0, 0, 1.01]
+l10_l_derict = [-1, -1, -1, -1, -1, -1, 0, -1, -1, -1]
+# L10 R OK
+l10_r_min = [0, 0, 0, 0, 0, 0, -0.26, 0, 0, -0.52]
+l10_r_max = [0.75, 1.43, 1.62, 1.62, 1.62, 1.62, 0.21, 0.21, 0.34, 1.01]
+l10_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, -1]
+#---------------------------------------------------------------------------------------------------
+# L20 L OK
+l20_l_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0.122, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l20_l_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08]
+l20_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+# L20 R OK
+l20_r_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l20_r_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08]
+l20_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L21 L OK
+l21_l_min = [0, 0, 0, 0, 0, 0, 0, -0.18, -0.18, 0, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l21_l_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57]
+l21_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+# L21 R OK
+l21_r_min = [0, 0, 0, 0, 0, 0, -0.18, -0.18, -0.18, -0.18, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l21_r_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57]
+l21_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+#---------------------------------------------------------------------------------------------------
+# L25 L OK
+l25_l_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l25_l_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57]
+l25_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
+# L25 R OK
+l25_r_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l25_r_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57]
+l25_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+
+def range_to_arc_left(left_range,hand_joint):
+    num=0
+    if hand_joint == "L6":
+        num = 6
+        l_min = l6_l_min
+        l_max = l6_l_max
+        l_derict = l6_l_derict
+    elif hand_joint == "O6":
+        num = 6
+        l_min = o6_l_min
+        l_max = o6_l_max
+        l_derict = o6_l_derict
+    elif hand_joint == "L7":
+        num = 7
+        l_min = l7_l_min
+        l_max = l7_l_max
+        l_derict = l7_l_derict
+    elif hand_joint == "L10":
+        num = 10
+        l_min = l10_l_min
+        l_max = l10_l_max
+        l_derict = l10_l_derict
+    elif hand_joint == "L20":
+        num = 20
+        l_min = l20_l_min
+        l_max = l20_l_max
+        l_derict = l20_l_derict
+    elif hand_joint == "L21":
+        num = 25
+        l_min = l21_l_min
+        l_max = l21_l_max
+        l_derict = l21_l_derict
+    hand_arc = [0] * num
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_l = is_within_range(left_range[i], 0, 255)
+        if l_derict[i] == -1:
+            hand_arc[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i])
+        else:
+            hand_arc[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i])
+    return hand_arc
+
+def range_to_arc_right(right_range,hand_joint):
+    num=0
+    if hand_joint == "L6":
+        num = 6
+        r_min = l6_r_min
+        r_max = l6_r_max
+        r_derict = l6_r_derict
+    elif hand_joint == "O6":
+        num = 6
+        r_min = o6_r_min
+        r_max = o6_r_max
+        r_derict = o6_r_derict
+    elif hand_joint == "L7":
+        num = 7
+        r_min = l7_r_min
+        r_max = l7_r_max
+        r_derict = l7_r_derict
+    elif hand_joint == "L10":
+        num = 10
+        r_min = l10_r_min
+        r_max = l10_r_max
+        r_derict = l10_r_derict
+    elif hand_joint == "L20":
+        num = 20
+        r_min = l20_r_min
+        r_max = l20_r_max
+        r_derict = l20_r_derict
+    elif hand_joint == "L21":
+        num = 25
+        r_min = l21_r_min
+        r_max = l21_r_max
+        r_derict = l21_r_derict
+    hand_arc = [0] * num
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_r = is_within_range(right_range[i], 0, 255)
+        if r_derict[i] == -1:
+            hand_arc[i] = scale_value(val_r, 0, 255, r_max[i], r_min[i])
+        else:
+            hand_arc[i] = scale_value(val_r, 0, 255, r_min[i], r_max[i])
+    return hand_arc
+        
+'''
+def arc_to_range_left(left_arc,hand_joint):
+    num=0
+    if hand_joint == "L7":
+        num = 7
+        l_min = l7_l_min
+        l_max = l7_l_max
+        l_derict = l7_l_derict
+    elif hand_joint == "L10":
+        num = 10
+        l_min = l10_l_min
+        l_max = l10_l_max
+        l_derict = l10_l_derict
+    elif hand_joint == "L20":
+        num = 20
+        l_min = l20_l_min
+        l_max = l20_l_max
+        l_derict = l20_l_derict
+    elif hand_joint == "L21":
+        num = 25
+        l_min = l21_l_min
+        l_max = l21_l_max
+        l_derict = l21_l_derict
+    hand_range = [0] * num
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_l = is_within_range(left_arc[i], 0, 255)
+        if l_derict[i] == -1:
+            hand_range[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i])
+        else:
+            hand_range[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i])
+    return hand_range
+    '''
+def arc_to_range_left(hand_arc_l,hand_joint):
+    num=0
+    if hand_joint == "O6":
+        num = 6
+        l_min = o6_l_min
+        l_max = o6_l_max
+        l_derict = o6_l_derict
+    elif hand_joint == "L7":
+        num = 7
+        l_min = l7_l_min
+        l_max = l7_l_max
+        l_derict = l7_l_derict
+    elif hand_joint == "L10":
+        num = 10
+        l_min = l10_l_min
+        l_max = l10_l_max
+        l_derict = l10_l_derict
+    elif hand_joint == "L20":
+        num = 20
+        l_min = l20_l_min
+        l_max = l20_l_max
+        l_derict = l20_l_derict
+    elif hand_joint == "L21":
+        num = 25
+        l_min = l21_l_min
+        l_max = l21_l_max
+        l_derict = l21_l_derict
+    hand_range = [0] * num
+    #hand_range_l = [0] * 7
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_l = is_within_range(hand_arc_l[i], l_min[i], l_max[i])
+        if l_derict[i] == -1:
+            hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 255, 0)
+        else:
+            hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 0, 255)
+
+    return hand_range
+
+def arc_to_range_right(right_arc,hand_joint):
+    num=0
+    if hand_joint == "O6":
+        num = 6
+        r_min = o6_r_min
+        r_max = o6_r_max
+        r_derict = o6_r_derict
+    elif hand_joint == "L7":
+        num = 7
+        r_min = l7_r_min
+        r_max = l7_r_max
+        r_derict = l7_r_derict
+    elif hand_joint == "L10":
+        num = 10
+        r_min = l10_r_min
+        r_max = l10_r_max
+        r_derict = l10_r_derict
+    elif hand_joint == "L20":
+        num = 20
+        r_min = l20_r_min
+        r_max = l20_r_max
+        r_derict = l20_r_derict
+    elif hand_joint == "L21":
+        num = 25
+        r_min = l21_r_min
+        r_max = l21_r_max
+        r_derict = l21_r_derict
+    hand_range = [0] * num
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_r = is_within_range(right_arc[i], r_min[i], r_max[i])
+        if r_derict[i] == -1:
+            hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 255, 0)
+        else:
+            hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 0, 255)
+    return hand_range
+
+
+
+
+def range_to_arc_right_l20(hand_range_r):
+    hand_arc_r = [0] * 20
+    for i in range(20):
+        if 11 <= i <= 14: continue
+        val_r = is_within_range(hand_range_r[i], 0, 255)
+        if l20_r_derict[i] == -1:
+            hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_max[i], l20_r_min[i])
+        else:
+            hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_min[i], l20_r_max[i])
+    return hand_arc_r
+
+
+def range_to_arc_left_l20(hand_range_l):
+    hand_arc_l = [0] * 20
+    for i in range(20):
+        if 11 <= i <= 14: continue
+        val_l = is_within_range(hand_range_l[i], 0, 255)
+        if l20_l_derict[i] == -1:
+            hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_max[i], l20_l_min[i])
+        else:
+            hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_min[i], l20_l_max[i])
+    return hand_arc_l
+
+
+def arc_to_range_right_l20(hand_arc_r):
+    hand_range_r = [0] * 20
+    for i in range(20):
+        if 11 <= i <= 14: continue
+        val_r = is_within_range(hand_arc_r[i], l20_r_min[i], l20_r_max[i])
+        if l20_r_derict[i] == -1:
+            hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 255, 0)
+        else:
+            hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 0, 255)
+    return hand_range_r
+
+
+def arc_to_range_left_l20(hand_arc_l):
+    hand_range_l = [0] * 20
+    for i in range(20):
+        if 11 <= i <= 14: continue
+        val_l = is_within_range(hand_arc_l[i], l20_l_min[i], l20_l_max[i])
+        if l20_l_derict[i] == -1:
+            hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 255, 0)
+        else:
+            hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 0, 255)
+
+    return hand_range_l
+
+
+def range_to_arc_right_10(hand_range_r):
+    hand_arc_r = [0] * 10
+    for i in range(10):
+        val_r = is_within_range(hand_range_r[i], 0, 255)
+        if l10_r_derict[i] == -1:
+            hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_max[i], l10_r_min[i])
+        else:
+            hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_min[i], l10_r_max[i])
+
+    return hand_arc_r
+
+
+def range_to_arc_left_10(hand_range_l):
+    hand_arc_l = [0] * 10
+    for i in range(10):
+        val_l = is_within_range(hand_range_l[i], 0, 255)
+        if l10_l_derict[i] == -1:
+            hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_max[i], l10_l_min[i])
+        else:
+            hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_min[i], l10_l_max[i])
+    return hand_arc_l
+
+
+def arc_to_range_right_10(hand_arc_r):
+    hand_range_r = [0] * 10
+    for i in range(10):
+        val_r = is_within_range(hand_arc_r[i], l10_r_min[i], l10_r_max[i])
+        if l10_r_derict[i] == -1:
+            hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 255, 0)
+        else:
+            hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 0, 255)
+    return hand_range_r
+
+
+def arc_to_range_left_10(hand_arc_l):
+    hand_range_l = [0] * 10
+    for i in range(10):
+        val_l = is_within_range(hand_arc_l[i], l10_l_min[i], l10_l_max[i])
+        if l10_l_derict[i] == -1:
+            hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 255, 0)
+        else:
+            hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 0, 255)
+
+    return hand_range_l
+
+
+def scale_value(original_value, a_min, a_max, b_min, b_max):
+    return (original_value - a_min) * (b_max - b_min) / (a_max - a_min) + b_min
+
+
+def is_within_range(value, min_value, max_value):
+    return min(max_value, max(min_value, value))

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/open_can.py

@@ -0,0 +1,145 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+'''
+Author: HJX
+Date: 2025-04-01 14:09:21
+LastEditors: Please set LastEditors
+LastEditTime: 2025-04-11 09:15:31
+FilePath: /Linker_Hand_SDK_ROS/src/linker_hand_sdk_ros/scripts/LinkerHand/utils/open_can.py
+Description: 
+symbol_custom_string_obkorol_copyright: 
+'''
+import sys,os,time,subprocess
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+from color_msg import ColorMsg
+from load_write_yaml import LoadWriteYaml
+# from ament_index_python.packages import get_package_share_directory
+import os
+
+
+class OpenCan:
+    def __init__(self,load_yaml=None):
+        self.yaml = LoadWriteYaml()
+        self.password = self.yaml.load_setting_yaml()["PASSWORD"]
+
+    def open_can0(self):
+        try:
+            # 检查 can0 接口是否已存在并处于 up 状态
+            result = subprocess.run(
+                ["ip", "link", "show", "can0"],
+                check=True,
+                text=True,
+                capture_output=True
+            )
+            if "state UP" in result.stdout:
+                return 
+            # 如果没有处于 UP 状态，则配置接口
+            subprocess.run(
+                ["sudo", "-S", "ip", "link", "set", "can0", "up", "type", "can", "bitrate", "1000000"],
+                input=f"{self.password}\n",
+                check=True,
+                text=True,
+                capture_output=True
+            )
+            
+        except subprocess.CalledProcessError as e:
+            pass
+        except Exception as e:
+            pass
+    def open_can(self,can="can0"):
+        try:
+            # 检查 can0 接口是否已存在并处于 up 状态
+            result = subprocess.run(
+                ["ip", "link", "show", can],
+                check=True,
+                text=True,
+                capture_output=True
+            )
+            if "state UP" in result.stdout:
+                return 
+            # 如果没有处于 UP 状态，则配置接口
+            subprocess.run(
+                ["sudo", "-S", "ip", "link", "set", can, "up", "type", "can", "bitrate", "1000000"],
+                input=f"{self.password}\n",
+                check=True,
+                text=True,
+                capture_output=True
+            )
+        except subprocess.CalledProcessError as e:
+            pass
+        except Exception as e:
+            pass
+            
+
+    def is_can_up_sysfs(self, interface="can0"):
+    # 检查接口目录是否存在
+        if not os.path.exists(f"/sys/class/net/{interface}"):
+            return False
+        # 读取接口状态
+        try:
+            with open(f"/sys/class/net/{interface}/operstate", "r") as f:
+                state = f.read().strip()
+            if state == "up":
+                return True
+        except Exception as e:
+            print(f"Error reading CAN interface state: {e}")
+            return False
+        
+    def close_can0(self):
+        try:
+            # 检查 can0 接口是否存在
+            result = subprocess.run(
+                ["ip", "link", "show", "can0"],
+                check=True,
+                text=True,
+                capture_output=True
+            )
+            
+            # 如果接口存在且处于 UP 状态，则关闭它
+            if "state UP" in result.stdout:
+                subprocess.run(
+                    ["sudo", "-S", "ip", "link", "set", "can0", "down"],
+                    input=f"{self.password}\n",
+                    check=True,
+                    text=True,
+                    capture_output=True
+                )
+                return True
+            return False
+            
+        except subprocess.CalledProcessError as e:
+            print(f"Error closing CAN interface: {e}")
+            return False
+        except Exception as e:
+            print(f"Unexpected error: {e}")
+            return False
+    
+    def close_can(self,can="can0"):
+        try:
+            # 检查 can0 接口是否存在
+            result = subprocess.run(
+                ["ip", "link", "show", can],
+                check=True,
+                text=True,
+                capture_output=True
+            )
+            
+            # 如果接口存在且处于 UP 状态，则关闭它
+            if "state UP" in result.stdout:
+                subprocess.run(
+                    ["sudo", "-S", "ip", "link", "set", can, "down"],
+                    input=f"{self.password}\n",
+                    check=True,
+                    text=True,
+                    capture_output=True
+                )
+                return True
+            return False
+            
+        except subprocess.CalledProcessError as e:
+            print(f"Error closing CAN interface: {e}")
+            return False
+        except Exception as e:
+            print(f"Unexpected error: {e}")
+            return False
+    

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py

@@ -0,0 +1,421 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys                                     # ROS2 Python接口库
+import time
+import numpy as np
+from rclpy.node import Node                      # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+
+class LinkerHand(Node):
+    def __init__(self, name):
+        super().__init__(name)
+        # 声明参数（带默认值）
+        self.declare_parameter('hand_type', 'left')
+        self.declare_parameter('hand_joint', 'L6')
+        self.declare_parameter('is_touch', False)
+        self.declare_parameter('can', 'can0')
+        self.declare_parameter('modbus', "None")        
+
+        # ros时间获取
+        self.stamp_clock = Clock()
+        # 获取参数值
+        self.hand_type = self.get_parameter('hand_type').value
+        self.hand_joint = self.get_parameter('hand_joint').value
+        self.is_touch = self.get_parameter('is_touch').value
+        self.can = self.get_parameter('can').value
+        self.modbus = self.get_parameter('modbus').value
+        self.sdk_v = 2
+        self.sleep_time = 0.005
+        self.cmd_lock = False
+        self.last_hand_post_cmd = None # 最新手指位置命令
+        self.last_hand_vel_cmd = None # 最新手指速度命令
+        self.last_hand_eff_cmd = None # 最新手指力矩命令
+
+        self.last_hand_state = [-1] * 10
+        self.last_hand_vel = [-1] * 10
+        self.force = [[-1] * 5] * 4
+        self.matrix_dic = {
+            "stamp":{
+                "sec": 0,
+                "nanosec": 0,
+            },
+            "thumb_matrix":[[-1] * 6 for _ in range(12)],
+            "index_matrix":[[-1] * 6 for _ in range(12)],
+            "middle_matrix":[[-1] * 6 for _ in range(12)],
+            "ring_matrix":[[-1] * 6 for _ in range(12)],
+            "little_matrix":[[-1] * 6 for _ in range(12)]
+        }
+        # 压感矩阵合值，单位g 克
+        self.matrix_mass_dic = {
+            "stamp":{
+                "secs": 0,
+                "nsecs": 0,
+            },
+            "thumb_mass":[-1],
+            "index_mass":[-1],
+            "middle_mass":[-1],
+            "ring_mass":[-1],
+            "little_mass":[-1]
+        }
+        self.last_hand_info = {
+            "version": [-1], # Dexterous hand version number
+            "hand_joint": self.hand_joint, # Dexterous hand joint type
+            "speed": [-1] * 10, # Current speed threshold of the dexterous hand
+            "current": [-1] * 10, # Current of the dexterous hand
+            "fault": [-1] * 10, # Current fault of the dexterous hand
+            "motor_temperature": [-1] * 10, # Current motor temperature of the dexterous hand
+            "torque": [-1] * 10, # Current torque of the dexterous hand
+            "is_touch":self.is_touch,
+            "touch_type": -1,
+            "finger_order": None # Finger motor order
+        }
+        self.version = []
+        self.touch_type = -1
+        self.hz = 1.0/60.0
+
+        self.hand_setting_sub = self.create_subscription(String,'/cb_hand_setting_cmd', self.hand_setting_cb, 10)
+        self._init_hand()
+        time.sleep(1)
+        self.run_count = 0 # 计数器，用于记录运行次数
+        self.timer = self.create_timer(0.01, self.run)  # 100 Hz
+        self.thread_pub_state = threading.Thread(target=self.pub_state)
+        self.thread_pub_state.daemon = True
+        self.thread_pub_state.start()
+
+    def _init_hand(self):
+        self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+        time.sleep(0.1)
+        self.touch_type = self.api.get_touch_type()
+        self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+        self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10)
+        if self.is_touch == True:
+            if self.modbus != "None":
+                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+            elif self.touch_type > 1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+            elif self.touch_type != -1 and self.modbus == "None":
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+                self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+            else:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+                self.is_touch = False
+            
+        self.embedded_version = self.api.get_embedded_version()
+        pose = None
+        torque = [200, 200, 200, 200, 200]
+        speed = [200, 250, 250, 250, 250]
+        if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6" or self.hand_joint.upper() == "L6P":
+            pose = [200, 255, 255, 255, 255, 180]
+            torque = [250, 250, 250, 250, 250, 250]
+            # O6 最大速度阈值
+            speed = [200, 250, 250, 250, 250, 250]
+        elif self.hand_joint == "L7":
+            # The data length of L7 is 7, reinitialize here
+            pose = [255, 200, 255, 255, 255, 255, 180]
+            torque = [250, 250, 250, 250, 250, 250, 250]
+            speed = [120, 250, 250, 250, 250, 250, 250]
+        elif self.hand_joint == "L10":
+            torque = [255] * 10
+            pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+            speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250]
+        elif self.hand_joint == "L20":
+            pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+        elif self.hand_joint == "L21":
+            pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+        elif self.hand_joint == "L25":
+            pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+        if pose is not None:
+            for i in range(1): 
+                self.api.set_speed(speed=speed)
+                time.sleep(0.1)
+                self.api.set_torque(torque=torque)
+                time.sleep(0.1)
+                self.api.finger_move(pose=pose)
+                time.sleep(0.1)
+
+    def list_check(self,pose):
+        if isinstance(pose, list) == False:
+            return False
+        if len(self.last_hand_post_cmd) != len(pose):
+            return False
+        return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+    def hand_control_cb(self, msg):
+        if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+            self.last_hand_post_cmd = msg.position
+        if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+            self.last_hand_vel_cmd = msg.velocity
+        if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+            self.last_hand_eff_cmd = msg.effort
+
+    def run(self):
+        if self.sdk_v == 1:
+            self.sleep_time = 0.009
+        if self.hand_state_pub.get_subscription_count() > 0:
+            # 优先获取手指状态并且发布
+            self.last_hand_state = self.api.get_state()
+            time.sleep(0.003)
+            self.last_hand_vel = self.api.get_joint_speed()
+            time.sleep(0.002)
+        if self.cmd_lock == False:
+            if self.last_hand_post_cmd != None:
+                self.api.finger_move(pose=self.last_hand_post_cmd)
+                self.last_hand_post_cmd = None
+            if self.last_hand_vel_cmd != None:
+                vel = list(self.last_hand_vel_cmd)
+                if all(x == 0 for x in vel):
+                    pass
+                else:
+                    if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
+                        speed = vel
+                        self.api.set_joint_speed(speed=speed)
+                    elif self.hand_joint == "L7" and len(vel) == 7:
+                        speed = vel
+                        self.api.set_joint_speed(speed=speed)
+                    elif self.hand_joint == "L10" and len(vel) == 10:
+                        speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
+                        self.api.set_joint_speed(speed=speed)
+                    elif self.hand_joint == "L20" and len(vel) == 20:
+                        speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
+                        self.api.set_joint_speed(speed=speed)
+                    elif self.hand_joint == "L21" and len(vel) == 25:
+                        speed = vel
+                        self.api.set_joint_speed(speed=speed)
+                    elif self.hand_joint == "L25" and len(vel) == 25:
+                        speed = vel
+                        self.api.set_joint_speed(speed=speed)
+                self.last_hand_vel_cmd = None
+            time.sleep(0.003)
+            if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.modbus == "None" and self.touch_pub.get_subscription_count() > 0:
+                """单点式压力传感器"""
+                self.force = self.api.get_force()
+            if self.is_touch == True and (self.touch_type > 1 or self.modbus != "None") and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
+                """矩阵式压力传感器"""
+                if self.run_count == 3:
+                    self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.sleep_time).tolist()
+                if self.run_count == 4:
+                    self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.sleep_time).tolist()
+                if self.run_count == 5:
+                    self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.sleep_time).tolist()
+                if self.run_count == 6:
+                    self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.sleep_time).tolist()
+                if self.run_count == 7:
+                    self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist()
+                time.sleep(0.005)
+            if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0:
+                """手部信息"""
+                self.last_hand_info = {
+                    "version": self.embedded_version, # Dexterous hand version number
+                    "hand_joint": self.hand_joint, # Dexterous hand joint type
+                    "speed": self.api.get_speed(), # Current speed threshold of the dexterous hand
+                    "current": self.api.get_current(), # Current of the dexterous hand
+                    "fault": self.api.get_fault(), # Current fault of the dexterous hand
+                    "motor_temperature": self.api.get_temperature(), # Current motor temperature of the dexterous hand
+                    "torque": self.api.get_torque(), # Current torque of the dexterous hand
+                    "is_touch":self.is_touch,
+                    "touch_type": self.touch_type,
+                    "finger_order": self.api.get_finger_order() # Finger motor order
+                }
+                
+            if self.run_count == 9:
+                self.api.clear_faults() # 自动清除错误编码
+                self.run_count = 0
+            self.run_count += 1
+            time.sleep(0.003)
+
+
+    def pub_state(self):
+        while True:
+            if self.hand_state_pub.get_subscription_count() > 0:
+                msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+                self.hand_state_pub.publish(msg)
+            if self.is_touch == True and self.touch_type == 1 and self.modbus == "None" and self.touch_pub.get_subscription_count() > 0:
+                msg = Float32MultiArray()
+                msg.data = [float(val) for sublist in self.force for val in sublist]
+                self.touch_pub.publish(msg)
+            if self.is_touch == True and (self.touch_type > 1 or self.modbus != "None") and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
+                # 发布矩阵压感数据JSON格式
+                self.pub_matrix_dic()
+                # 发布矩阵压感和值JSON格式
+                self.pub_matrix_mass(dic=self.matrix_dic)
+                # 发布矩阵压感点云格式
+                self.pub_matrix_point_cloud()
+            if self.hand_info_pub.get_subscription_count() > 0:
+                msg = String()
+                msg.data = json.dumps(self.last_hand_info)
+                self.hand_info_pub.publish(msg)
+            time.sleep(self.hz)
+
+    def pub_matrix_mass(self, dic):
+        """发布矩阵数据合值 单位g 克 JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_mass_dic["stamp"]["secs"] = t_secs
+        self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+        self.matrix_mass_dic["unit"] = "g"
+        self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+        self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+        self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+        self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+        self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+        msg.data = json.dumps(self.matrix_mass_dic)
+        self.matrix_touch_mass_pub.publish(msg)
+
+    def pub_matrix_point_cloud(self):
+        """发布矩阵数据点云格式"""
+        tmp_dic = self.matrix_dic.copy()
+        del tmp_dic['stamp']               # 去掉时间戳字段
+        all_matrices = list(tmp_dic.values())  # 5 帧，每帧 6×12=72 个数
+        # 摊平到一维：360 个 float
+        flat_list = [v for frame in all_matrices for v in frame]  # 360
+        flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+        fields = [PointField(
+            name='val',
+            offset=0,
+            datatype=PointField.UINT8,
+            count=1
+        )]
+        pc = PointCloud2()
+        pc.header.stamp = self.stamp_clock.now().to_msg()
+        pc.header.frame_id = ''
+        pc.height = 1
+        pc.width = flat.size         # 360
+        pc.fields = fields
+        pc.is_bigendian = False
+        pc.point_step = 1            # 1 个 float32
+        pc.row_step = pc.point_step * pc.width
+        pc.data = flat.tobytes()     # 1440 字节
+        self.matrix_touch_pub_pc.publish(pc)
+
+    def pub_matrix_dic(self):
+        """发布矩阵数据JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_dic["stamp"]["secs"] = t_secs
+        self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+        msg.data = json.dumps(self.matrix_dic)
+        self.matrix_touch_pub.publish(msg)
+
+    def joint_state_msg(self, pose,vel=[]):
+        joint_state = JointState()
+        joint_state.header = Header()
+        joint_state.header.stamp = self.get_clock().now().to_msg()
+        joint_state.name = self.api.get_finger_order()
+        joint_state.position = [float(x) for x in pose]
+        if len(vel) > 1:
+            joint_state.velocity = [float(x) for x in vel]
+        else:
+            joint_state.velocity = [0.0] * len(pose)
+        joint_state.effort = [0.0] * len(pose)
+        return joint_state
+    
+    
+
+    
+
+    def hand_setting_cb(self,msg):
+        '''控制命令回调'''
+        data = json.loads(msg.data)
+        print(f"Received setting command: {data['setting_cmd']}",flush=True)
+        try:
+            if data["params"]["hand_type"] == "left":
+                hand = self.api
+                hand_left = True
+            elif data["params"]["hand_type"] == "right":
+                hand = self.api
+                hand_right = True
+            else:
+                print("Please specify the hand part to be set",flush=True)
+                return
+            self.cmd_lock = True
+            # Set maximum torque
+            if data["setting_cmd"] == "set_max_torque_limits": # Set maximum torque
+                torque = list(data["params"]["torque"])
+                hand.set_torque(torque=torque)
+                
+            if data["setting_cmd"] == "set_speed": # Set speed
+                if isinstance(data["params"]["speed"], list) == True:
+                    speed = data["params"]["speed"]
+                    hand.set_speed(speed=speed)
+                else:
+                    ColorMsg(msg=f"Speed parameter error, speed must be a list", color="red")
+            if data["setting_cmd"] == "clear_faults": # Clear faults
+                if hand_left == True and self.hand_joint == "L10" :
+                    ColorMsg(msg=f"L10 left hand cannot clear faults")
+                elif hand_right == True and self.hand_joint == "L10" :
+                    ColorMsg(msg=f"L10 right hand cannot clear faults")
+                else:
+                    hand.clear_faults()
+            if data["setting_cmd"] == "get_faults": # Get faults
+                f = hand.get_fault()
+                ColorMsg(msg=f"Get faults: {f}")
+            if data["setting_cmd"] == "electric_current": # Get current
+                ColorMsg(msg=f"Get current: {hand.get_current()}")
+            if data["setting_cmd"] == "set_electric_current": # Set current
+                if isinstance(data["params"]["current"], list) == True:
+                    hand.set_current(data["params"]["current"])
+            if data["setting_cmd"] == "show_fun_table": # Get faults
+                f = hand.show_fun_table()
+        except:
+            print("命令参数错误")
+            self.cmd_lock = False
+        finally:
+            self.cmd_lock = False
+
+
+    def close_can(self):
+        self.api.open_can.close_can(can=self.can)
+        sys.exit(0)
+
+        
+def main(args=None):
+    try:
+        rclpy.init(args=args)
+        node = LinkerHand("linker_hand_sdk")
+        embedded_version = node.embedded_version
+        if len(embedded_version) == 3 or node.hand_joint.upper() == "O6" or node.hand_joint.upper() == "L6" or node.hand_joint.upper() == "G20":
+            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+            node.sdk_v = 2
+        elif len(embedded_version) == 6 and node.hand_joint == "L10":
+            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+            node.sdk_v = 2
+        elif len(embedded_version) > 4 and ((embedded_version[0]==10 and embedded_version[4]>35) or (embedded_version[0]==7 and embedded_version[4]>50) or (embedded_version[0] == 6)):
+            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+            node.sdk_v = 2
+        else:
+            ColorMsg(msg=f"SDK V1", color="green")
+            node.sdk_v = 1
+        rclpy.spin(node)         # 主循环，监听 ROS 回调
+    except KeyboardInterrupt:
+        print("收到 Ctrl+C，准备退出...")
+    finally:
+        # node.close_can()         # 关闭 CAN 或其他硬件资源
+        # node.destroy_node()      # 销毁 ROS 节点
+        # rclpy.shutdown()         # 关闭 ROS
+        print("程序已退出。")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak

@@ -0,0 +1,414 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys                                     # ROS2 Python接口库
+import time
+import numpy as np
+from rclpy.node import Node                      # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+
+class LinkerHand(Node):
+    def __init__(self, name):
+        super().__init__(name)
+        # 声明参数（带默认值）
+        self.declare_parameter('hand_type', 'left')
+        self.declare_parameter('hand_joint', 'L6')
+        self.declare_parameter('is_touch', False)
+        self.declare_parameter('can', 'can0')
+        self.declare_parameter('modbus', "None")        
+
+        # ros时间获取
+        self.stamp_clock = Clock()
+        # 获取参数值
+        self.hand_type = self.get_parameter('hand_type').value
+        self.hand_joint = self.get_parameter('hand_joint').value
+        self.is_touch = self.get_parameter('is_touch').value
+        self.can = self.get_parameter('can').value
+        self.modbus = self.get_parameter('modbus').value
+        self.sdk_v = 2
+        self.sleep_time = 0.005
+        self.cmd_lock = False
+        self.last_hand_post_cmd = None # 最新手指位置命令
+        self.last_hand_vel_cmd = None # 最新手指速度命令
+        self.last_hand_eff_cmd = None # 最新手指力矩命令
+
+        self.last_hand_state = [-1] * 10
+        self.last_hand_vel = [-1] * 10
+        self.force = [[-1] * 5] * 4
+        self.matrix_dic = {
+            "stamp":{
+                "sec": 0,
+                "nanosec": 0,
+            },
+            "thumb_matrix":[[-1] * 6 for _ in range(12)],
+            "index_matrix":[[-1] * 6 for _ in range(12)],
+            "middle_matrix":[[-1] * 6 for _ in range(12)],
+            "ring_matrix":[[-1] * 6 for _ in range(12)],
+            "little_matrix":[[-1] * 6 for _ in range(12)]
+        }
+        # 压感矩阵合值，单位g 克
+        self.matrix_mass_dic = {
+            "stamp":{
+                "secs": 0,
+                "nsecs": 0,
+            },
+            "thumb_mass":[-1],
+            "index_mass":[-1],
+            "middle_mass":[-1],
+            "ring_mass":[-1],
+            "little_mass":[-1]
+        }
+        self.last_hand_info = {
+            "version": [-1], # Dexterous hand version number
+            "hand_joint": self.hand_joint, # Dexterous hand joint type
+            "speed": [-1] * 10, # Current speed threshold of the dexterous hand
+            "current": [-1] * 10, # Current of the dexterous hand
+            "fault": [-1] * 10, # Current fault of the dexterous hand
+            "motor_temperature": [-1] * 10, # Current motor temperature of the dexterous hand
+            "torque": [-1] * 10, # Current torque of the dexterous hand
+            "is_touch":self.is_touch,
+            "touch_type": -1,
+            "finger_order": None # Finger motor order
+        }
+        self.version = []
+        self.touch_type = -1
+        self.hz = 1.0/60.0
+
+        self.hand_setting_sub = self.create_subscription(String,'/cb_hand_setting_cmd', self.hand_setting_cb, 10)
+        self._init_hand()
+        time.sleep(1)
+        self.run_count = 0 # 计数器，用于记录运行次数
+        self.timer = self.create_timer(0.01, self.run)  # 100 Hz
+        self.thread_pub_state = threading.Thread(target=self.pub_state)
+        self.thread_pub_state.daemon = True
+        self.thread_pub_state.start()
+
+    def _init_hand(self):
+        self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+        time.sleep(0.1)
+        self.touch_type = self.api.get_touch_type()
+        self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+        self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10)
+        if self.is_touch == True:
+            if self.touch_type > 1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+            elif self.touch_type != -1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+                self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+            else:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+                self.is_touch = False
+        self.embedded_version = self.api.get_embedded_version()
+        pose = None
+        torque = [200, 200, 200, 200, 200]
+        speed = [200, 250, 250, 250, 250]
+        if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6" or self.hand_joint.upper() == "L6P":
+            pose = [200, 255, 255, 255, 255, 180]
+            torque = [250, 250, 250, 250, 250, 250]
+            # O6 最大速度阈值
+            speed = [200, 250, 250, 250, 250, 250]
+        elif self.hand_joint == "L7":
+            # The data length of L7 is 7, reinitialize here
+            pose = [255, 200, 255, 255, 255, 255, 180]
+            torque = [250, 250, 250, 250, 250, 250, 250]
+            speed = [120, 250, 250, 250, 250, 250, 250]
+        elif self.hand_joint == "L10":
+            torque = [255] * 10
+            pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+            speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250]
+        elif self.hand_joint == "L20":
+            pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+        elif self.hand_joint == "L21":
+            pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+        elif self.hand_joint == "L25":
+            pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+        if pose is not None:
+            for i in range(1): 
+                self.api.set_speed(speed=speed)
+                time.sleep(0.1)
+                self.api.set_torque(torque=torque)
+                time.sleep(0.1)
+                self.api.finger_move(pose=pose)
+                time.sleep(0.1)
+
+    def list_check(self,pose):
+        if isinstance(pose, list) == False:
+            return False
+        if len(self.last_hand_post_cmd) != len(pose):
+            return False
+        return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+    def hand_control_cb(self, msg):
+        if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+            self.last_hand_post_cmd = msg.position
+        if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+            self.last_hand_vel_cmd = msg.velocity
+        if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+            self.last_hand_eff_cmd = msg.effort
+
+    def run(self):
+        if self.sdk_v == 1:
+            self.sleep_time = 0.009
+        if self.hand_state_pub.get_subscription_count() > 0:
+            # 优先获取手指状态并且发布
+            self.last_hand_state = self.api.get_state()
+            time.sleep(0.003)
+            self.last_hand_vel = self.api.get_joint_speed()
+            time.sleep(0.002)
+        if self.cmd_lock == False:
+            if self.last_hand_post_cmd != None:
+                self.api.finger_move(pose=self.last_hand_post_cmd)
+                self.last_hand_post_cmd = None
+            if self.last_hand_vel_cmd != None:
+                vel = list(self.last_hand_vel_cmd)
+                if all(x == 0 for x in vel):
+                    pass
+                else:
+                    if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
+                        speed = vel
+                        self.api.set_joint_speed(speed=speed)
+                    elif self.hand_joint == "L7" and len(vel) == 7:
+                        speed = vel
+                        self.api.set_joint_speed(speed=speed)
+                    elif self.hand_joint == "L10" and len(vel) == 10:
+                        speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
+                        self.api.set_joint_speed(speed=speed)
+                    elif self.hand_joint == "L20" and len(vel) == 20:
+                        speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
+                        self.api.set_joint_speed(speed=speed)
+                    elif self.hand_joint == "L21" and len(vel) == 25:
+                        speed = vel
+                        self.api.set_joint_speed(speed=speed)
+                    elif self.hand_joint == "L25" and len(vel) == 25:
+                        speed = vel
+                        self.api.set_joint_speed(speed=speed)
+                self.last_hand_vel_cmd = None
+            time.sleep(0.003)
+            if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
+                """单点式压力传感器"""
+                self.force = self.api.get_force()
+            if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
+                """矩阵式压力传感器"""
+                if self.run_count == 3:
+                    self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.sleep_time).tolist()
+                if self.run_count == 4:
+                    self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.sleep_time).tolist()
+                if self.run_count == 5:
+                    self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.sleep_time).tolist()
+                if self.run_count == 6:
+                    self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.sleep_time).tolist()
+                if self.run_count == 7:
+                    self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist()
+                time.sleep(0.005)
+            if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0:
+                """手部信息"""
+                self.last_hand_info = {
+                    "version": self.embedded_version, # Dexterous hand version number
+                    "hand_joint": self.hand_joint, # Dexterous hand joint type
+                    "speed": self.api.get_speed(), # Current speed threshold of the dexterous hand
+                    "current": self.api.get_current(), # Current of the dexterous hand
+                    "fault": self.api.get_fault(), # Current fault of the dexterous hand
+                    "motor_temperature": self.api.get_temperature(), # Current motor temperature of the dexterous hand
+                    "torque": self.api.get_torque(), # Current torque of the dexterous hand
+                    "is_touch":self.is_touch,
+                    "touch_type": self.touch_type,
+                    "finger_order": self.api.get_finger_order() # Finger motor order
+                }
+            if self.run_count == 9:
+                self.run_count = 0
+            self.run_count += 1
+            time.sleep(0.003)
+
+
+    def pub_state(self):
+        while True:
+            if self.hand_state_pub.get_subscription_count() > 0:
+                msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+                self.hand_state_pub.publish(msg)
+            if self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
+                msg = Float32MultiArray()
+                msg.data = [float(val) for sublist in self.force for val in sublist]
+                self.touch_pub.publish(msg)
+            if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
+                # 发布矩阵压感数据JSON格式
+                self.pub_matrix_dic()
+                # 发布矩阵压感和值JSON格式
+                self.pub_matrix_mass(dic=self.matrix_dic)
+                # 发布矩阵压感点云格式
+                self.pub_matrix_point_cloud()
+            if self.hand_info_pub.get_subscription_count() > 0:
+                msg = String()
+                msg.data = json.dumps(self.last_hand_info)
+                self.hand_info_pub.publish(msg)
+            time.sleep(self.hz)
+
+    def pub_matrix_mass(self, dic):
+        """发布矩阵数据合值 单位g 克 JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_mass_dic["stamp"]["secs"] = t_secs
+        self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+        self.matrix_mass_dic["unit"] = "g"
+        self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+        self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+        self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+        self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+        self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+        msg.data = json.dumps(self.matrix_mass_dic)
+        self.matrix_touch_mass_pub.publish(msg)
+
+    def pub_matrix_point_cloud(self):
+        """发布矩阵数据点云格式"""
+        tmp_dic = self.matrix_dic.copy()
+        del tmp_dic['stamp']               # 去掉时间戳字段
+        all_matrices = list(tmp_dic.values())  # 5 帧，每帧 6×12=72 个数
+        # 摊平到一维：360 个 float
+        flat_list = [v for frame in all_matrices for v in frame]  # 360
+        flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+        fields = [PointField(
+            name='val',
+            offset=0,
+            datatype=PointField.UINT8,
+            count=1
+        )]
+        pc = PointCloud2()
+        pc.header.stamp = self.stamp_clock.now().to_msg()
+        pc.header.frame_id = ''
+        pc.height = 1
+        pc.width = flat.size         # 360
+        pc.fields = fields
+        pc.is_bigendian = False
+        pc.point_step = 1            # 1 个 float32
+        pc.row_step = pc.point_step * pc.width
+        pc.data = flat.tobytes()     # 1440 字节
+        self.matrix_touch_pub_pc.publish(pc)
+
+    def pub_matrix_dic(self):
+        """发布矩阵数据JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_dic["stamp"]["secs"] = t_secs
+        self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+        msg.data = json.dumps(self.matrix_dic)
+        self.matrix_touch_pub.publish(msg)
+
+    def joint_state_msg(self, pose,vel=[]):
+        joint_state = JointState()
+        joint_state.header = Header()
+        joint_state.header.stamp = self.get_clock().now().to_msg()
+        joint_state.name = self.api.get_finger_order()
+        joint_state.position = [float(x) for x in pose]
+        if len(vel) > 1:
+            joint_state.velocity = [float(x) for x in vel]
+        else:
+            joint_state.velocity = [0.0] * len(pose)
+        joint_state.effort = [0.0] * len(pose)
+        return joint_state
+    
+    
+
+    
+
+    def hand_setting_cb(self,msg):
+        '''控制命令回调'''
+        data = json.loads(msg.data)
+        print(f"Received setting command: {data['setting_cmd']}",flush=True)
+        try:
+            if data["params"]["hand_type"] == "left":
+                hand = self.api
+                hand_left = True
+            elif data["params"]["hand_type"] == "right":
+                hand = self.api
+                hand_right = True
+            else:
+                print("Please specify the hand part to be set",flush=True)
+                return
+            self.cmd_lock = True
+            # Set maximum torque
+            if data["setting_cmd"] == "set_max_torque_limits": # Set maximum torque
+                torque = list(data["params"]["torque"])
+                hand.set_torque(torque=torque)
+                
+            if data["setting_cmd"] == "set_speed": # Set speed
+                if isinstance(data["params"]["speed"], list) == True:
+                    speed = data["params"]["speed"]
+                    hand.set_speed(speed=speed)
+                else:
+                    ColorMsg(msg=f"Speed parameter error, speed must be a list", color="red")
+            if data["setting_cmd"] == "clear_faults": # Clear faults
+                if hand_left == True and self.hand_joint == "L10" :
+                    ColorMsg(msg=f"L10 left hand cannot clear faults")
+                elif hand_right == True and self.hand_joint == "L10" :
+                    ColorMsg(msg=f"L10 right hand cannot clear faults")
+                else:
+                    hand.clear_faults()
+            if data["setting_cmd"] == "get_faults": # Get faults
+                f = hand.get_fault()
+                ColorMsg(msg=f"Get faults: {f}")
+            if data["setting_cmd"] == "electric_current": # Get current
+                ColorMsg(msg=f"Get current: {hand.get_current()}")
+            if data["setting_cmd"] == "set_electric_current": # Set current
+                if isinstance(data["params"]["current"], list) == True:
+                    hand.set_current(data["params"]["current"])
+            if data["setting_cmd"] == "show_fun_table": # Get faults
+                f = hand.show_fun_table()
+        except:
+            print("命令参数错误")
+            self.cmd_lock = False
+        finally:
+            self.cmd_lock = False
+
+
+    def close_can(self):
+        self.api.open_can.close_can(can=self.can)
+        sys.exit(0)
+
+        
+def main(args=None):
+    try:
+        rclpy.init(args=args)
+        node = LinkerHand("linker_hand_sdk")
+        embedded_version = node.embedded_version
+        if len(embedded_version) == 3 or node.hand_joint.upper() == "O6" or node.hand_joint.upper() == "L6" or node.hand_joint.upper() == "G20":
+            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+            node.sdk_v = 2
+        elif len(embedded_version) == 6 and node.hand_joint == "L10":
+            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+            node.sdk_v = 2
+        elif len(embedded_version) > 4 and ((embedded_version[0]==10 and embedded_version[4]>35) or (embedded_version[0]==7 and embedded_version[4]>50) or (embedded_version[0] == 6)):
+            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+            node.sdk_v = 2
+        else:
+            ColorMsg(msg=f"SDK V1", color="green")
+            node.sdk_v = 1
+        rclpy.spin(node)         # 主循环，监听 ROS 回调
+    except KeyboardInterrupt:
+        print("收到 Ctrl+C，准备退出...")
+    finally:
+        # node.close_can()         # 关闭 CAN 或其他硬件资源
+        # node.destroy_node()      # 销毁 ROS 节点
+        # rclpy.shutdown()         # 关闭 ROS
+        print("程序已退出。")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py

@@ -0,0 +1,258 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+
+from re import A
+import rclpy,sys                                     # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node                      # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+# Linker Hand 型号
+HAND_JOINT = "G20"
+# 默认手指关节位置
+DEFAULT_POSITION =  [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+# 默认手指关节速度
+DEFAULT_SPEED=[255, 255, 255, 255, 255]
+# 默认手指关节力矩
+DEFAULT_TORQUE = [255, 255, 255, 255, 255]
+# 压感传感器延迟时间
+TOUCH_SLEEP_TIME = 0.003
+
+
+class LinkerHandAdvancedG20(Node):
+    def __init__(self, name, hand_type, can, is_touch):
+        super().__init__(name)       
+        self.hand_type = hand_type
+        self.hand_joint = HAND_JOINT
+        if is_touch == "true":
+            self.is_touch = True
+        else:
+            self.is_touch = False
+        self.can = can
+        self.modbus = "None"
+        time.sleep(0.1)
+        self._check_linker_hand_type()
+        self.last_hand_post_cmd = None # 最新手指位置命令
+        self.last_hand_vel_cmd = None # 最新手指速度命令
+        self.last_hand_eff_cmd = None # 最新手指力矩命令
+        self.matrix_dic = {
+            "stamp":{
+                "sec": 0,
+                "nanosec": 0,
+            },
+            "thumb_matrix":[[-1] * 6 for _ in range(12)],
+            "index_matrix":[[-1] * 6 for _ in range(12)],
+            "middle_matrix":[[-1] * 6 for _ in range(12)],
+            "ring_matrix":[[-1] * 6 for _ in range(12)],
+            "little_matrix":[[-1] * 6 for _ in range(12)]
+        }
+        # 压感矩阵合值，单位g 克
+        self.matrix_mass_dic = {
+            "stamp":{
+                "secs": 0,
+                "nsecs": 0,
+            },
+            "thumb_mass":[-1],
+            "index_mass":[-1],
+            "middle_mass":[-1],
+            "ring_mass":[-1],
+            "little_mass":[-1]
+        }
+        self.hz = 1.0/60.0
+        # ros时间获取
+        self.stamp_clock = Clock()
+        self._init_hand()
+        time.sleep(2)
+        self.count = 0
+        self.timer = self.create_timer(self.hz, self.run)  # 100 Hz
+
+
+    def _check_linker_hand_type(self):
+        if self.modbus != "None":
+            ColorMsg(msg=f"Modbus暂不支持", color="red")
+            sys.exit(0)
+        if self.hand_joint.upper() != "G20":
+            ColorMsg(msg=f"Linker Hand hand_joint参数错误", color="red")
+            sys.exit(0)
+
+    def _init_hand(self):
+        self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+        time.sleep(0.1)
+        self.touch_type = self.api.get_touch_type()
+        self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+        if self.is_touch == True:
+            if self.touch_type > 1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+            elif self.touch_type != -1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+                self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+            else:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+                self.is_touch = False
+        self.embedded_version = self.api.get_embedded_version()
+        self.api.set_speed(speed=DEFAULT_SPEED)
+        time.sleep(0.1)
+        self.api.set_torque(torque=DEFAULT_TORQUE)
+        time.sleep(0.1)
+        self.api.finger_move(pose=DEFAULT_POSITION)
+        time.sleep(0.1)
+
+    def hand_control_cb(self, msg):
+        if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+            self.last_hand_post_cmd = msg.position
+        if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+            self.last_hand_vel_cmd = msg.velocity
+        if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+            self.last_hand_eff_cmd = msg.effort
+    
+    def list_check(self,pose):
+        if isinstance(pose, list) == False:
+            return False
+        if len(self.last_hand_post_cmd) != len(pose):
+            return False
+        return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+    
+    def joint_state_msg(self, pose,vel=[]):
+        joint_state = JointState()
+        joint_state.header = Header()
+        joint_state.header.stamp = self.get_clock().now().to_msg()
+        joint_state.name = self.api.get_finger_order()
+        joint_state.position = [float(x) for x in pose]
+        if len(vel) > 1:
+            joint_state.velocity = [float(x) for x in vel]
+        else:
+            joint_state.velocity = [0.0] * len(pose)
+        joint_state.effort = [0.0] * len(pose)
+        return joint_state
+
+    def run(self):
+        # 执行手控制指令
+        if self.last_hand_post_cmd != None:
+            self.api.finger_move(pose=self.last_hand_post_cmd)
+            self.last_hand_post_cmd = None
+        # 优先获取手指状态并且发布
+        self.last_hand_state = self.api.get_state()
+        self.last_hand_vel = [0.0] * len(self.last_hand_state)
+        # 发布手状态
+        msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+        self.hand_state_pub.publish(msg_state)
+        
+        if self.is_touch == True:
+            # 获取压感数据
+            if self.count == 2:
+                self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+            if self.count == 4:
+                self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+            if self.count == 6:
+                self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+            if self.count == 8:
+                self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+            if self.count == 10:
+                self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+            # 发布矩阵压感数据JSON格式
+            self.pub_matrix_dic()
+            # 发布矩阵压感和值JSON格式
+            self.pub_matrix_mass(dic=self.matrix_dic)
+            # 发布矩阵压感点云格式
+            self.pub_matrix_point_cloud()
+        self.count += 1
+        if self.count == 11:
+            self.count = 0
+
+    def pub_matrix_dic(self):
+        """发布矩阵数据JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_dic["stamp"]["secs"] = t_secs
+        self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+        msg.data = json.dumps(self.matrix_dic)
+        self.matrix_touch_pub.publish(msg)
+
+    def pub_matrix_mass(self, dic):
+        """发布矩阵数据合值 单位g 克 JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_mass_dic["stamp"]["secs"] = t_secs
+        self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+        self.matrix_mass_dic["unit"] = "g"
+        self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+        self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+        self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+        self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+        self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+        msg.data = json.dumps(self.matrix_mass_dic)
+        self.matrix_touch_mass_pub.publish(msg)
+
+    def pub_matrix_point_cloud(self):
+        tmp_dic = self.matrix_dic.copy()
+        del tmp_dic['stamp']               # 去掉时间戳字段
+        all_matrices = list(tmp_dic.values())  # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+        # 摊平到一维
+        flat_list = [v for frame in all_matrices for v in frame]  
+        flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+        fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+        pc = PointCloud2()
+        pc.header.stamp =  self.get_clock().now().to_msg()
+        pc.header.frame_id = ''   # 可改成你需要的坐标系
+        pc.height = 1
+        pc.width = flat.size         # 360
+        pc.fields = fields
+        pc.is_bigendian = False
+        pc.point_step = 1            # 1 个 float32
+        pc.row_step = pc.point_step * pc.width
+        pc.data = flat.tobytes()     # 1440 字节
+        self.matrix_touch_pub_pc.publish(pc)
+
+
+    def close_can(self):
+        self.api.open_can.close_can(can=self.can)
+        sys.exit(0)
+
+
+def main(args=None):
+    '''
+    本节点用于收集手指状态和压感数据。
+    '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+    /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz
+    '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz
+    启动命令:
+    ros2 run linker_hand_ros2_sdk linker_hand_advanced_g20 --hand_type left --can can0 --is_touch true
+    '''
+    try:
+        rclpy.init(args=args)
+        parser = argparse.ArgumentParser()
+        parser.add_argument('--hand_type', required=True)
+        parser.add_argument('--can',        required=True)
+        parser.add_argument('--is_touch',   choices=['true','false'], required=True)
+
+        args = parser.parse_args()
+        node = LinkerHandAdvancedG20(name="linker_hand_advanced_g20",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+        embedded_version = node.embedded_version
+        rclpy.spin(node)         # 主循环，监听 ROS 回调
+    except KeyboardInterrupt:
+        print("收到 Ctrl+C，准备退出...")
+    finally:
+        # node.close_can()         # 关闭 CAN 或其他硬件资源
+        # node.destroy_node()      # 销毁 ROS 节点
+        # rclpy.shutdown()         # 关闭 ROS
+        print("程序已退出。")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py

@@ -0,0 +1,246 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+
+from re import A
+import rclpy,sys                                     # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node                      # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+class LinkerHandAdvancedL10(Node):
+    def __init__(self, name, hand_type, can, is_touch):
+        super().__init__(name)       
+        self.hand_type = hand_type
+        self.hand_joint = "L10"
+        if is_touch == "true":
+            self.is_touch = True
+        else:
+            self.is_touch = False
+        self.can = can
+        self.modbus = "None"
+        time.sleep(0.1)
+        self._check_linker_hand_type()
+        self.last_hand_post_cmd = None # 最新手指位置命令
+        self.last_hand_vel_cmd = None # 最新手指速度命令
+        self.last_hand_eff_cmd = None # 最新手指力矩命令
+        self.count = 0 # 循环计数器
+        self.matrix_dic = {
+            "stamp":{
+                "sec": 0,
+                "nanosec": 0,
+            },
+            "thumb_matrix":[[-1] * 6 for _ in range(12)],
+            "index_matrix":[[-1] * 6 for _ in range(12)],
+            "middle_matrix":[[-1] * 6 for _ in range(12)],
+            "ring_matrix":[[-1] * 6 for _ in range(12)],
+            "little_matrix":[[-1] * 6 for _ in range(12)]
+        }
+        # 压感矩阵合值，单位g 克
+        self.matrix_mass_dic = {
+            "stamp":{
+                "secs": 0,
+                "nsecs": 0,
+            },
+            "thumb_mass":[-1],
+            "index_mass":[-1],
+            "middle_mass":[-1],
+            "ring_mass":[-1],
+            "little_mass":[-1]
+        }
+        self.hz = 1.0/60.0
+        # ros时间获取
+        self.stamp_clock = Clock()
+        self._init_hand()
+        time.sleep(2)
+        self.timer = self.create_timer(self.hz, self.run)  # 100 Hz
+
+
+    def _check_linker_hand_type(self):
+        if self.modbus != "None":
+            ColorMsg(msg=f"Modbus暂不支持", color="red")
+            sys.exit(0)
+        if self.hand_joint.upper() != "L10":
+            ColorMsg(msg=f"L10以外其他Linker Hand暂不支持", color="red")
+            sys.exit(0)
+
+
+    def _init_hand(self):
+        self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+        time.sleep(0.1)
+        self.touch_type = self.api.get_touch_type()
+        self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+        if self.is_touch == True:
+            if self.touch_type > 1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+            elif self.touch_type != -1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+                self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+            else:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+                self.is_touch = False
+        self.embedded_version = self.api.get_embedded_version()
+        if self.hand_joint.upper() == "L10":
+            pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+            torque = [255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+            speed = [255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+            self.api.set_speed(speed=speed)
+            time.sleep(0.1)
+            self.api.set_torque(torque=torque)
+            time.sleep(0.1)
+            self.api.finger_move(pose=pose)
+            time.sleep(0.1)
+        self.serial_number = self.api.get_serial_number()
+
+    def hand_control_cb(self, msg):
+        if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+            self.last_hand_post_cmd = msg.position
+        if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+            self.last_hand_vel_cmd = msg.velocity
+        if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+            self.last_hand_eff_cmd = msg.effort
+    
+    def list_check(self,pose):
+        if isinstance(pose, list) == False:
+            return False
+        if len(self.last_hand_post_cmd) != len(pose):
+            return False
+        return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+    
+    def joint_state_msg(self, pose,vel=[]):
+        joint_state = JointState()
+        joint_state.header = Header()
+        joint_state.header.stamp = self.get_clock().now().to_msg()
+        joint_state.name = self.api.get_finger_order()
+        joint_state.position = [float(x) for x in pose]
+        if len(vel) > 1:
+            joint_state.velocity = [float(x) for x in vel]
+        else:
+            joint_state.velocity = [0.0] * len(pose)
+        joint_state.effort = [0.0] * len(pose)
+        return joint_state
+
+    def run(self):
+        # 执行手控制指令
+        if self.last_hand_post_cmd != None:
+            self.api.finger_move(pose=self.last_hand_post_cmd)
+            self.last_hand_post_cmd = None
+        # 优先获取手指状态并且发布
+        self.last_hand_state = self.api.get_state()
+        self.last_hand_vel = [0.0] * len(self.last_hand_state)
+        # 发布手状态
+        msg_state = self.joint_state_msg(self.last_hand_state)
+        self.hand_state_pub.publish(msg_state)
+        # 获取压感数据
+        if self.is_touch == True:
+            self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.003).tolist()
+            self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.004).tolist()
+            self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.003).tolist()
+            self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.004).tolist()
+            self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.004).tolist()
+            # 发布矩阵压感数据JSON格式
+            self.pub_matrix_dic()
+            # 发布矩阵压感和值JSON格式
+            self.pub_matrix_mass(dic=self.matrix_dic)
+            # 发布矩阵压感点云格式
+            self.pub_matrix_point_cloud()
+        
+
+    def pub_matrix_dic(self):
+        """发布矩阵数据JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_dic["stamp"]["secs"] = t_secs
+        self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+        msg.data = json.dumps(self.matrix_dic)
+        self.matrix_touch_pub.publish(msg)
+
+    def pub_matrix_mass(self, dic):
+        """发布矩阵数据合值 单位g 克 JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_mass_dic["stamp"]["secs"] = t_secs
+        self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+        self.matrix_mass_dic["unit"] = "g"
+        self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+        self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+        self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+        self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+        self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+        msg.data = json.dumps(self.matrix_mass_dic)
+        self.matrix_touch_mass_pub.publish(msg)
+
+    def pub_matrix_point_cloud(self):
+        tmp_dic = self.matrix_dic.copy()
+        del tmp_dic['stamp']               # 去掉时间戳字段
+        all_matrices = list(tmp_dic.values())  # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+        # 摊平到一维
+        flat_list = [v for frame in all_matrices for v in frame]  
+        flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+        fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+        pc = PointCloud2()
+        pc.header.stamp =  self.get_clock().now().to_msg()
+        pc.header.frame_id = ''   # 可改成你需要的坐标系
+        pc.height = 1
+        pc.width = flat.size         # 360
+        pc.fields = fields
+        pc.is_bigendian = False
+        pc.point_step = 1            # 1 个 float32
+        pc.row_step = pc.point_step * pc.width
+        pc.data = flat.tobytes()     # 1440 字节
+        self.matrix_touch_pub_pc.publish(pc)
+
+
+    def close_can(self):
+        self.api.open_can.close_can(can=self.can)
+        sys.exit(0)
+
+
+def main(args=None):
+    '''
+    本节点用于收集手指状态和压感数据。
+    '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+    /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz
+    '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz
+    '/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+    启动命令:
+    ros2 run linker_hand_ros2_sdk linker_hand_advanced_l10 --hand_type left --can can0 --is_touch true
+    '''
+    try:
+        rclpy.init(args=args)
+        parser = argparse.ArgumentParser()
+        parser.add_argument('--hand_type', required=True)
+        parser.add_argument('--can',        required=True)
+        parser.add_argument('--is_touch',   choices=['true','false'], required=True)
+
+        args = parser.parse_args()
+        node = LinkerHandAdvancedL10(name="linker_hand_advanced_l10",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+        embedded_version = node.embedded_version
+        rclpy.spin(node)         # 主循环，监听 ROS 回调
+    except KeyboardInterrupt:
+        print("收到 Ctrl+C，准备退出...")
+    finally:
+        # node.close_can()         # 关闭 CAN 或其他硬件资源
+        # node.destroy_node()      # 销毁 ROS 节点
+        # rclpy.shutdown()         # 关闭 ROS
+        print("程序已退出。")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py

@@ -0,0 +1,251 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys                                     # ROS2 Python接口库
+import time
+import numpy as np
+import argparse
+from rclpy.node import Node                      # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+class LinkerHandAdvancedL6(Node):
+    def __init__(self, name, hand_type, can, is_touch):
+        super().__init__(name)       
+        self.hand_type = hand_type
+        self.hand_joint = "L6"
+        if is_touch == "true":
+            self.is_touch = True
+        else:
+            self.is_touch = False
+        self.can = can
+        self.modbus = "None"
+        time.sleep(0.1)
+        self._check_linker_hand_type()
+        self.last_hand_post_cmd = None # 最新手指位置命令
+        self.last_hand_vel_cmd = None # 最新手指速度命令
+        self.last_hand_eff_cmd = None # 最新手指力矩命令
+        self.count = 0 # 循环计数器
+        self.matrix_dic = {
+            "stamp":{
+                "sec": 0,
+                "nanosec": 0,
+            },
+            "thumb_matrix":[[-1] * 6 for _ in range(12)],
+            "index_matrix":[[-1] * 6 for _ in range(12)],
+            "middle_matrix":[[-1] * 6 for _ in range(12)],
+            "ring_matrix":[[-1] * 6 for _ in range(12)],
+            "little_matrix":[[-1] * 6 for _ in range(12)]
+        }
+        # 压感矩阵合值，单位g 克
+        self.matrix_mass_dic = {
+            "stamp":{
+                "secs": 0,
+                "nsecs": 0,
+            },
+            "thumb_mass":[-1],
+            "index_mass":[-1],
+            "middle_mass":[-1],
+            "ring_mass":[-1],
+            "little_mass":[-1]
+        }
+        self.hz = 1.0/60.0
+        # ros时间获取
+        self.stamp_clock = Clock()
+        self._init_hand()
+        time.sleep(2)
+        self.timer = self.create_timer(self.hz, self.run)  # 60 Hz
+
+
+    def _check_linker_hand_type(self):
+        if self.modbus != "None":
+            ColorMsg(msg=f"Modbus暂不支持", color="red")
+            sys.exit(0)
+        if self.hand_joint.upper() != "L6":
+            ColorMsg(msg=f"L6以外其他Linker Hand暂不支持", color="red")
+            sys.exit(0)
+
+    def _init_hand(self):
+        self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+        time.sleep(0.1)
+        self.touch_type = self.api.get_touch_type()
+        self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+        if self.is_touch == True:
+            if self.touch_type > 1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+            elif self.touch_type != -1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+                self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+            else:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+                self.is_touch = False
+        self.embedded_version = self.api.get_embedded_version()
+        if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6":
+            pose = [200, 255, 255, 255, 255, 180]
+            torque = [255, 255, 255, 255, 255, 255]
+            # O6 最大速度阈值
+            speed = [255, 255, 255, 255, 255, 255]
+            self.api.set_speed(speed=speed)
+            time.sleep(0.1)
+            self.api.set_torque(torque=torque)
+            time.sleep(0.1)
+            self.api.finger_move(pose=pose)
+            time.sleep(0.1)
+
+    def hand_control_cb(self, msg):
+        if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+            self.last_hand_post_cmd = msg.position
+        if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+            self.last_hand_vel_cmd = msg.velocity
+        if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+            self.last_hand_eff_cmd = msg.effort
+    
+    def list_check(self,pose):
+        if isinstance(pose, list) == False:
+            return False
+        if len(self.last_hand_post_cmd) != len(pose):
+            return False
+        return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+    
+    def joint_state_msg(self, pose,vel=[]):
+        joint_state = JointState()
+        joint_state.header = Header()
+        joint_state.header.stamp = self.get_clock().now().to_msg()
+        joint_state.name = self.api.get_finger_order()
+        joint_state.position = [float(x) for x in pose]
+        if len(vel) > 1:
+            joint_state.velocity = [float(x) for x in vel]
+        else:
+            joint_state.velocity = [0.0] * len(pose)
+        joint_state.effort = [0.0] * len(pose)
+        return joint_state
+
+    def run(self):
+        # 执行手控制指令
+        if self.last_hand_post_cmd != None:
+            self.api.finger_move(pose=self.last_hand_post_cmd)
+            self.last_hand_post_cmd = None
+            #time.sleep(0.002)
+        # 优先获取手指状态并且发布
+        self.last_hand_state = self.api.get_state()
+        self.last_hand_vel = self.api.get_joint_speed()
+        # 发布手状态
+        msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+        self.hand_state_pub.publish(msg_state)
+        time.sleep(0.002)
+        # 获取压感数据
+        if self.is_touch == True:
+            self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.003).tolist()
+            self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.003).tolist()
+            self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.003).tolist()
+            self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.003).tolist()
+            self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.003).tolist()
+            # 发布矩阵压感数据JSON格式
+            self.pub_matrix_dic()
+            # 发布矩阵压感和值JSON格式
+            self.pub_matrix_mass(dic=self.matrix_dic)
+            # 发布矩阵压感点云格式
+            self.pub_matrix_point_cloud()
+    
+    def pub_matrix_dic(self):
+        """发布矩阵数据JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_dic["stamp"]["secs"] = t_secs
+        self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+        msg.data = json.dumps(self.matrix_dic)
+        self.matrix_touch_pub.publish(msg)
+
+    def pub_matrix_mass(self, dic):
+        """发布矩阵数据合值 单位g 克 JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_mass_dic["stamp"]["secs"] = t_secs
+        self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+        self.matrix_mass_dic["unit"] = "g"
+        self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+        self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+        self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+        self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+        self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+        msg.data = json.dumps(self.matrix_mass_dic)
+        self.matrix_touch_mass_pub.publish(msg)
+
+    def pub_matrix_point_cloud(self):
+        tmp_dic = self.matrix_dic.copy()
+        del tmp_dic['stamp']               # 去掉时间戳字段
+        all_matrices = list(tmp_dic.values())  # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+        # 摊平到一维
+        flat_list = [v for frame in all_matrices for v in frame]  
+        flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+        fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+        pc = PointCloud2()
+        pc.header.stamp =  self.get_clock().now().to_msg()
+        pc.header.frame_id = ''   # 可改成你需要的坐标系
+        pc.height = 1
+        pc.width = flat.size         # 360
+        pc.fields = fields
+        pc.is_bigendian = False
+        pc.point_step = 1            # 1 个 float32
+        pc.row_step = pc.point_step * pc.width
+        pc.data = flat.tobytes()     # 1440 字节
+        self.matrix_touch_pub_pc.publish(pc)
+
+
+
+    def close_can(self):
+        self.api.open_can.close_can(can=self.can)
+        sys.exit(0)
+
+
+def main(args=None):
+    '''
+    本节点用于收集手指状态和压感数据。
+    '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+    /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz
+    '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 40Hz
+    启动命令:
+    ros2 run linker_hand_ros2_sdk linker_hand_advanced_l6 --hand_type left --can can0 --is_touch true
+    '''
+    try:
+        rclpy.init(args=args)
+        parser = argparse.ArgumentParser()
+        parser.add_argument('--hand_type', required=True)
+        parser.add_argument('--can',        required=True)
+        parser.add_argument('--is_touch',   choices=['true','false'], required=True)
+
+        args = parser.parse_args()
+        node = LinkerHandAdvancedL6(name="linker_hand_advanced_l6",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+        embedded_version = node.embedded_version
+        if embedded_version[2] < 8 and len(embedded_version) != 3:
+            ColorMsg(msg=f"固件版本过低，请升级固件到V{embedded_version[0]}.{embedded_version[1]}.8及以上版本", color="red")
+            sys.exit(0)
+        rclpy.spin(node)         # 主循环，监听 ROS 回调
+    except KeyboardInterrupt:
+        print("收到 Ctrl+C，准备退出...")
+    finally:
+        # node.close_can()         # 关闭 CAN 或其他硬件资源
+        # node.destroy_node()      # 销毁 ROS 节点
+        # rclpy.shutdown()         # 关闭 ROS
+        print("程序已退出。")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py

@@ -0,0 +1,281 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys                                     # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node                      # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+class LinkerHandAdvancedL7(Node):
+    def __init__(self, name, hand_type, can, is_touch):
+        super().__init__(name)       
+        self.hand_type = hand_type
+        self.hand_joint = "L7"
+        if is_touch == "true":
+            self.is_touch = True
+        else:
+            self.is_touch = False
+        self.can = can
+        self.modbus = "None"
+        time.sleep(0.1)
+        self._check_linker_hand_type()
+        self.last_hand_post_cmd = None # 最新手指位置命令
+        self.last_hand_vel_cmd = None # 最新手指速度命令
+        self.last_hand_eff_cmd = None # 最新手指力矩命令
+        self.count = 0 # 循环计数器
+        self.matrix_dic = {
+            "stamp":{
+                "sec": 0,
+                "nanosec": 0,
+            },
+            "thumb_matrix":[[-1] * 6 for _ in range(12)],
+            "index_matrix":[[-1] * 6 for _ in range(12)],
+            "middle_matrix":[[-1] * 6 for _ in range(12)],
+            "ring_matrix":[[-1] * 6 for _ in range(12)],
+            "little_matrix":[[-1] * 6 for _ in range(12)]
+        }
+        # 压感矩阵合值，单位g 克
+        self.matrix_mass_dic = {
+            "stamp":{
+                "secs": 0,
+                "nsecs": 0,
+            },
+            "thumb_mass":[-1],
+            "index_mass":[-1],
+            "middle_mass":[-1],
+            "ring_mass":[-1],
+            "little_mass":[-1]
+        }
+        self.hz = 1.0/60.0
+        # ros时间获取
+        self.stamp_clock = Clock()
+        self._init_hand()
+        time.sleep(2)
+        self.timer = self.create_timer(self.hz, self.run)  # 100 Hz
+
+
+    def _check_linker_hand_type(self):
+        if self.modbus != "None":
+            ColorMsg(msg=f"Modbus暂不支持", color="red")
+            sys.exit(0)
+        if self.hand_joint.upper() != "L7":
+            ColorMsg(msg=f"L6以外其他Linker Hand暂不支持", color="red")
+            sys.exit(0)
+
+
+    def _init_hand(self):
+        self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+        time.sleep(0.1)
+        self.touch_type = self.api.get_touch_type()
+        self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+        if self.is_touch == True:
+            if self.touch_type > 1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+            elif self.touch_type != -1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+                self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+            else:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+                self.is_touch = False
+        self.embedded_version = self.api.get_embedded_version()
+        if self.hand_joint.upper() == "L7":
+            pose = [255, 200, 255, 255, 255, 255, 180]
+            torque = [255] * 7
+            speed = [255] * 7
+            self.api.set_speed(speed=speed)
+            time.sleep(0.1)
+            self.api.set_torque(torque=torque)
+            time.sleep(0.1)
+            self.api.finger_move(pose=pose)
+            time.sleep(0.1)
+        self.serial_number = self.api.get_serial_number()
+
+    def hand_control_cb(self, msg):
+        if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+            self.last_hand_post_cmd = msg.position
+        if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+            self.last_hand_vel_cmd = msg.velocity
+        if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+            self.last_hand_eff_cmd = msg.effort
+    
+    def list_check(self,pose):
+        if isinstance(pose, list) == False:
+            return False
+        if len(self.last_hand_post_cmd) != len(pose):
+            return False
+        return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+    
+    def joint_state_msg(self, pose,vel=[]):
+        joint_state = JointState()
+        joint_state.header = Header()
+        joint_state.header.stamp = self.get_clock().now().to_msg()
+        joint_state.name = self.api.get_finger_order()
+        joint_state.position = [float(x) for x in pose]
+        if len(vel) > 1:
+            joint_state.velocity = [float(x) for x in vel]
+        else:
+            joint_state.velocity = [0.0] * len(pose)
+        joint_state.effort = [0.0] * len(pose)
+        return joint_state
+
+    def run(self):
+        # 优先获取手指状态并且发布
+        self.last_hand_state = self.api.get_state()
+        self.last_hand_vel = self.api.get_joint_speed()
+        # 发布手状态
+        msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+        self.hand_state_pub.publish(msg_state)
+        # 执行手控制指令
+        if self.last_hand_post_cmd != None:
+            self.api.finger_move(pose=self.last_hand_post_cmd)
+            time.sleep(0.003)
+            self.last_hand_post_cmd = None
+        if self.last_hand_vel_cmd != None:
+            vel = list(self.last_hand_vel_cmd)
+            if all(x == 0 for x in vel):
+                pass
+            else:
+                if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
+                    speed = vel
+                    self.api.set_joint_speed(speed=speed)
+                elif self.hand_joint == "L7" and len(vel) == 7:
+                    speed = vel
+                    self.api.set_joint_speed(speed=speed)
+                elif self.hand_joint == "L10" and len(vel) == 10:
+                    speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
+                    self.api.set_joint_speed(speed=speed)
+                elif self.hand_joint == "L20" and len(vel) == 20:
+                    speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
+                    self.api.set_joint_speed(speed=speed)
+                elif self.hand_joint == "L21" and len(vel) == 25:
+                    speed = vel
+                    self.api.set_joint_speed(speed=speed)
+                elif self.hand_joint == "L25" and len(vel) == 25:
+                    speed = vel
+                    self.api.set_joint_speed(speed=speed)
+            self.last_hand_vel_cmd = None
+        time.sleep(0.005)
+        # 获取压感数据
+        if self.is_touch == True:
+            if self.count == 3:
+                self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.006).tolist()
+            if self.count == 4:
+                self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.006).tolist()
+            if self.count == 5:
+                self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.006).tolist()
+            if self.count == 6:
+                self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.006).tolist()
+            if self.count == 7:
+                self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.006).tolist()
+            # 发布矩阵压感数据JSON格式
+            self.pub_matrix_dic()
+            # 发布矩阵压感和值JSON格式
+            self.pub_matrix_mass(dic=self.matrix_dic)
+            # 发布矩阵压感点云格式
+            self.pub_matrix_point_cloud()
+        self.count += 1
+        if self.count == 8:
+            self.count = 0
+        time.sleep(0.006)
+
+    def pub_matrix_dic(self):
+        """发布矩阵数据JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_dic["stamp"]["secs"] = t_secs
+        self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+        msg.data = json.dumps(self.matrix_dic)
+        self.matrix_touch_pub.publish(msg)
+
+    def pub_matrix_mass(self, dic):
+        """发布矩阵数据合值 单位g 克 JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_mass_dic["stamp"]["secs"] = t_secs
+        self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+        self.matrix_mass_dic["unit"] = "g"
+        self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+        self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+        self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+        self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+        self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+        msg.data = json.dumps(self.matrix_mass_dic)
+        self.matrix_touch_mass_pub.publish(msg)
+
+    def pub_matrix_point_cloud(self):
+        tmp_dic = self.matrix_dic.copy()
+        del tmp_dic['stamp']               # 去掉时间戳字段
+        all_matrices = list(tmp_dic.values())  # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+        # 摊平到一维
+        flat_list = [v for frame in all_matrices for v in frame]  
+        flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+        fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+        pc = PointCloud2()
+        pc.header.stamp =  self.get_clock().now().to_msg()
+        pc.header.frame_id = ''   # 可改成你需要的坐标系
+        pc.height = 1
+        pc.width = flat.size         # 360
+        pc.fields = fields
+        pc.is_bigendian = False
+        pc.point_step = 1            # 1 个 float32
+        pc.row_step = pc.point_step * pc.width
+        pc.data = flat.tobytes()     # 1440 字节
+        self.matrix_touch_pub_pc.publish(pc)
+
+
+    def close_can(self):
+        self.api.open_can.close_can(can=self.can)
+        sys.exit(0)
+
+
+def main(args=None):
+    '''
+    本节点用于收集手指状态和压感数据。
+    '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+    /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz
+    '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 40Hz
+    启动命令:
+    ros2 run linker_hand_ros2_sdk linker_hand_advanced_l7 --hand_type left --can can0 --is_touch true
+    '''
+    try:
+        rclpy.init(args=args)
+        parser = argparse.ArgumentParser()
+        parser.add_argument('--hand_type', required=True)
+        parser.add_argument('--can',        required=True)
+        parser.add_argument('--is_touch',   choices=['true','false'], required=True)
+
+        args = parser.parse_args()
+        node = LinkerHandAdvancedL7(name="linker_hand_collect_l7",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+        embedded_version = node.embedded_version
+        rclpy.spin(node)         # 主循环，监听 ROS 回调
+    except KeyboardInterrupt:
+        print("收到 Ctrl+C，准备退出...")
+    finally:
+        # node.close_can()         # 关闭 CAN 或其他硬件资源
+        # node.destroy_node()      # 销毁 ROS 节点
+        # rclpy.shutdown()         # 关闭 ROS
+        print("程序已退出。")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py

@@ -0,0 +1,252 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys                                     # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node                      # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+class LinkerHandAdvancedO6(Node):
+    def __init__(self, name, hand_type, can, is_touch):
+        super().__init__(name)       
+        self.hand_type = hand_type
+        self.hand_joint = "O6"
+        if is_touch == "true":
+            self.is_touch = True
+        else:
+            self.is_touch = False
+        self.can = can
+        self.modbus = "None"
+        time.sleep(0.1)
+        self._check_linker_hand_type()
+        self.last_hand_post_cmd = None # 最新手指位置命令
+        self.last_hand_vel_cmd = None # 最新手指速度命令
+        self.last_hand_eff_cmd = None # 最新手指力矩命令
+        self.matrix_dic = {
+            "stamp":{
+                "sec": 0,
+                "nanosec": 0,
+            },
+            "thumb_matrix":[[-1] * 6 for _ in range(12)],
+            "index_matrix":[[-1] * 6 for _ in range(12)],
+            "middle_matrix":[[-1] * 6 for _ in range(12)],
+            "ring_matrix":[[-1] * 6 for _ in range(12)],
+            "little_matrix":[[-1] * 6 for _ in range(12)]
+        }
+        # 压感矩阵合值，单位g 克
+        self.matrix_mass_dic = {
+            "stamp":{
+                "secs": 0,
+                "nsecs": 0,
+            },
+            "thumb_mass":[-1],
+            "index_mass":[-1],
+            "middle_mass":[-1],
+            "ring_mass":[-1],
+            "little_mass":[-1]
+        }
+        self.hz = 1.0/60.0
+        # ros时间获取
+        self.stamp_clock = Clock()
+        self._init_hand()
+        time.sleep(2)
+        self.timer = self.create_timer(self.hz, self.run)  # 100 Hz
+
+
+    def _check_linker_hand_type(self):
+        if self.modbus != "None":
+            ColorMsg(msg=f"Modbus暂不支持", color="red")
+            sys.exit(0)
+        if self.hand_joint.upper() != "O6":
+            ColorMsg(msg=f"O6以外其他Linker Hand暂不支持", color="red")
+            sys.exit(0)
+
+    def _init_hand(self):
+        self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+        time.sleep(0.1)
+        self.touch_type = self.api.get_touch_type()
+        self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+        if self.is_touch == True:
+            if self.touch_type > 1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+            elif self.touch_type != -1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+                self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+            else:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+                self.is_touch = False
+        self.embedded_version = self.api.get_embedded_version()
+        if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6":
+            pose = [200, 255, 255, 255, 255, 180]
+            torque = [255, 255, 255, 255, 255, 255]
+            # O6 最大速度阈值
+            speed = [255, 255, 255, 255, 255, 255]
+            self.api.set_speed(speed=speed)
+            time.sleep(0.1)
+            self.api.set_torque(torque=torque)
+            time.sleep(0.1)
+            self.api.finger_move(pose=pose)
+            time.sleep(0.1)
+
+    def hand_control_cb(self, msg):
+        if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+            self.last_hand_post_cmd = msg.position
+        if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+            self.last_hand_vel_cmd = msg.velocity
+        if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+            self.last_hand_eff_cmd = msg.effort
+    
+    def list_check(self,pose):
+        if isinstance(pose, list) == False:
+            return False
+        if len(self.last_hand_post_cmd) != len(pose):
+            return False
+        return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+    
+    def joint_state_msg(self, pose,vel=[]):
+        joint_state = JointState()
+        joint_state.header = Header()
+        joint_state.header.stamp = self.get_clock().now().to_msg()
+        joint_state.name = self.api.get_finger_order()
+        joint_state.position = [float(x) for x in pose]
+        if len(vel) > 1:
+            joint_state.velocity = [float(x) for x in vel]
+        else:
+            joint_state.velocity = [0.0] * len(pose)
+        joint_state.effort = [0.0] * len(pose)
+        return joint_state
+
+    def run(self):
+        # 执行手控制指令
+        if self.last_hand_post_cmd != None:
+            self.api.finger_move(pose=self.last_hand_post_cmd)
+            self.last_hand_post_cmd = None
+        if self.last_hand_vel_cmd != None:
+            vel = list(self.last_hand_vel_cmd)
+            if all(x == 0 for x in vel):
+                pass
+            else:
+                speed = vel
+                self.api.set_joint_speed(speed=speed)
+            self.last_hand_vel_cmd = None
+        # 优先获取手指状态并且发布
+        self.last_hand_state = self.api.get_state()
+        self.last_hand_vel = self.api.get_joint_speed()
+        # 发布手状态
+        msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+        self.hand_state_pub.publish(msg_state)
+        if self.is_touch == True:
+            # 获取压感数据
+            self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.002).tolist()
+            self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.002).tolist()
+            self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.002).tolist()
+            self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.002).tolist()
+            self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.002).tolist()
+            # 发布矩阵压感数据JSON格式
+            self.pub_matrix_dic()
+            # 发布矩阵压感和值JSON格式
+            self.pub_matrix_mass(dic=self.matrix_dic)
+            # 发布矩阵压感点云格式
+            self.pub_matrix_point_cloud()
+    
+    def pub_matrix_dic(self):
+        """发布矩阵数据JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_dic["stamp"]["secs"] = t_secs
+        self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+        msg.data = json.dumps(self.matrix_dic)
+        self.matrix_touch_pub.publish(msg)
+
+    def pub_matrix_mass(self, dic):
+        """发布矩阵数据合值 单位g 克 JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_mass_dic["stamp"]["secs"] = t_secs
+        self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+        self.matrix_mass_dic["unit"] = "g"
+        self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+        self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+        self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+        self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+        self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+        msg.data = json.dumps(self.matrix_mass_dic)
+        self.matrix_touch_mass_pub.publish(msg)
+
+    def pub_matrix_point_cloud(self):
+        tmp_dic = self.matrix_dic.copy()
+        del tmp_dic['stamp']               # 去掉时间戳字段
+        all_matrices = list(tmp_dic.values())  # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+        # 摊平到一维
+        flat_list = [v for frame in all_matrices for v in frame]  
+        flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+        fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+        pc = PointCloud2()
+        pc.header.stamp =  self.get_clock().now().to_msg()
+        pc.header.frame_id = ''   # 可改成你需要的坐标系
+        pc.height = 1
+        pc.width = flat.size         # 360
+        pc.fields = fields
+        pc.is_bigendian = False
+        pc.point_step = 1            # 1 个 float32
+        pc.row_step = pc.point_step * pc.width
+        pc.data = flat.tobytes()     # 1440 字节
+        self.matrix_touch_pub_pc.publish(pc)
+
+
+    def close_can(self):
+        self.api.open_can.close_can(can=self.can)
+        sys.exit(0)
+
+
+def main(args=None):
+    '''
+    本节点用于收集手指状态和压感数据。
+    '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+    /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 50Hz
+    '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 50Hz
+    启动命令:
+    ros2 run linker_hand_ros2_sdk linker_hand_advanced_o6 --hand_type right --can can0 --is_touch true
+    '''
+    try:
+        rclpy.init(args=args)
+        parser = argparse.ArgumentParser()
+        parser.add_argument('--hand_type', required=True)
+        parser.add_argument('--can',        required=True)
+        parser.add_argument('--is_touch',   choices=['true','false'], required=True)
+
+        args = parser.parse_args()
+        node = LinkerHandAdvancedO6(name="linker_hand_advanced_o6",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+        embedded_version = node.embedded_version
+        rclpy.spin(node)         # 主循环，监听 ROS 回调
+    except KeyboardInterrupt:
+        print("收到 Ctrl+C，准备退出...")
+    finally:
+        # node.close_can()         # 关闭 CAN 或其他硬件资源
+        # node.destroy_node()      # 销毁 ROS 节点
+        # rclpy.shutdown()         # 关闭 ROS
+        print("程序已退出。")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py

@@ -0,0 +1,268 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+
+from re import A
+import rclpy,sys                                     # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node                      # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+# Linker Hand 型号
+HAND_JOINT = "G20"
+# 默认手指关节位置
+DEFAULT_POSITION =  [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+# 默认手指关节速度
+DEFAULT_SPEED=[255, 255, 255, 255, 255]
+# 默认手指关节力矩
+DEFAULT_TORQUE = [255, 255, 255, 255, 255]
+# 压感传感器延迟时间
+TOUCH_SLEEP_TIME = 0.003
+
+
+
+class LinkerHandAdvancedG20(Node):
+    def __init__(self, name, hand_type, can, is_touch):
+        super().__init__(name)       
+        self.hand_type = hand_type
+        self.hand_joint = HAND_JOINT
+        if is_touch == "true":
+            self.is_touch = True
+        else:
+            self.is_touch = False
+        self.can = can
+        self.modbus = "None"
+        time.sleep(0.1)
+        self._check_linker_hand_type()
+        self.last_hand_post_cmd = None # 最新手指位置命令
+        self.last_hand_vel_cmd = None # 最新手指速度命令
+        self.last_hand_eff_cmd = None # 最新手指力矩命令
+        self.matrix_dic = {
+            "stamp":{
+                "sec": 0,
+                "nanosec": 0,
+            },
+            "thumb_matrix":[[-1] * 6 for _ in range(12)],
+            "index_matrix":[[-1] * 6 for _ in range(12)],
+            "middle_matrix":[[-1] * 6 for _ in range(12)],
+            "ring_matrix":[[-1] * 6 for _ in range(12)],
+            "little_matrix":[[-1] * 6 for _ in range(12)]
+        }
+        # 压感矩阵合值，单位g 克
+        self.matrix_mass_dic = {
+            "stamp":{
+                "secs": 0,
+                "nsecs": 0,
+            },
+            "thumb_mass":[-1],
+            "index_mass":[-1],
+            "middle_mass":[-1],
+            "ring_mass":[-1],
+            "little_mass":[-1]
+        }
+        self.hz = 1.0/60.0
+        # ros时间获取
+        self.stamp_clock = Clock()
+        self._init_hand()
+        time.sleep(2)
+        self.count = 0
+        self.timer = self.create_timer(self.hz, self.run)  
+
+
+    def _check_linker_hand_type(self):
+        if self.modbus != "None":
+            ColorMsg(msg=f"Modbus暂不支持", color="red")
+            sys.exit(0)
+        if self.hand_joint.upper() != "G20":
+            ColorMsg(msg=f"Linker Hand hand_joint参数错误", color="red")
+            sys.exit(0)
+
+    def _init_hand(self):
+        self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+        time.sleep(0.1)
+        self.touch_type = self.api.get_touch_type()
+        self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+        if self.is_touch == True:
+            if self.touch_type > 1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+                #self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+            elif self.touch_type != -1:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+                self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+            else:
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+                self.is_touch = False
+        self.embedded_version = self.api.get_embedded_version()
+        self.api.set_speed(speed=DEFAULT_SPEED)
+        time.sleep(0.1)
+        self.api.set_torque(torque=DEFAULT_TORQUE)
+        time.sleep(0.1)
+        self.api.finger_move(pose=DEFAULT_POSITION)
+        time.sleep(0.1)
+        self.palm_touch = self.api.is_palm_touch
+        if self.palm_touch == 5:
+            self.touch_sleep_time = 0.03
+            ColorMsg(msg=f"{self.hand_type} {self.hand_joint} 全掌压感版", color="green")
+        else:
+            self.touch_sleep_time = 0.003
+
+    def hand_control_cb(self, msg):
+        if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+            self.last_hand_post_cmd = msg.position
+        if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+            self.last_hand_vel_cmd = msg.velocity
+        if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+            self.last_hand_eff_cmd = msg.effort
+    
+    def list_check(self,pose):
+        if isinstance(pose, list) == False:
+            return False
+        if len(self.last_hand_post_cmd) != len(pose):
+            return False
+        return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+    
+    def joint_state_msg(self, pose,vel=[]):
+        joint_state = JointState()
+        joint_state.header = Header()
+        joint_state.header.stamp = self.get_clock().now().to_msg()
+        joint_state.name = self.api.get_finger_order()
+        joint_state.position = [float(x) for x in pose]
+        if len(vel) > 1:
+            joint_state.velocity = [float(x) for x in vel]
+        else:
+            joint_state.velocity = [0.0] * len(pose)
+        joint_state.effort = [0.0] * len(pose)
+        return joint_state
+
+    def run(self):
+        # 执行手控制指令
+        if self.last_hand_post_cmd != None:
+            self.api.finger_move(pose=self.last_hand_post_cmd)
+            self.last_hand_post_cmd = None
+        # 优先获取手指状态并且发布
+        self.last_hand_state = self.api.get_state()
+        self.last_hand_vel = [0.0] * len(self.last_hand_state)
+        # 发布手状态
+        msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+        self.hand_state_pub.publish(msg_state)
+        
+        if self.is_touch == True:
+            # 获取压感数据
+            if self.count == 2:
+                self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+            if self.count == 4:
+                self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+            if self.count == 6:
+                self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+            if self.count == 8:
+                self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+            if self.count == 10:
+                self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+            if self.count == 14 and self.palm_touch == 5:
+                self.matrix_dic["palm_matrix"] = self.api.get_palm_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+            # 发布矩阵压感数据JSON格式
+            self.pub_matrix_dic()
+            # 发布矩阵压感和值JSON格式
+            self.pub_matrix_mass(dic=self.matrix_dic)
+            # 发布矩阵压感点云格式
+            #self.pub_matrix_point_cloud()
+        self.count += 1
+        if self.count == 15:
+            self.count = 0
+
+    def pub_matrix_dic(self):
+        """发布矩阵数据JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_dic["stamp"]["secs"] = t_secs
+        self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+        msg.data = json.dumps(self.matrix_dic)
+        self.matrix_touch_pub.publish(msg)
+
+    def pub_matrix_mass(self, dic):
+        """发布矩阵数据合值 单位g 克 JSON格式"""
+        msg = String()
+        # 获取当前的 ROS 时间
+        current_time = self.stamp_clock.now()
+        # 提取 secs 和 nsecs
+        t_secs = current_time.to_msg().sec
+        t_nsecs = current_time.to_msg().nanosec
+        self.matrix_mass_dic["stamp"]["secs"] = t_secs
+        self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+        self.matrix_mass_dic["unit"] = "g"
+        self.matrix_mass_dic["thumb_mass"] = self.api.hand.thumb_matrix_palm_mass
+        self.matrix_mass_dic["index_mass"] = self.api.hand.index_matrix_palm_mass
+        self.matrix_mass_dic["middle_mass"] = self.api.hand.middle_matrix_palm_mass
+        self.matrix_mass_dic["ring_mass"] = self.api.hand.ring_matrix_palm_mass
+        self.matrix_mass_dic["little_mass"] = self.api.hand.little_matrix_palm_mass
+        self.matrix_mass_dic["palm_mass"] = self.api.hand.palm_matrix_palm_mass
+        msg.data = json.dumps(self.matrix_mass_dic)
+        self.matrix_touch_mass_pub.publish(msg)
+
+    # def pub_matrix_point_cloud(self):
+    #     tmp_dic = self.matrix_dic.copy()
+    #     del tmp_dic['stamp']               # 去掉时间戳字段
+    #     all_matrices = list(tmp_dic.values())  # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+    #     # 摊平到一维
+    #     flat_list = [v for frame in all_matrices for v in frame]  
+    #     flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+    #     fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+    #     pc = PointCloud2()
+    #     pc.header.stamp =  self.get_clock().now().to_msg()
+    #     pc.header.frame_id = ''   # 可改成你需要的坐标系
+    #     pc.height = 1
+    #     pc.width = flat.size         # 360
+    #     pc.fields = fields
+    #     pc.is_bigendian = False
+    #     pc.point_step = 1            # 1 个 float32
+    #     pc.row_step = pc.point_step * pc.width
+    #     pc.data = flat.tobytes()     # 1440 字节
+    #     self.matrix_touch_pub_pc.publish(pc)
+
+
+    def close_can(self):
+        self.api.open_can.close_can(can=self.can)
+        sys.exit(0)
+
+
+def main(args=None):
+    '''
+    本节点用于收集手指状态和压感数据。
+    '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+    /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz
+    '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz
+    启动命令:
+    ros2 run linker_hand_ros2_sdk linker_hand_g20_palm_touch --hand_type left --can can0 --is_touch true
+    '''
+    try:
+        rclpy.init(args=args)
+        parser = argparse.ArgumentParser()
+        parser.add_argument('--hand_type', required=True)
+        parser.add_argument('--can',        required=True)
+        parser.add_argument('--is_touch',   choices=['true','false'], required=True)
+
+        args = parser.parse_args()
+        node = LinkerHandAdvancedG20(name="linker_hand_g20_palm_touch",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+        embedded_version = node.embedded_version
+        rclpy.spin(node)         # 主循环，监听 ROS 回调
+    except KeyboardInterrupt:
+        print("收到 Ctrl+C，准备退出...")
+    finally:
+        node.close_can()         # 关闭 CAN 或其他硬件资源
+        # node.destroy_node()      # 销毁 ROS 节点
+        # rclpy.shutdown()         # 关闭 ROS
+        print("程序已退出。")

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/package.xml

@@ -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>linker_hand_ros2_sdk</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="linker-robot@todo.todo">linker-robot</maintainer>
+  <license>TODO: License declaration</license>
+  
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>launch</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/pyproject.toml

@@ -0,0 +1,3 @@
+[build-system]
+requires = ["setuptools>=61.0"]
+build-backend = "setuptools.build_meta"

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/linker_hand_ros2_sdk
+[install]
+install_scripts=$base/lib/linker_hand_ros2_sdk

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/setup.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'linker_hand_ros2_sdk'
+
+this_dir = os.path.abspath(os.path.dirname(__file__))
+custom_dir = os.path.join(this_dir, package_name, "LinkerHand")
+
+data_files = [
+    ('share/ament_index/resource_index/packages',
+     ['resource/' + package_name]),
+    ('share/' + package_name, ['package.xml']),
+    (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+]
+
+# for root, dirs, files in os.walk(custom_dir):
+#     if files:
+#         relative_path = os.path.relpath(root, os.path.join(this_dir, package_name))
+#         target_path = os.path.join('share', package_name, relative_path)
+#         # 修复这里：路径必须是相对路径
+#         files_full_path = [os.path.relpath(os.path.join(root, f), start=os.getcwd()) for f in files]
+#         data_files.append((target_path, files_full_path))
+        
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(include=[package_name, f"{package_name}.*"]),
+    data_files=data_files,
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='linker-robot',
+    maintainer_email='linker-robot@todo.todo',
+    description='ROS2 SDK for Linker Hand',
+    license='TODO: License declaration',
+    entry_points={
+        'console_scripts': [
+            'linker_hand_sdk = linker_hand_ros2_sdk.linker_hand:main',
+            'linker_hand_advanced_o6 = linker_hand_ros2_sdk.linker_hand_advanced_o6:main',
+            'linker_hand_advanced_l6 = linker_hand_ros2_sdk.linker_hand_advanced_l6:main',
+            'linker_hand_advanced_l7 = linker_hand_ros2_sdk.linker_hand_advanced_l7:main',
+            'linker_hand_advanced_l10 = linker_hand_ros2_sdk.linker_hand_advanced_l10:main',
+            'linker_hand_advanced_g20 = linker_hand_ros2_sdk.linker_hand_advanced_g20:main',
+            'linker_hand_g20_palm_touch = linker_hand_ros2_sdk.linker_hand_g20_palm_touch:main',
+        ],
+    },
+)

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

linker_hand_ros2_sdk_ws/src/linker_hand_ros2_sdk/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

linker_hand_ros2_sdk_ws/src/pressure_diagram/launch/pressure_diagram.launch.py

@@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='pressure_diagram',
+            executable='pressure_diagram',
+            name='pressure_diagram_node',
+            output='screen',
+            parameters=[{
+
+            }],
+        ),
+
+    ])

linker_hand_ros2_sdk_ws/src/pressure_diagram/package.xml

@@ -0,0 +1,28 @@
+<?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>pressure_diagram</name>
+  <version>0.0.0</version>
+  <description>ROS2 Pressure Diagram - Real-time pressure sensor visualization for Linker Hand</description>
+  <maintainer email="linkerhand@todo.todo">linkerhand</maintainer>
+  <license>Apache-2.0</license>
+
+  <!-- ROS2 dependencies -->
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>ament_index_python</exec_depend>
+
+  <!-- Python dependencies -->
+  <exec_depend>python3-pyqt5</exec_depend>
+  <exec_depend>python3-pyqtgraph</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

linker_hand_ros2_sdk_ws/src/pressure_diagram/pressure_diagram/pressure_diagram.py

@@ -0,0 +1,421 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+import signal
+import sys
+import json
+import numpy as np
+from std_msgs.msg import String
+from PyQt5 import QtWidgets, QtCore, QtGui
+import pyqtgraph as pg
+
+pg.setConfigOption('imageAxisOrder', 'row-major')
+pg.setConfigOption('useOpenGL', False)  
+
+class PressureDiagram(Node, QtWidgets.QMainWindow):
+    def __init__(self):
+        # 先调用 Node 的 __init__，避免 super() 歧义
+        Node.__init__(self, 'pressure_diagram')
+        QtWidgets.QMainWindow.__init__(self)
+
+        # 键名与ROS数据一致
+        self.fingers_matrix = ['thumb_matrix', 'index_matrix', 'middle_matrix', 'ring_matrix', 'little_matrix']
+        self.fingers_mass = ['thumb_mass', 'index_mass', 'middle_mass', 'ring_mass', 'little_mass']
+
+        self.finger_colors = [
+            (255, 75, 75),    # Thumb - Red
+            (50, 205, 50),    # Index - Green
+            (65, 105, 255),   # Middle - Blue
+            (255, 215, 0),    # Ring - Gold
+            (218, 112, 214)   # Pinky - Purple
+        ]
+
+        # 波形图数据
+        self.wave_data = {f: np.zeros(100) for f in self.fingers_mass}
+        self.current_wave_values = {f: 0.0 for f in self.fingers_mass}
+
+        # 热力图数据 - 12行×6列 (高大于宽)
+        self.matrix_data = {f: np.zeros((12, 6)) for f in self.fingers_matrix}
+        self.matrix_shapes = {f: (12, 6) for f in self.fingers_matrix}  # (rows, cols)
+        self.data_received = {f: False for f in self.fingers_matrix}
+
+        self.wave_sub = None
+        self.matrix_sub = None
+
+        # ROS2 节流计时器
+        self.last_matrix_log_time = {f: None for f in self.fingers_matrix}
+
+        # QoS 配置
+        qos_profile = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=100
+        )
+        self.qos_profile = qos_profile
+
+        self.init_ui()
+        self.setup_ros()
+
+    def init_ui(self):
+        self.setWindowTitle("Robotic Hand Sensor Fusion Interface")
+        self.resize(1600, 900)
+        self.setStyleSheet("background-color: #0f172a;")
+        
+        central_widget = QtWidgets.QWidget()
+        self.setCentralWidget(central_widget)
+        
+        main_layout = QtWidgets.QHBoxLayout(central_widget)
+        main_layout.setSpacing(15)
+        main_layout.setContentsMargins(10, 10, 10, 10)
+        
+        # ============== 左侧：波形图 ==============
+        left_widget = self.create_waveform_panel()
+        main_layout.addWidget(left_widget, stretch=1)
+        
+        line = QtWidgets.QFrame()
+        line.setFrameShape(QtWidgets.QFrame.VLine)
+        line.setStyleSheet("QFrame { background-color: #334155; max-width: 2px; }")
+        main_layout.addWidget(line)
+        
+        # ============== 右侧：热力图 ==============
+        right_widget = self.create_heatmap_panel()
+        main_layout.addWidget(right_widget, stretch=1)
+        
+        self.timer = QtCore.QTimer()
+        self.timer.timeout.connect(self.update_display)
+        self.timer.start(33)
+
+    def create_waveform_panel(self):
+        panel = QtWidgets.QWidget()
+        layout = QtWidgets.QVBoxLayout(panel)
+        layout.setSpacing(8)
+        
+        wave_ctrl = QtWidgets.QFrame()
+        wave_ctrl.setStyleSheet("""QFrame { background-color: #1e293b; border: 2px solid #3b82f6; border-radius: 4px; }""")
+        wave_layout = QtWidgets.QHBoxLayout(wave_ctrl)
+        wave_layout.setContentsMargins(12, 6, 12, 6)
+        
+        title = QtWidgets.QLabel("WAVEFORM MONITOR (30Hz)")
+        title.setStyleSheet("color: #60a5fa; font-weight: bold; font-size: 13px;")
+        
+        self.wave_combo = QtWidgets.QComboBox()
+        self.wave_combo.addItems([
+            "/cb_left_hand_matrix_touch_mass",
+            "/cb_right_hand_matrix_touch_mass"
+        ])
+        self.wave_combo.setStyleSheet("""
+            QComboBox { color: white; background-color: #334155; border: 1px solid #475569; border-radius: 4px; padding: 4px 8px; min-width: 260px; }
+            QComboBox QAbstractItemView { background-color: #1e293b; color: white; selection-background-color: #3b82f6; }
+        """)
+        self.wave_combo.currentTextChanged.connect(self.switch_wave_topic)
+        
+        wave_layout.addWidget(title)
+        wave_layout.addWidget(self.wave_combo)
+        wave_layout.addStretch()
+        layout.addWidget(wave_ctrl)
+        
+        self.wave_widget = pg.GraphicsLayoutWidget()
+        self.wave_widget.setBackground('#0f172a')
+        layout.addWidget(self.wave_widget, stretch=1)
+        
+        self.wave_plots = {}
+        self.wave_curves = {}
+        
+        for i, finger in enumerate(self.fingers_mass):
+            p = self.wave_widget.addPlot(row=i, col=0)
+            p.setMenuEnabled(False)
+            p.setMouseEnabled(x=False, y=False)
+            p.setYRange(0, 4500)
+            p.setXRange(0, 100)
+            p.showGrid(x=True, y=True, alpha=0.3)
+            p.getAxis('left').setTextPen('#64748b')
+            p.getAxis('bottom').setTextPen('#64748b')
+            
+            color = QtGui.QColor(*self.finger_colors[i])
+            display_name = finger.replace('_mass', '').upper()
+            p.setTitle(f"[ {display_name} ]", color=color, size='11pt')
+            
+            if i == 4:
+                p.getAxis('bottom').setLabel('Time', color='#64748b')
+            else:
+                p.getAxis('bottom').setStyle(showValues=False)
+            
+            pen = pg.mkPen(color=self.finger_colors[i], width=2.5)
+            curve = p.plot(pen=pen)
+            self.wave_curves[finger] = curve
+            
+            text = pg.TextItem(text="0", color=(255,255,255), anchor=(1, 0.5))
+            text.setFont(QtGui.QFont("Arial", 10, QtGui.QFont.Bold))
+            p.addItem(text)
+            self.wave_curves[finger + '_text'] = text
+            
+        return panel
+
+    def create_heatmap_panel(self):
+        panel = QtWidgets.QWidget()
+        layout = QtWidgets.QVBoxLayout(panel)
+        layout.setSpacing(8)
+        
+        heat_ctrl = QtWidgets.QFrame()
+        heat_ctrl.setStyleSheet("""QFrame { background-color: #1e293b; border: 2px solid #f43f5e; border-radius: 4px; }""")
+        heat_layout = QtWidgets.QHBoxLayout(heat_ctrl)
+        heat_layout.setContentsMargins(12, 6, 12, 6)
+        
+        title = QtWidgets.QLabel("PRESSURE MATRIX (12×6)")
+        title.setStyleSheet("color: #f43f5e; font-weight: bold; font-size: 13px;")
+        
+        # 单位标签
+        unit_label = QtWidgets.QLabel("Max Value (g)")
+        unit_label.setStyleSheet("color: #94a3b8; font-size: 11px; border: none; background: transparent;")
+        heat_layout.addWidget(unit_label)
+        heat_layout.addSpacing(10)
+        
+        self.matrix_combo = QtWidgets.QComboBox()
+        self.matrix_combo.addItems([
+            "/cb_left_hand_matrix_touch",
+            "/cb_right_hand_matrix_touch"
+        ])
+        self.matrix_combo.setStyleSheet("""
+            QComboBox { color: white; background-color: #334155; border: 1px solid #475569; border-radius: 4px; padding: 4px 8px; min-width: 260px; }
+            QComboBox QAbstractItemView { background-color: #1e293b; color: white; selection-background-color: #f43f5e; }
+        """)
+        self.matrix_combo.currentTextChanged.connect(self.switch_matrix_topic)
+        
+        heat_layout.addWidget(title)
+        heat_layout.addWidget(self.matrix_combo)
+        heat_layout.addStretch()
+        layout.addWidget(heat_ctrl)
+        
+        heat_container = QtWidgets.QWidget()
+        heat_grid = QtWidgets.QGridLayout(heat_container)
+        heat_grid.setSpacing(10)
+        heat_grid.setContentsMargins(5, 5, 5, 5)
+        
+        self.matrix_plots = {}
+        self.matrix_images = {}
+        self.matrix_peaks = {}
+        
+        positions = [
+            ('thumb_matrix', 0, 0), ('index_matrix', 0, 1), ('middle_matrix', 0, 2),
+            ('ring_matrix', 1, 0), ('little_matrix', 1, 1)
+        ]
+        
+        self.colormap = pg.colormap.get('plasma')
+        
+        for idx, (finger, row, col) in enumerate(positions):
+            container = QtWidgets.QWidget()
+            vbox = QtWidgets.QVBoxLayout(container)
+            vbox.setSpacing(2)
+            vbox.setContentsMargins(0, 0, 0, 0)
+            
+            display_name = finger.replace('_matrix', '').upper()
+            name_label = QtWidgets.QLabel(display_name)
+            name_label.setAlignment(QtCore.Qt.AlignCenter)
+            color_hex = '#{:02x}{:02x}{:02x}'.format(*self.finger_colors[idx])
+            name_label.setStyleSheet(f"color: {color_hex}; font-weight: bold; font-size: 12px;")
+            vbox.addWidget(name_label)
+            
+            # 固定比例显示 12×6 (高:宽 = 2:1)
+            view = pg.PlotWidget()
+            view.setMenuEnabled(False)
+            view.setMouseEnabled(x=False, y=False)
+            view.setAspectLocked(True, ratio=6/12)  # X:Y = 1:2
+            view.setMaximumSize(240, 280)
+            view.setMinimumSize(120, 160)
+            view.setBackground('#0f172a')
+            view.hideAxis('left')
+            view.hideAxis('bottom')
+            
+            img = pg.ImageItem()
+            img.setLookupTable(self.colormap.getLookupTable())
+            img.setImage(np.zeros((12, 6)), levels=[0, 100])
+            view.addItem(img)
+            
+            peak_text = pg.TextItem(text="", color=(255,255,255), anchor=(0.5, 0.5))
+            peak_text.setFont(QtGui.QFont("Arial", 10, QtGui.QFont.Bold))
+            view.addItem(peak_text)
+            
+            vbox.addWidget(view, stretch=1)
+            heat_grid.addWidget(container, row, col)
+            
+            self.matrix_plots[finger] = view
+            self.matrix_images[finger] = img
+            self.matrix_peaks[finger] = peak_text
+        
+        # 颜色条
+        legend_widget = QtWidgets.QWidget()
+        legend_layout = QtWidgets.QVBoxLayout(legend_widget)
+        legend_layout.setAlignment(QtCore.Qt.AlignCenter)
+        
+        lbl_max = QtWidgets.QLabel("MAX")
+        lbl_max.setStyleSheet("color: #fbbf24; font-size: 9px;")
+        lbl_max.setAlignment(QtCore.Qt.AlignCenter)
+        
+        gradient = pg.GradientWidget(orientation='right')
+        gradient.setMaximumWidth(25)
+        gradient.setMaximumHeight(180)
+        gradient.setColorMap(self.colormap)
+        
+        lbl_min = QtWidgets.QLabel("0")
+        lbl_min.setStyleSheet("color: #3b82f6; font-size: 9px;")
+        lbl_min.setAlignment(QtCore.Qt.AlignCenter)
+        
+        legend_layout.addWidget(lbl_max)
+        legend_layout.addWidget(gradient, stretch=1)
+        legend_layout.addWidget(lbl_min)
+        
+        heat_grid.addWidget(legend_widget, 1, 2)
+        
+        layout.addWidget(heat_container, stretch=1)
+        return panel
+
+    def setup_ros(self):
+        self.get_logger().info("Setting up ROS...")
+        self.subscribe_waveform("/cb_left_hand_matrix_touch_mass")
+        self.subscribe_matrix("/cb_left_hand_matrix_touch")
+
+    def subscribe_waveform(self, topic):
+        if self.wave_sub:
+            self.destroy_subscription(self.wave_sub)
+        self.wave_sub = self.create_subscription(String, topic, self.wave_callback, self.qos_profile)
+        self.get_logger().info(f"WAVEFORM: {topic}")
+
+    def subscribe_matrix(self, topic):
+        if self.matrix_sub:
+            self.destroy_subscription(self.matrix_sub)
+        self.matrix_sub = self.create_subscription(String, topic, self.matrix_callback, self.qos_profile)
+        self.get_logger().info(f"MATRIX: {topic}")
+
+    def switch_wave_topic(self, topic):
+        self.subscribe_waveform(topic)
+        for f in self.fingers_mass:
+            self.wave_data[f] = np.zeros(100)
+            self.current_wave_values[f] = 0.0
+
+    def switch_matrix_topic(self, topic):
+        self.subscribe_matrix(topic)
+        for f in self.fingers_matrix:
+            self.matrix_data[f] = np.zeros((12, 6))
+            self.matrix_shapes[f] = (12, 6)
+            self.data_received[f] = False
+
+    def wave_callback(self, msg):
+        try:
+            data = json.loads(msg.data)
+            for finger in self.fingers_mass:
+                if finger in data:
+                    self.current_wave_values[finger] = float(data[finger])
+        except Exception as e:
+            pass
+
+    def matrix_callback(self, msg):
+        """处理 12×6 矩阵数据"""
+        try:
+            data = json.loads(msg.data)
+
+            for finger in self.fingers_matrix:
+                if finger not in data:
+                    continue
+
+                arr = np.array(data[finger], dtype=np.float32)
+
+                if arr.ndim == 2:
+                    rows, cols = arr.shape
+                    self.data_received[finger] = True
+                    self.matrix_data[finger] = arr
+                    self.matrix_shapes[finger] = (rows, cols)
+
+                    # ROS2 节流日志
+                    current_time = self.get_clock().now()
+                    if self.last_matrix_log_time[finger] is None or \
+                       (current_time - self.last_matrix_log_time[finger]).nanoseconds > 3e9:
+                        self.last_matrix_log_time[finger] = current_time
+                        self.get_logger().info(f"{finger}: {rows}×{cols}, max={arr.max():.1f}")
+
+        except Exception as e:
+            self.get_logger().error(f"Matrix error: {str(e)}")
+
+    def update_display(self):
+        # 更新波形图
+        for finger in self.fingers_mass:
+            self.wave_data[finger] = np.roll(self.wave_data[finger], -1)
+            self.wave_data[finger][-1] = self.current_wave_values[finger]
+            self.wave_curves[finger].setData(self.wave_data[finger])
+            
+            val = self.current_wave_values[finger]
+            self.wave_curves[finger + '_text'].setText(f"{val:.0f}")
+            self.wave_curves[finger + '_text'].setPos(99, val)
+
+        # 更新热力图 - 修复翻转问题
+        for finger in self.fingers_matrix:
+            data = self.matrix_data[finger]
+            rows, cols = self.matrix_shapes[finger]  # 12×6
+            
+            if not self.data_received[finger]:
+                continue
+            
+            img = self.matrix_images[finger]
+            view = self.matrix_plots[finger]
+            peak_text = self.matrix_peaks[finger]
+            
+            max_val = data.max()
+            
+            if max_val > 0:
+                display_data = data / max_val if max_val > 1 else data
+            else:
+                display_data = data
+            
+            # **关键修复**: 垂直翻转，使第0行(ROS数据第1行)显示在顶部
+            display_data_flipped = np.flipud(display_data)
+            
+            # 显示翻转后的数据 (12×6)
+            img.setImage(display_data_flipped, autoLevels=False, levels=[0, 1])
+            
+            # 设置视图范围
+            view.setRange(xRange=(-0.5, cols-0.5), yRange=(-0.5, rows-0.5))
+            
+            # 显示数值：矩阵中的最大压力值，单位g(克)
+            if max_val > 0:
+                # 找到原始数据中的最大值位置
+                max_idx = np.unravel_index(np.argmax(data), data.shape)
+                
+                # 峰值显示格式: 最大值 + 单位
+                peak_text.setText(f"{max_val:.0f}")
+                
+                # 坐标转换：因为图像翻转了，y坐标也要翻转
+                # 原始行号row，在翻转后的图像中是 (rows-1-row)
+                flipped_row = (rows - 1) - max_idx[0]
+                peak_text.setPos(max_idx[1], flipped_row)
+            else:
+                peak_text.setText("0")
+
+def signal_handler(sig, frame):
+    QtWidgets.QApplication.quit()
+    sys.exit(0)
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    app = QtWidgets.QApplication(sys.argv)
+    app.setStyle('Fusion')
+
+    signal.signal(signal.SIGINT, signal_handler)
+
+    gui = PressureDiagram()
+    gui.show()
+
+    # ROS2 spin 在 Qt 定时器中处理
+    def ros_spin():
+        rclpy.spin_once(gui, timeout_sec=0)
+
+    ros_timer = QtCore.QTimer()
+    ros_timer.timeout.connect(ros_spin)
+    ros_timer.start(1)  # ~1000Hz
+
+    sys.exit(app.exec_())
+
+if __name__ == '__main__':
+    main()

linker_hand_ros2_sdk_ws/src/pressure_diagram/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/pressure_diagram
+[install]
+install_scripts=$base/lib/pressure_diagram

linker_hand_ros2_sdk_ws/src/pressure_diagram/setup.py

@@ -0,0 +1,34 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+import os
+from glob import glob
+from setuptools import find_packages, setup
+package_name = 'pressure_diagram'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='linkerhand',
+    maintainer_email='linkerhand@todo.todo',
+    description='ROS2 Pressure Diagram - Real-time pressure sensor visualization for Linker Hand',
+    license='Apache-2.0',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': [
+            'pressure_diagram = pressure_diagram.pressure_diagram:main',
+        ],
+    },
+)

linker_hand_ros2_sdk_ws/src/pressure_diagram/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

linker_hand_ros2_sdk_ws/src/pressure_diagram/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

linker_hand_ros2_sdk_ws/src/pressure_diagram/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

linker_hand_ros2_sdk_ws/src/setting_info/package.xml

@@ -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>setting_info</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="linkerhand@todo.todo">linkerhand</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

linker_hand_ros2_sdk_ws/src/setting_info/setting_info/setting_info_node.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+import rclpy,math,sys                                     # ROS2 Python接口库
+from rclpy.node import Node                      # ROS2 节点类
+import rclpy.time
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState
+import time,threading, json
+
+
+class SettingInfoNode(Node):
+    def __init__(self):
+        super().__init__("setting_info_node")
+        self.hand_joint = "L10"
+        self.hand_type = "left"
+        self.hand_info = {}
+        self.cb_count = 0
+        self.hand_info_sub = self.create_subscription(String, f"/cb_{self.hand_type}_hand_info",self.setting_cb,10)
+        self.setting_pub = self.create_publisher(String, '/cb_hand_setting_cmd', 10)
+        
+
+    def setting_cb(self, msg):
+        self.cb_count += 1
+        self.hand_info = json.loads(msg.data)
+        if self.cb_count == 5:
+            print(self.hand_info)
+            sys.exit(1)
+        time.sleep(1)
+
+    
+    def pub_msg(self, cmd_dic):
+        count = 0
+        while True:
+            msg = String()
+            msg.data = json.dumps(cmd_dic)
+            print(msg)
+            self.setting_pub.publish(msg)
+            time.sleep(1)
+            if count == 5:
+                break
+            count += 1
+        
+
+    # 设置速度
+    def set_speed(self, speed=[91] * 10):
+        cmd_dic = {
+            "setting_cmd": "set_speed",
+            "params":{
+                "hand_type": self.hand_type,
+                "speed":speed
+            }
+        }
+        self.pub_msg(cmd_dic=cmd_dic)
+
+    # 设置扭矩
+    def set_max_torque_limits(self, torque=[100] * 5):
+        cmd_dic = {
+            "setting_cmd": "set_max_torque_limits",
+            "params":{
+                "hand_type": self.hand_type,
+                "torque":torque
+            }
+        }
+        self.pub_msg(cmd_dic=cmd_dic)
+
+def main(args=None):
+    rclpy.init(args=args)
+    set = SettingInfoNode()
+    pub_ther = threading.Thread(target=set.set_speed)
+    pub_ther.daemon = True
+    pub_ther.start()
+    #set.set_speed()
+    
+    rclpy.spin(set)

linker_hand_ros2_sdk_ws/src/setting_info/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/setting_info
+[install]
+install_scripts=$base/lib/setting_info

linker_hand_ros2_sdk_ws/src/setting_info/setup.py

@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'setting_info'
+
+setup(
+    name=package_name,
+    version='0.0.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='linkerhand',
+    maintainer_email='linkerhand@todo.todo',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'setting_info_node = setting_info.setting_info_node:main',
+        ],
+    },
+)