Merge pull request '机械臂与灵巧手驱动已构建完成' (#1) from feature/2026-9-20 into main

Reviewed-on: #1

.gitignore

@@ -0,0 +1,3 @@
+build/
+install/
+log/

README.md

@@ -0,0 +1,559 @@
+# weipz-Lbot_Bridge 接口说明
+
+本工程当前使用 `src/officer_sdk/lbot_driver` 作为机器人网口连接和 ROS2 bridge。启动脚本为：
+
+```bash
+cd ~/workspace/weipz-Lbot_Bridge/scripts
+./lbot_arm_control.sh start
+```
+
+停止服务：
+
+```bash
+cd ~/workspace/weipz-Lbot_Bridge/scripts
+./lbot_arm_control.sh stop
+```
+
+默认启动的机器人 namespace 是 `/robot1`，默认连接控制器 IP 为 `192.168.10.21`。
+
+## 机械臂反馈订阅
+
+外部软件如果需要订阅机器人当前机械臂各关节角度、速度、电流/力矩，使用下面两个 topic：
+
+```text
+/robot1/left_arm/joint_states
+/robot1/right_arm/joint_states
+```
+
+消息类型：
+
+```text
+sensor_msgs/msg/JointState
+```
+
+字段含义：
+
+```text
+name[]      关节名称
+position[]  当前 7 个关节角度，单位 rad
+velocity[]  当前 7 个关节速度，单位 rad/s
+effort[]    当前 7 个关节 effort 字段；当前 driver 直接填入 SDK 的 effort[7]，按项目使用可视为关节电流/力矩反馈字段
+```
+
+示例：
+
+```bash
+ros2 topic echo /robot1/left_arm/joint_states
+ros2 topic echo /robot1/right_arm/joint_states
+```
+
+当前反馈中没有关节加速度字段。如果外部软件必须获取关节加速度，需要在上游根据 `velocity[]` 和时间戳自行差分，或者让厂家提供 SDK 原生加速度反馈接口。
+
+## 机器人健康/故障状态
+
+当前 `officer_sdk/lbot_driver` 已发布系统错误信息 topic：
+
+```text
+/robot1/system_error
+```
+
+消息类型：
+
+```text
+lbot_arm_interfaces/msg/SystemError
+```
+
+消息定义：
+
+```text
+std_msgs/Header header
+int32 error_code
+string error_msg
+bool connected
+```
+
+字段含义：
+
+```text
+error_code  SDK 或 driver 上报的错误码
+error_msg   错误信息
+connected   发布错误时 driver 是否认为机器人处于连接状态
+```
+
+示例：
+
+```bash
+ros2 topic echo /robot1/system_error
+```
+
+目前该 topic 会在以下情况发布：
+
+```text
+SDK error callback 触发时
+连接机器人失败时，error_code = -1
+状态监控启动失败时，error_code = -2
+```
+
+注意：SDK 当前状态结构 `lbot_full_state_t` 只包含左右臂关节状态、末端位姿、时间戳和 IP；没有完整的故障码、告警码、使能状态、急停状态等字段。因此 `/robot1/system_error` 当前是错误事件 topic，不是完整健康状态快照。
+
+仍然可以辅助查看 ROS 日志：
+
+```text
+/rosout
+```
+
+示例：
+
+```bash
+ros2 topic echo /rosout
+```
+
+也可以查看启动脚本日志：
+
+```bash
+tail -f ~/workspace/weipz-Lbot_Bridge/log/lbot_arm_control.log
+```
+
+如果后续需要稳定给外部软件使用的完整健康状态快照，建议在 `SystemError.msg` 之外再新增 `RobotHealth.msg`，并在连接/重连逻辑、使能/急停服务回调中维护并周期发布该状态。需要修改的位置：
+
+```text
+src/officer_sdk/lbot_arm_interfaces/msg/RobotHealth.msg
+src/officer_sdk/lbot_arm_interfaces/CMakeLists.txt
+src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h
+src/officer_sdk/lbot_driver/src/lbot_driver.cpp
+```
+
+## 机械臂关节空间控制
+
+外部软件如果需要发送机械臂各关节目标位置，使用 MoveJ 服务，不是 topic。
+
+左臂：
+
+```text
+/robot1/left_arm/move_joint
+```
+
+右臂：
+
+```text
+/robot1/right_arm/move_joint
+```
+
+服务类型：
+
+```text
+lbot_arm_interfaces/srv/MoveJ
+```
+
+服务定义：
+
+```srv
+float32[] joints
+float32 speed
+float32 acce
+bool block
+---
+bool success
+```
+
+字段含义：
+
+```text
+joints  7 个目标关节角，单位 rad
+speed   整条机械臂统一目标速度，单位 rad/s
+acce    整条机械臂统一目标加速度，单位 rad/s^2
+block   true 表示阻塞等待运动完成，false 表示非阻塞下发
+```
+
+示例：
+
+```bash
+ros2 service call /robot1/left_arm/move_joint lbot_arm_interfaces/srv/MoveJ \
+"{joints: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], speed: 1.0, acce: 1.0, block: true}"
+```
+
+当前限制：
+
+```text
+MoveJ 支持每个关节独立目标位置 joints[7]
+MoveJ 不支持每个关节独立目标速度 velocity[7]
+MoveJ 不支持每个关节独立目标加速度 acceleration[7]
+speed 和 acce 是整条机械臂统一标量
+```
+
+如果后续必须支持每个关节单独目标速度和加速度，需要修改：
+
+```text
+src/officer_sdk/lbot_arm_interfaces/srv/MoveJ.srv
+src/officer_sdk/lbot_driver/src/lbot_driver.cpp
+src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h
+```
+
+但当前 SDK 的 `lbot_move_joint()` 接口本身只接收：
+
+```cpp
+const double joints[7], double speed, double accel, bool block
+```
+
+因此即使 ROS service 增加 `velocity[7]`、`acceleration[7]` 字段，也不能直接传给现有 SDK。要真正实现每关节速度/加速度控制，需要厂家提供更底层的控制接口，或者在 ROS 层自己做轨迹插值，再周期性下发关节位置。
+
+## L20/R20 灵巧手整手位置控制
+
+外部软件如果需要一次性控制机器人灵巧手所有关节位置，使用下面两个 topic：
+
+```text
+/robot1/left_hand/set_l20_joint
+/robot1/right_hand/set_l20_joint
+```
+
+消息类型：
+
+```text
+std_msgs/msg/Int32MultiArray
+```
+
+字段含义：
+
+```text
+data[]  L20/R20 灵巧手目标位置数组
+```
+
+当前 driver 中该 topic 直接调用 SDK：
+
+```cpp
+lbot_l20_set_all_position(lbot_handle, LBOT_LEFT_ARM, hand_joints)
+lbot_l20_set_all_position(lbot_handle, LBOT_RIGHT_ARM, hand_joints)
+```
+
+SDK 头文件注释写明该接口参数是：
+
+```text
+16 个自由度的目标位置，单位 degree
+```
+
+因此当前整手控制按 16 个整数位置发送。
+
+示例：
+
+```bash
+ros2 topic pub --once /robot1/left_hand/set_l20_joint std_msgs/msg/Int32MultiArray \
+"{data: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}"
+```
+
+右手示例：
+
+```bash
+ros2 topic pub --once /robot1/right_hand/set_l20_joint std_msgs/msg/Int32MultiArray \
+"{data: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}"
+```
+
+## Linker Hand L20 独立 CAN 控制
+
+当前工程也集成了 `src/linker_hand_ros2_sdk`，用于在灵巧手单独接入域控 CAN 口时控制左右 L20 灵巧手，并获取关节状态和矩阵触觉合力反馈。
+
+启动服务：
+
+```bash
+cd ~/workspace/weipz-Lbot_Bridge
+./scripts/linker_hand_control.sh start
+```
+
+停止服务：
+
+```bash
+cd ~/workspace/weipz-Lbot_Bridge
+./scripts/linker_hand_control.sh stop
+```
+
+查看状态：
+
+```bash
+./scripts/linker_hand_control.sh status
+```
+
+脚本当前会启动 `can0` 和 `can1`，波特率为 `1000000`。默认 launch 配置为：
+
+```text
+左手：can1，hand_type = left，hand_joint = L20
+右手：can0，hand_type = right，hand_joint = L20
+```
+
+脚本会强制设置：
+
+```text
+ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST
+```
+
+这样当前机器只发现本机 ROS 2 节点，避免同一局域网内旧的 `/linker_hand_sdk` 节点污染话题。
+
+### 灵巧手控制话题
+
+左手整手位置控制：
+
+```text
+/l20_left_hand_control_cmd
+```
+
+右手整手位置控制：
+
+```text
+/l20_right_hand_control_cmd
+```
+
+消息类型：
+
+```text
+sensor_msgs/msg/JointState
+```
+
+字段含义：
+
+```text
+position[]  L20 目标关节位置数组，当前按 20 个 uint8 位置值使用，范围 0~255
+velocity[]  可选速度数组；不需要设置速度时可以不填
+effort[]    当前控制链路不使用
+```
+
+左手发布示例：
+
+```bash
+ros2 topic pub --once /l20_left_hand_control_cmd sensor_msgs/msg/JointState \
+"{position: [205, 206, 230, 220, 226, 159, 127, 127, 32, 101, 80, 0, 0, 0, 0, 226, 254, 254, 248, 253]}"
+```
+
+右手发布示例：
+
+```bash
+ros2 topic pub --once /l20_right_hand_control_cmd sensor_msgs/msg/JointState \
+"{position: [252, 147, 150, 136, 96, 255, 254, 202, 88, 0, 198, 0, 0, 0, 0, 254, 244, 241, 235, 211]}"
+```
+
+如果需要同时设置速度，可以填入 `velocity[]`：
+
+```bash
+ros2 topic pub --once /l20_right_hand_control_cmd sensor_msgs/msg/JointState \
+"{position: [252, 147, 150, 136, 96, 255, 254, 202, 88, 0, 198, 0, 0, 0, 0, 254, 244, 241, 235, 211], velocity: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]}"
+```
+
+注意：这里的 `position[]` 不是弧度，是 Linker Hand CAN 协议中的 0~255 位置值。
+
+### 灵巧手状态反馈
+
+左手状态：
+
+```text
+/l20_left_hand_state
+```
+
+右手状态：
+
+```text
+/l20_right_hand_state
+```
+
+消息类型：
+
+```text
+sensor_msgs/msg/JointState
+```
+
+字段含义：
+
+```text
+name[]      关节名称
+position[]  当前 L20 关节位置反馈，0~255
+velocity[]  当前 L20 速度/速度阈值反馈，按 SDK 原始值发布
+effort[]    当前未填有效力矩，通常为 0
+```
+
+订阅示例：
+
+```bash
+ros2 topic echo /l20_left_hand_state
+ros2 topic echo /l20_right_hand_state
+```
+
+### 灵巧手矩阵触觉合力反馈
+
+左手触觉合力：
+
+```text
+/l20_left_hand_matrix_touch_mass
+```
+
+右手触觉合力：
+
+```text
+/l20_right_hand_matrix_touch_mass
+```
+
+消息类型：
+
+```text
+std_msgs/msg/Float32MultiArray
+```
+
+字段含义：
+
+```text
+data[0]  thumb_mass，拇指矩阵触觉合力
+data[1]  index_mass，食指矩阵触觉合力
+data[2]  middle_mass，中指矩阵触觉合力
+data[3]  ring_mass，无名指矩阵触觉合力
+data[4]  little_mass，小指矩阵触觉合力
+```
+
+订阅示例：
+
+```bash
+ros2 topic echo /l20_left_hand_matrix_touch_mass
+ros2 topic echo /l20_right_hand_matrix_touch_mass
+```
+
+完整矩阵触觉和点云话题也会发布：
+
+```text
+/l20_left_hand_matrix_touch
+/l20_left_hand_matrix_touch_pc
+/l20_right_hand_matrix_touch
+/l20_right_hand_matrix_touch_pc
+```
+
+其中 `matrix_touch` 当前仍是 JSON 字符串，`matrix_touch_pc` 为 `sensor_msgs/msg/PointCloud2`。
+
+### 灵巧手配置指令
+
+配置指令话题：
+
+```text
+/l20_hand_setting_cmd
+```
+
+消息类型：
+
+```text
+std_msgs/msg/String
+```
+
+该话题用于向 SDK 发送设置类 JSON 指令，例如清故障、读取故障、设置电流等。一般控制整手位置不需要使用该话题。
+
+### 灵巧手 rosbag 录制
+
+`linker_hand_control.sh start` 会同时启动 rosbag 录制，`stop` 会停止录制。每满 1 小时自动切一个新包。
+
+录制文件命名：
+
+```text
+/tmp/L20_Hand_YYYYmmdd_HHMMSS.mcap
+```
+
+录制话题：
+
+```text
+/l20_left_hand_control_cmd
+/l20_left_hand_state
+/l20_left_hand_matrix_touch_mass
+/l20_right_hand_control_cmd
+/l20_right_hand_state
+/l20_right_hand_matrix_touch_mass
+```
+
+录制日志：
+
+```bash
+tail -f ~/workspace/weipz-Lbot_Bridge/log/linker_hand_rosbag.log
+```
+
+查看录包进程状态：
+
+```bash
+./scripts/linker_hand_control.sh status
+```
+
+回放示例：
+
+```bash
+ros2 bag play /tmp/L20_Hand_YYYYmmdd_HHMMSS.mcap
+```
+
+## 相关代码位置
+
+反馈发布：
+
+```text
+src/officer_sdk/lbot_driver/src/lbot_driver.cpp
+```
+
+主要位置：
+
+```text
+LBot::state_publish_timer_callback()
+```
+
+机械臂 MoveJ 服务：
+
+```text
+LeftArmServiceNode::move_joint_callback()
+RightArmServiceNode::move_joint_callback()
+```
+
+L20/R20 灵巧手 topic：
+
+```text
+LeftArmServiceNode::left_hand_l20_set_joint_callback()
+RightArmServiceNode::right_hand_l20_set_joint_callback()
+```
+
+SDK API 声明：
+
+```text
+src/officer_sdk/lbot_driver/include/lbot_driver/lbot_api_cpp.h
+```
+
+ROS 接口定义：
+
+```text
+src/officer_sdk/lbot_arm_interfaces/srv/MoveJ.srv
+```
+
+Linker Hand L20 独立 CAN 控制：
+
+```text
+src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py
+src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py
+scripts/linker_hand_control.sh
+```
+
+## 左右灵巧手 CAN 口对调
+
+如果后续需要再次颠倒左右灵巧手的 CAN 口，只需要修改：
+
+```text
+src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py
+```
+
+当前默认配置为：
+
+```python
+左手：'hand_type': 'left',  'can': 'can1'
+右手：'hand_type': 'right', 'can': 'can0'
+```
+
+如果要左右 CAN 口对调，改成：
+
+```python
+左手：'hand_type': 'left',  'can': 'can0'
+右手：'hand_type': 'right', 'can': 'can1'
+```
+
+修改后重新编译并重启服务：
+
+```bash
+cd ~/workspace/weipz-Lbot_Bridge
+colcon build --packages-select linker_hand_ros2_sdk
+source install/setup.bash
+./scripts/linker_hand_control.sh stop
+./scripts/linker_hand_control.sh start
+```
+
+注意：只改 `can` 字段，不要交换 `hand_type`。`hand_type` 决定 ROS 话题名和左右手设备 ID，交换它会导致 `/l20_left_*` 和 `/l20_right_*` 语义反掉。

scripts/lbot_arm_control.sh

@@ -0,0 +1,258 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+WS_DIR="$(cd "${SCRIPT_DIR}/.." && pwd)"
+STATE_DIR="/tmp/lbot_arm_control"
+PID_FILE="${STATE_DIR}/lbot_driver.pid"
+PGID_FILE="${STATE_DIR}/lbot_driver.pgid"
+BAG_PID_FILE="${STATE_DIR}/rosbag.pid"
+BAG_PGID_FILE="${STATE_DIR}/rosbag.pgid"
+LOG_FILE="${WS_DIR}/log/lbot_arm_control.log"
+BAG_LOG_FILE="${WS_DIR}/log/lbot_arm_rosbag.log"
+
+usage() {
+  echo "Usage: $0 {start|stop|status}"
+}
+
+is_running() {
+  [[ -f "${PID_FILE}" ]] || return 1
+  local pid
+  pid="$(cat "${PID_FILE}")"
+  [[ -n "${pid}" ]] || return 1
+  kill -0 "${pid}" 2>/dev/null
+}
+
+is_bag_running() {
+  [[ -f "${BAG_PID_FILE}" ]] || return 1
+  local pid
+  pid="$(cat "${BAG_PID_FILE}")"
+  [[ -n "${pid}" ]] || return 1
+  kill -0 "${pid}" 2>/dev/null
+}
+
+source_workspace() {
+  set +u
+  if [[ -n "${ROS_DISTRO:-}" && -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then
+    # shellcheck disable=SC1090
+    source "/opt/ros/${ROS_DISTRO}/setup.bash"
+  elif [[ -f "/opt/ros/jazzy/setup.bash" ]]; then
+    # shellcheck disable=SC1091
+    source "/opt/ros/jazzy/setup.bash"
+  fi
+  set -u
+
+  if [[ ! -f "${WS_DIR}/install/setup.bash" ]]; then
+    echo "Workspace is not built: ${WS_DIR}/install/setup.bash not found"
+    echo "Run: cd ${WS_DIR} && colcon build --packages-select lbot_arm_interfaces lbot_driver"
+    exit 1
+  fi
+
+  # shellcheck disable=SC1091
+  set +u
+  source "${WS_DIR}/install/setup.bash"
+  set -u
+}
+
+start_bag_recorder() {
+  mkdir -p "${STATE_DIR}" "$(dirname "${BAG_LOG_FILE}")"
+
+  if is_bag_running; then
+    echo "rosbag recorder is already running, pid=$(cat "${BAG_PID_FILE}")"
+    return 0
+  fi
+
+  echo "Starting rosbag recorder..."
+  echo "Rosbag log: ${BAG_LOG_FILE}"
+
+  : > "${BAG_LOG_FILE}"
+  setsid env WS_DIR="${WS_DIR}" bash -lc '
+    set -euo pipefail
+    set +u
+    source "${WS_DIR}/install/setup.bash"
+    set -u
+    while true; do
+      start_time=$(date +%Y%m%d_%H%M%S)
+      output=/tmp/lbot_arm_${start_time}.mcap
+      echo "Starting rosbag: ${output}"
+      set +e
+      timeout 3600 ros2 bag record \
+        --storage mcap \
+        --output "${output}" \
+        --polling-interval 2 \
+        --include-unpublished-topics \
+        --disable-keyboard-controls \
+        --log-level warn \
+        --topics \
+          /robot1/left_arm/joint_states \
+          /robot1/right_arm/joint_states \
+          /robot1/system_error \
+          /robot1/left_hand/set_l20_joint \
+          /robot1/right_hand/set_l20_joint \
+        --services \
+          /robot1/left_arm/move_joint \
+          /robot1/right_arm/move_joint
+      rc=$?
+      set -e
+      if [[ ${rc} -ne 124 ]]; then
+        exit ${rc}
+      fi
+    done
+  ' >> "${BAG_LOG_FILE}" 2>&1 &
+
+  local pid=$!
+  local pgid
+  pgid="$(ps -o pgid= -p "${pid}" | tr -d ' ')"
+  echo "${pid}" > "${BAG_PID_FILE}"
+  echo "${pgid:-${pid}}" > "${BAG_PGID_FILE}"
+
+  sleep 1
+  if is_bag_running; then
+    echo "rosbag recorder started, pid=${pid}, pgid=$(cat "${BAG_PGID_FILE}")"
+  else
+    echo "rosbag recorder failed to start. Check log: ${BAG_LOG_FILE}"
+    rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
+    return 1
+  fi
+}
+
+stop_bag_recorder() {
+  if ! [[ -f "${BAG_PID_FILE}" ]]; then
+    return 0
+  fi
+
+  local pid pgid
+  pid="$(cat "${BAG_PID_FILE}")"
+  pgid="${pid}"
+  if [[ -f "${BAG_PGID_FILE}" ]]; then
+    pgid="$(cat "${BAG_PGID_FILE}")"
+  fi
+
+  if ! kill -0 "${pid}" 2>/dev/null; then
+    rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
+    return 0
+  fi
+
+  echo "Stopping rosbag recorder, pid=${pid}, pgid=${pgid}..."
+  kill -TERM -- "-${pgid}" 2>/dev/null || kill -TERM "${pid}" 2>/dev/null || true
+
+  for _ in {1..30}; do
+    if ! kill -0 "${pid}" 2>/dev/null; then
+      rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
+      echo "rosbag recorder stopped"
+      return 0
+    fi
+    sleep 0.2
+  done
+
+  echo "rosbag recorder did not exit after SIGTERM, forcing stop..."
+  kill -KILL -- "-${pgid}" 2>/dev/null || kill -KILL "${pid}" 2>/dev/null || true
+  rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
+  echo "rosbag recorder stopped"
+}
+
+start_driver() {
+  mkdir -p "${STATE_DIR}" "$(dirname "${LOG_FILE}")"
+
+  if is_running; then
+    echo "lbot_driver is already running, pid=$(cat "${PID_FILE}")"
+    start_bag_recorder
+    return 0
+  fi
+
+  source_workspace
+
+  echo "Starting lbot_driver from officer_sdk..."
+  echo "Log: ${LOG_FILE}"
+
+  : > "${LOG_FILE}"
+  setsid bash -lc "set +u; source '${WS_DIR}/install/setup.bash'; set -u; exec ros2 launch lbot_driver lbot_start_driver.launch.py" \
+    >> "${LOG_FILE}" 2>&1 &
+
+  local pid=$!
+  local pgid
+  pgid="$(ps -o pgid= -p "${pid}" | tr -d ' ')"
+  echo "${pid}" > "${PID_FILE}"
+  echo "${pgid:-${pid}}" > "${PGID_FILE}"
+
+  sleep 1
+  if is_running; then
+    echo "lbot_driver started, pid=${pid}, pgid=$(cat "${PGID_FILE}")"
+    start_bag_recorder
+  else
+    echo "lbot_driver failed to start. Check log: ${LOG_FILE}"
+    rm -f "${PID_FILE}" "${PGID_FILE}"
+    exit 1
+  fi
+}
+
+stop_driver() {
+  if ! [[ -f "${PID_FILE}" ]]; then
+    echo "lbot_driver is not running"
+    stop_bag_recorder
+    return 0
+  fi
+
+  local pid pgid
+  pid="$(cat "${PID_FILE}")"
+  pgid="${pid}"
+  if [[ -f "${PGID_FILE}" ]]; then
+    pgid="$(cat "${PGID_FILE}")"
+  fi
+
+  if ! kill -0 "${pid}" 2>/dev/null; then
+    echo "lbot_driver process is not alive, cleaning state"
+    rm -f "${PID_FILE}" "${PGID_FILE}"
+    stop_bag_recorder
+    return 0
+  fi
+
+  echo "Stopping lbot_driver, pid=${pid}, pgid=${pgid}..."
+  kill -TERM -- "-${pgid}" 2>/dev/null || kill -TERM "${pid}" 2>/dev/null || true
+
+  for _ in {1..30}; do
+    if ! kill -0 "${pid}" 2>/dev/null; then
+      rm -f "${PID_FILE}" "${PGID_FILE}"
+      echo "lbot_driver stopped"
+      stop_bag_recorder
+      return 0
+    fi
+    sleep 0.2
+  done
+
+  echo "lbot_driver did not exit after SIGTERM, forcing stop..."
+  kill -KILL -- "-${pgid}" 2>/dev/null || kill -KILL "${pid}" 2>/dev/null || true
+  rm -f "${PID_FILE}" "${PGID_FILE}"
+  echo "lbot_driver stopped"
+  stop_bag_recorder
+}
+
+status_driver() {
+  if is_running; then
+    echo "lbot_driver is running, pid=$(cat "${PID_FILE}"), pgid=$(cat "${PGID_FILE}" 2>/dev/null || cat "${PID_FILE}")"
+  else
+    echo "lbot_driver is not running"
+  fi
+
+  if is_bag_running; then
+    echo "rosbag recorder is running, pid=$(cat "${BAG_PID_FILE}"), pgid=$(cat "${BAG_PGID_FILE}" 2>/dev/null || cat "${BAG_PID_FILE}")"
+  else
+    echo "rosbag recorder is not running"
+  fi
+}
+
+case "${1:-}" in
+  start)
+    start_driver
+    ;;
+  stop)
+    stop_driver
+    ;;
+  status)
+    status_driver
+    ;;
+  *)
+    usage
+    exit 1
+    ;;
+esac

scripts/linker_hand_control.sh

@@ -0,0 +1,321 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+WS_DIR="$(cd "${SCRIPT_DIR}/.." && pwd)"
+STATE_DIR="/tmp/linker_hand_control"
+PID_FILE="${STATE_DIR}/linker_hand.pid"
+PGID_FILE="${STATE_DIR}/linker_hand.pgid"
+BAG_PID_FILE="${STATE_DIR}/rosbag.pid"
+BAG_PGID_FILE="${STATE_DIR}/rosbag.pgid"
+LOG_FILE="${WS_DIR}/log/linker_hand_control.log"
+BAG_LOG_FILE="${WS_DIR}/log/linker_hand_rosbag.log"
+CAN_INTERFACES=(can0 can1)
+CAN_BITRATE=1000000
+ROSBAG_TOPICS=(
+  /l20_left_hand_control_cmd
+  /l20_left_hand_state
+  /l20_left_hand_matrix_touch_mass
+  /l20_right_hand_control_cmd
+  /l20_right_hand_state
+  /l20_right_hand_matrix_touch_mass
+)
+
+# Keep this driver isolated from ROS 2 nodes on other machines in the same LAN.
+# Without this, DDS discovery can pick up stale linker_hand_sdk nodes from the
+# subnet and make the same topic appear to have multiple publishers.
+export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST
+
+usage() {
+  echo "Usage: $0 {start|stop|status}"
+}
+
+is_running() {
+  [[ -f "${PID_FILE}" ]] || return 1
+  local pid
+  pid="$(cat "${PID_FILE}")"
+  [[ -n "${pid}" ]] || return 1
+  kill -0 "${pid}" 2>/dev/null
+}
+
+is_bag_running() {
+  [[ -f "${BAG_PID_FILE}" ]] || return 1
+  local pid
+  pid="$(cat "${BAG_PID_FILE}")"
+  [[ -n "${pid}" ]] || return 1
+  kill -0 "${pid}" 2>/dev/null
+}
+
+source_workspace() {
+  set +u
+  if [[ -n "${ROS_DISTRO:-}" && -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then
+    # shellcheck disable=SC1090
+    source "/opt/ros/${ROS_DISTRO}/setup.bash"
+  elif [[ -f "/opt/ros/jazzy/setup.bash" ]]; then
+    # shellcheck disable=SC1091
+    source "/opt/ros/jazzy/setup.bash"
+  fi
+  set -u
+
+  if [[ ! -f "${WS_DIR}/install/setup.bash" ]]; then
+    echo "Workspace is not built: ${WS_DIR}/install/setup.bash not found"
+    echo "Run: cd ${WS_DIR} && colcon build --packages-select linker_hand_ros2_sdk"
+    exit 1
+  fi
+
+  set +u
+  # shellcheck disable=SC1091
+  source "${WS_DIR}/install/setup.bash"
+  set -u
+}
+
+restart_ros_daemon_localhost() {
+  ros2 daemon stop >/dev/null 2>&1 || true
+  ros2 daemon start >/dev/null 2>&1 || true
+}
+
+start_can() {
+  for can_if in "${CAN_INTERFACES[@]}"; do
+    if ! ip link show "${can_if}" >/dev/null 2>&1; then
+      echo "CAN interface not found: ${can_if}"
+      exit 1
+    fi
+
+    echo "Starting ${can_if} at ${CAN_BITRATE} bps..."
+    sudo ip link set "${can_if}" down 2>/dev/null || true
+    sudo ip link set "${can_if}" type can bitrate "${CAN_BITRATE}"
+    sudo ip link set "${can_if}" up
+  done
+}
+
+stop_can() {
+  for can_if in "${CAN_INTERFACES[@]}"; do
+    if ip link show "${can_if}" >/dev/null 2>&1; then
+      echo "Stopping ${can_if}..."
+      sudo ip link set "${can_if}" down 2>/dev/null || true
+    fi
+  done
+}
+
+start_bag_recorder() {
+  mkdir -p "${STATE_DIR}" "$(dirname "${BAG_LOG_FILE}")"
+
+  if is_bag_running; then
+    echo "rosbag recorder is already running, pid=$(cat "${BAG_PID_FILE}")"
+    return 0
+  fi
+
+  echo "Starting rosbag recorder..."
+  echo "Rosbag log: ${BAG_LOG_FILE}"
+
+  : > "${BAG_LOG_FILE}"
+  setsid env \
+    WS_DIR="${WS_DIR}" \
+    ROS_AUTOMATIC_DISCOVERY_RANGE="${ROS_AUTOMATIC_DISCOVERY_RANGE}" \
+    bash -lc '
+      set -euo pipefail
+      set +u
+      source "${WS_DIR}/install/setup.bash"
+      set -u
+      while true; do
+        start_time=$(date +%Y%m%d_%H%M%S)
+        output=/tmp/L20_Hand_${start_time}.mcap
+        echo "Starting rosbag: ${output}"
+        set +e
+        timeout 3600 ros2 bag record \
+          --storage mcap \
+          --output "${output}" \
+          --polling-interval 2 \
+          --include-unpublished-topics \
+          --disable-keyboard-controls \
+          --log-level warn \
+          --topics \
+            /l20_left_hand_control_cmd \
+            /l20_left_hand_state \
+            /l20_left_hand_matrix_touch_mass \
+            /l20_right_hand_control_cmd \
+            /l20_right_hand_state \
+            /l20_right_hand_matrix_touch_mass
+        rc=$?
+        set -e
+        if [[ ${rc} -ne 124 ]]; then
+          exit ${rc}
+        fi
+      done
+    ' >> "${BAG_LOG_FILE}" 2>&1 &
+
+  local pid=$!
+  local pgid
+  pgid="$(ps -o pgid= -p "${pid}" | tr -d ' ')"
+  echo "${pid}" > "${BAG_PID_FILE}"
+  echo "${pgid:-${pid}}" > "${BAG_PGID_FILE}"
+
+  sleep 1
+  if is_bag_running; then
+    echo "rosbag recorder started, pid=${pid}, pgid=$(cat "${BAG_PGID_FILE}")"
+  else
+    echo "rosbag recorder failed to start. Check log: ${BAG_LOG_FILE}"
+    rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
+    return 1
+  fi
+}
+
+stop_bag_recorder() {
+  if ! [[ -f "${BAG_PID_FILE}" ]]; then
+    return 0
+  fi
+
+  local pid pgid
+  pid="$(cat "${BAG_PID_FILE}")"
+  pgid="${pid}"
+  if [[ -f "${BAG_PGID_FILE}" ]]; then
+    pgid="$(cat "${BAG_PGID_FILE}")"
+  fi
+
+  if ! kill -0 "${pid}" 2>/dev/null; then
+    rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
+    return 0
+  fi
+
+  echo "Stopping rosbag recorder, pid=${pid}, pgid=${pgid}..."
+  kill -TERM -- "-${pgid}" 2>/dev/null || kill -TERM "${pid}" 2>/dev/null || true
+
+  for _ in {1..30}; do
+    if ! kill -0 "${pid}" 2>/dev/null; then
+      rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
+      echo "rosbag recorder stopped"
+      return 0
+    fi
+    sleep 0.2
+  done
+
+  echo "rosbag recorder did not exit after SIGTERM, forcing stop..."
+  kill -KILL -- "-${pgid}" 2>/dev/null || kill -KILL "${pid}" 2>/dev/null || true
+  rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
+  echo "rosbag recorder stopped"
+}
+
+start_driver() {
+  mkdir -p "${STATE_DIR}" "$(dirname "${LOG_FILE}")"
+
+  if is_running; then
+    echo "linker_hand service is already running, pid=$(cat "${PID_FILE}")"
+    start_bag_recorder
+    return 0
+  fi
+
+  source_workspace
+  restart_ros_daemon_localhost
+  start_can
+
+  echo "Starting linker_hand service..."
+  echo "Log: ${LOG_FILE}"
+
+  : > "${LOG_FILE}"
+  setsid bash -lc "export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST; set +u; source '${WS_DIR}/install/setup.bash'; set -u; exec ros2 launch linker_hand_ros2_sdk linker_hand_double.launch.py" \
+    >> "${LOG_FILE}" 2>&1 &
+
+  local pid=$!
+  local pgid
+  pgid="$(ps -o pgid= -p "${pid}" | tr -d ' ')"
+  echo "${pid}" > "${PID_FILE}"
+  echo "${pgid:-${pid}}" > "${PGID_FILE}"
+
+  sleep 1
+  if is_running; then
+    echo "linker_hand service started, pid=${pid}, pgid=$(cat "${PGID_FILE}")"
+    start_bag_recorder
+  else
+    echo "linker_hand service failed to start. Check log: ${LOG_FILE}"
+    rm -f "${PID_FILE}" "${PGID_FILE}"
+    stop_can
+    exit 1
+  fi
+}
+
+stop_driver() {
+  if ! [[ -f "${PID_FILE}" ]]; then
+    echo "linker_hand service is not running"
+    stop_bag_recorder
+    stop_can
+    return 0
+  fi
+
+  local pid pgid
+  pid="$(cat "${PID_FILE}")"
+  pgid="${pid}"
+  if [[ -f "${PGID_FILE}" ]]; then
+    pgid="$(cat "${PGID_FILE}")"
+  fi
+
+  if ! kill -0 "${pid}" 2>/dev/null; then
+    echo "linker_hand process is not alive, cleaning state"
+    rm -f "${PID_FILE}" "${PGID_FILE}"
+    stop_bag_recorder
+    stop_can
+    return 0
+  fi
+
+  echo "Stopping linker_hand service, pid=${pid}, pgid=${pgid}..."
+  kill -TERM -- "-${pgid}" 2>/dev/null || kill -TERM "${pid}" 2>/dev/null || true
+
+  for _ in {1..30}; do
+    if ! kill -0 "${pid}" 2>/dev/null; then
+      rm -f "${PID_FILE}" "${PGID_FILE}"
+      echo "linker_hand service stopped"
+      stop_bag_recorder
+      stop_can
+      return 0
+    fi
+    sleep 0.2
+  done
+
+  echo "linker_hand service did not exit after SIGTERM, forcing stop..."
+  kill -KILL -- "-${pgid}" 2>/dev/null || kill -KILL "${pid}" 2>/dev/null || true
+  rm -f "${PID_FILE}" "${PGID_FILE}"
+  echo "linker_hand service stopped"
+  stop_bag_recorder
+  stop_can
+}
+
+status_driver() {
+  if is_running; then
+    echo "linker_hand service is running, pid=$(cat "${PID_FILE}"), pgid=$(cat "${PGID_FILE}" 2>/dev/null || cat "${PID_FILE}")"
+  else
+    echo "linker_hand service is not running"
+  fi
+
+  echo "ROS_AUTOMATIC_DISCOVERY_RANGE=${ROS_AUTOMATIC_DISCOVERY_RANGE}"
+
+  if is_bag_running; then
+    echo "rosbag recorder is running, pid=$(cat "${BAG_PID_FILE}"), pgid=$(cat "${BAG_PGID_FILE}" 2>/dev/null || cat "${BAG_PID_FILE}")"
+  else
+    echo "rosbag recorder is not running"
+  fi
+
+  echo "rosbag topics: ${ROSBAG_TOPICS[*]}"
+
+  for can_if in "${CAN_INTERFACES[@]}"; do
+    if ip link show "${can_if}" >/dev/null 2>&1; then
+      ip -details link show "${can_if}" | sed -n '1,2p'
+    else
+      echo "${can_if}: not found"
+    fi
+  done
+}
+
+case "${1:-}" in
+  start)
+    start_driver
+    ;;
+  stop)
+    stop_driver
+    ;;
+  status)
+    status_driver
+    ;;
+  *)
+    usage
+    exit 1
+    ;;
+esac

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*权限
+            }],
+        ),
+    ])

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': "L20", # 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': "L20", # 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*
+            }],
+        ),
+    ])

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总线名称
+            }],
+        ),
+    ])

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

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

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

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

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

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

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: "can1"  # 配置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: L20 # 左手型号 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: 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: L20 # 右手型号 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权限

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

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

@@ -0,0 +1,546 @@
+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
+    ROLL_POS = 0x01                # All fingers roll joint position
+    YAW_POS = 0x02                 # All fingers yaw joint position
+    ROOT1_POS = 0x03               # All fingers root1 joint position
+    ROOT2_POS = 0x04               # All fingers root2 joint position
+    ROOT3_POS = 0x05               # All fingers root3 joint position
+    TIP_POS = 0x06                 # All fingers tip joint position
+    ROLL_SPEED = 0x09              # All fingers roll joint speed
+    YAW_SPEED = 0x0A               # All fingers yaw joint speed
+    ROOT1_SPEED = 0x0B             # All fingers root1 joint speed
+    ROOT2_SPEED = 0x0C             # All fingers root2 joint speed
+    ROOT3_SPEED = 0x0D             # All fingers root3 joint speed
+    TIP_SPEED = 0x0E               # All fingers tip joint speed
+    ROLL_TORQUE = 0x11             # All fingers roll joint load/current threshold
+    YAW_TORQUE = 0x12              # All fingers yaw joint load/current threshold
+    ROOT1_TORQUE = 0x13            # All fingers root1 joint load/current threshold
+    ROOT2_TORQUE = 0x14            # All fingers root2 joint load/current threshold
+    ROOT3_TORQUE = 0x15            # All fingers root3 joint load/current threshold
+    TIP_TORQUE = 0x16              # All fingers tip joint load/current threshold
+    ROLL_FAULT = 0x19              # All fingers roll joint fault
+    YAW_FAULT = 0x1A               # All fingers yaw joint fault
+    ROOT1_FAULT = 0x1B             # All fingers root1 joint fault
+    ROOT2_FAULT = 0x1C             # All fingers root2 joint fault
+    ROOT3_FAULT = 0x1D             # All fingers root3 joint fault
+    TIP_FAULT = 0x1E               # All fingers tip joint fault
+    ROLL_TEMPERATURE = 0x21        # All fingers roll joint temperature
+    YAW_TEMPERATURE = 0x22         # All fingers yaw joint temperature
+    ROOT1_TEMPERATURE = 0x23       # All fingers root1 joint temperature
+    ROOT2_TEMPERATURE = 0x24       # All fingers root2 joint temperature
+    ROOT3_TEMPERATURE = 0x25       # All fingers root3 joint temperature
+    TIP_TEMPERATURE = 0x26         # All fingers tip joint temperature
+    FINGER_SPEED = 0x81            # Per-finger speed
+    FINGER_TORQUE = 0x82           # Per-finger torque/current threshold
+    FINGER_FAULT = 0x83            # Per-finger fault
+    FINGER_TEMPERATURE = 0x84      # Per-finger temperature
+    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 = [],[]
+        self.x09, self.x0a, self.x0b, self.x0c, self.x0d, self.x0e = [[-1] * 5 for _ in range(6)]
+        self.x11, self.x12, self.x13, self.x14, self.x15, self.x16 = [[-1] * 5 for _ in range(6)]
+        self.x19, self.x1a, self.x1b, self.x1c, self.x1d, self.x1e = [[-1] * 5 for _ in range(6)]
+        self.x21, self.x22, self.x23, self.x24, self.x25, self.x26 = [[-1] * 5 for _ in range(6)]
+        self.x81, self.x82, self.x83, self.x84 = [[-1] * 5 for _ in range(4)]
+        # 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,
+        }
+        
+        # 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 before querying touch type so replies can be processed.
+        self.receive_thread = threading.Thread(target=self.receive_response)
+        self.receive_thread.daemon = True
+        self.receive_thread.start()
+        self.get_touch_type()
+        time.sleep(0.1)
+
+    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.ROOT1_POS, angles)
+
+    def set_finger_tip(self, angles):
+        self.send_command(FrameProperty.TIP_POS, angles)
+
+    def set_finger_middle(self, angles):
+        self.send_command(FrameProperty.YAW_POS, angles)
+
+    def set_thumb_roll(self, angle):
+        self.send_command(FrameProperty.ROLL_POS, 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(FrameProperty.YAW_POS, angles)
+
+    def set_joint_roll(self, thumb_roll):
+        self.send_command(FrameProperty.ROLL_POS, [thumb_roll, 0, 0, 0, 0])
+
+    def set_joint_speed(self, speed):
+        self.x81 = speed
+        self.send_command(FrameProperty.FINGER_SPEED, speed)
+    def set_electric_current(self, e_c=[]):
+        self.x82 = e_c
+        self.send_command(FrameProperty.FINGER_TORQUE, e_c)
+
+    def get_normal_force(self):
+        self.send_command(0x90, [])
+
+    def get_tangential_force(self):
+        self.send_command(0x91, [])
+
+
+    def get_tangential_force_dir(self):
+        self.send_command(0x92, [])
+
+    def get_approach_inc(self):
+        self.send_command(0x93, [])
+
+
+
+
+    def get_electric_current(self, e_c=[]):
+        self.send_command(FrameProperty.FINGER_TORQUE, 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 == 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 == 0x0E:
+                self.x0e = list(response_data)
+            elif frame_type == 0x11:
+                self.x11 = list(response_data)
+            elif frame_type == 0x12:
+                self.x12 = list(response_data)
+            elif frame_type == 0x13:
+                self.x13 = list(response_data)
+            elif frame_type == 0x14:
+                self.x14 = list(response_data)
+            elif frame_type == 0x15:
+                self.x15 = list(response_data)
+            elif frame_type == 0x16:
+                self.x16 = list(response_data)
+            elif frame_type == 0x19:
+                self.x19 = list(response_data)
+            elif frame_type == 0x1A:
+                self.x1a = list(response_data)
+            elif frame_type == 0x1B:
+                self.x1b = list(response_data)
+            elif frame_type == 0x1C:
+                self.x1c = list(response_data)
+            elif frame_type == 0x1D:
+                self.x1d = list(response_data)
+            elif frame_type == 0x1E:
+                self.x1e = list(response_data)
+            elif frame_type == 0x20:
+                d = list(response_data)
+                self.normal_force = [float(i) for i in d] 
+            elif frame_type == 0x21:
+                self.x21 = list(response_data)
+            elif frame_type == 0x22:
+                self.x22 = list(response_data)
+            elif frame_type == 0x23:
+                self.x23 = list(response_data)
+            elif frame_type == 0x24:
+                self.x24 = list(response_data)
+            elif frame_type == 0x25:
+                self.x25 = list(response_data)
+            elif frame_type == 0x26:
+                self.x26 = list(response_data)
+            elif frame_type == 0x81:
+                self.x81 = list(response_data)
+            elif frame_type == 0x82:
+                self.x82 = list(response_data)
+            elif frame_type == 0x83:
+                self.x83 = list(response_data)
+            elif frame_type == 0x84:
+                self.x84 = list(response_data)
+            elif frame_type == 0x90:
+                self.normal_force = [float(i) for i in list(response_data)]
+            elif frame_type == 0x91:
+                self.tangential_force = [float(i) for i in list(response_data)]
+            elif frame_type == 0x92:
+                self.tangential_force_dir = [float(i) for i in list(response_data)]
+            elif frame_type == 0x93:
+                self.approach_inc = [float(i) for i in 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
+    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.x81 = speed
+        self.send_command(FrameProperty.FINGER_SPEED, 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 per-finger torque/current threshold.'''
+        self.x82 = current
+        self.send_command(FrameProperty.FINGER_TORQUE, 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(FrameProperty.ROOT1_POS, [], sleep=0.01)
+        self.send_command(FrameProperty.YAW_POS, [], sleep=0.01)
+        self.send_command(FrameProperty.ROLL_POS, [], sleep=0.01)
+        self.send_command(FrameProperty.TIP_POS, [], sleep=0.01)
+        return self.x03 + self.x02 + self.x01 + self.x06
+    
+    def get_current_pub_status(self):
+        time.sleep(0.01)
+        return self.x03 + self.x02 + self.x01 + self.x06
+
+    def get_speed(self):
+        '''Get current motor speed'''
+        self.send_command(FrameProperty.FINGER_SPEED, [])
+        time.sleep(0.001)
+        return self.x81
+    def get_current(self):
+        '''Get per-finger torque/current threshold.'''
+        self.send_command(FrameProperty.FINGER_TORQUE, [])
+        return self.x82
+    def get_torque(self):
+        '''Get current motor torque/current threshold.'''
+        self.send_command(FrameProperty.FINGER_TORQUE, [])
+        return self.x82
+    def get_fault(self):
+        self.send_command(FrameProperty.FINGER_FAULT, [])
+        time.sleep(0.01)
+        return self.x83
+    
+    def get_temperature(self):
+        '''Get motor temperature'''
+        self.send_command(FrameProperty.FINGER_TEMPERATURE, [])
+        return self.x84
+        
+    def clear_faults(self):
+        '''Clear motor faults'''
+        self.send_command(FrameProperty.FINGER_FAULT, [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(0x90, [], 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(FrameProperty.FINGER_FAULT, [])
+        return self.x83
+    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

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

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

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

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

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

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

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}")

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}")

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}")

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}")

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}")

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")

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)

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
+
+        

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
+        

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

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
+    

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py

@@ -0,0 +1,571 @@
+#!/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 rclpy.parameter import Parameter
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+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
+
+
+COMMAND_QOS = QoSProfile(
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+    reliability=ReliabilityPolicy.RELIABLE,
+)
+
+
+def command_changed(previous, current):
+    """Return whether a non-empty command differs from the applied command."""
+    values = list(current)
+    if not values:
+        return False
+    if previous is None or len(previous) != len(values):
+        return True
+    return any(float(old) != float(new) for old, new in zip(previous, values))
+
+
+def state_poll_due(last_poll_time, now, poll_period):
+    """Keep slow CAN state reads off the latency-sensitive command path."""
+    return last_poll_time is None or now >= last_poll_time + poll_period
+
+
+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")
+        # -1 keeps the model's original startup speed. Camera teleoperation can
+        # set this to a conservative value before the startup pose is sent.
+        self.declare_parameter('startup_speed', -1)
+        # Empty keeps the legacy absolute topics/startup pose. A prefix lets
+        # two same-side hands coexist without receiving each other's commands.
+        self.declare_parameter('topic_prefix', '')
+        self.declare_parameter('startup_pose', Parameter.Type.INTEGER_ARRAY)
+        # Hardware state reads are synchronous CAN transactions.  Keeping them
+        # below the command rate prevents G20's five-frame reads from starving
+        # incoming position commands.
+        self.declare_parameter('state_poll_rate', 60.0)
+        self.declare_parameter('velocity_poll_rate', 60.0)
+
+        # 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.startup_speed = int(self.get_parameter('startup_speed').value)
+        if self.startup_speed < -1 or self.startup_speed > 255:
+            raise ValueError('startup_speed must be -1 or in the range [0, 255]')
+        self.topic_prefix = self.normalize_topic_prefix(
+            self.get_parameter('topic_prefix').value
+        )
+        self.state_poll_rate = float(
+            self.get_parameter('state_poll_rate').value
+        )
+        if self.state_poll_rate <= 0.0:
+            raise ValueError('state_poll_rate must be greater than zero')
+        self.state_poll_period = 1.0 / self.state_poll_rate
+        self.last_state_poll_time = None
+        self.velocity_poll_rate = float(
+            self.get_parameter('velocity_poll_rate').value
+        )
+        if self.velocity_poll_rate <= 0.0:
+            raise ValueError('velocity_poll_rate must be greater than zero')
+        self.velocity_poll_period = 1.0 / self.velocity_poll_rate
+        self.last_velocity_poll_time = None
+        configured_startup_pose = self.get_parameter_or(
+            'startup_pose',
+            Parameter('startup_pose', Parameter.Type.INTEGER_ARRAY, []),
+        ).value
+        self.startup_pose = [int(value) for value in configured_startup_pose]
+        if any(value < 0 or value > 255 for value in self.startup_pose):
+            raise ValueError('startup_pose values must be in the range [0, 255]')
+        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.applied_hand_post_cmd = None
+        self.applied_hand_vel_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, self.topic('/l20_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()
+
+    @staticmethod
+    def normalize_topic_prefix(prefix):
+        prefix = str(prefix).strip()
+        if not prefix or prefix == '/':
+            return ''
+        if not prefix.startswith('/'):
+            prefix = '/' + prefix
+        return prefix.rstrip('/')
+
+    def topic(self, absolute_topic):
+        if not absolute_topic.startswith('/'):
+            raise ValueError('base topic must be absolute')
+        return self.topic_prefix + absolute_topic
+
+    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,
+            self.topic(f'/l20_{self.hand_type}_hand_control_cmd'),
+            self.hand_control_cb,
+            COMMAND_QOS,
+        )
+        self.hand_state_pub = self.create_publisher(JointState, self.topic(f'/l20_{self.hand_type}_hand_state'),10)
+        self.hand_info_pub = self.create_publisher(String, self.topic(f'/l20_{self.hand_type}_hand_info'), 10)
+        if self.is_touch == True:
+            if self.modbus != "None":
+                self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch'), 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch_pc'), 10)
+                self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch_mass'), 10)
+            elif self.hand_joint == "L20":
+                ColorMsg(msg=f"{self.hand_type} {self.hand_joint} matrix pressure sensing enabled", color='green')
+                self.touch_type = 2
+                self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch'), 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch_pc'), 10)
+                self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, self.topic(f'/l20_{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, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch'), 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/l20_{self.hand_type}_hand_matrix_touch_pc'), 10)
+                self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, self.topic(f'/l20_{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, self.topic(f'/l20_{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 = None
+        elif self.hand_joint == "G20":
+            # G20 uses a different CAN protocol from L20 even though both
+            # expose a 20-value ROS command. This is the calibrated startup
+            # pose also used by linker_hand_advanced_g20.
+            pose = [255,255,255,255,255,255,193,148,105,42,245,255,255,255,255,255,255,255,255,255]
+            torque = [255] * 5
+            speed = [255] * 5
+        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 self.startup_pose:
+            expected_lengths = {
+                "O6": 6,
+                "L6": 6,
+                "L6P": 6,
+                "L7": 7,
+                "L10": 10,
+                "L20": 20,
+                "G20": 20,
+                "L21": 25,
+                "L25": 25,
+            }
+            expected = expected_lengths.get(str(self.hand_joint).upper(), 0)
+            if len(self.startup_pose) != expected:
+                raise ValueError(
+                    f'startup_pose for {self.hand_joint} must contain {expected} values'
+                )
+            pose = list(self.startup_pose)
+        if self.startup_speed >= 0:
+            speed = [self.startup_speed] * len(speed)
+        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 hand_control_cb(self, msg):
+        # The hardware can be slower than the camera.  Always replace a
+        # pending command with the newest sample and never replay an already
+        # applied sample; this prevents latency from accumulating in software.
+        position = list(msg.position)
+        if position:
+            self.last_hand_post_cmd = (
+                position
+                if command_changed(self.applied_hand_post_cmd, position)
+                else None
+            )
+
+        velocity = list(msg.velocity)
+        if velocity:
+            self.last_hand_vel_cmd = (
+                velocity
+                if command_changed(self.applied_hand_vel_cmd, velocity)
+                else None
+            )
+
+        effort = list(msg.effort)
+        if effort:
+            self.last_hand_eff_cmd = effort
+
+    def _apply_pending_commands(self):
+        if self.cmd_lock:
+            return
+
+        if self.last_hand_post_cmd is not None:
+            pose = list(self.last_hand_post_cmd)
+            self.api.finger_move(pose=pose)
+            self.applied_hand_post_cmd = pose
+            self.last_hand_post_cmd = None
+
+        if self.last_hand_vel_cmd is not None:
+            vel = list(self.last_hand_vel_cmd)
+            if not all(x == 0 for x in vel):
+                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 == "G20" and len(vel) == 20:
+                    # G20 speed is configured per finger, not per one of
+                    # the 20 exposed position slots.
+                    speed = [vel[0],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.applied_hand_vel_cmd = vel
+            self.last_hand_vel_cmd = None
+
+    def _poll_state_if_due(self):
+        if self.hand_state_pub.get_subscription_count() < 1:
+            return
+        now = time.monotonic()
+        if not state_poll_due(
+            self.last_state_poll_time, now, self.state_poll_period
+        ):
+            return
+        # Record the start time so a slow G20 read cannot immediately trigger
+        # another read on the following timer callback.
+        self.last_state_poll_time = now
+        self.last_hand_state = self.api.get_state()
+        time.sleep(0.003)
+        if state_poll_due(
+            self.last_velocity_poll_time, now, self.velocity_poll_period
+        ):
+            self.last_velocity_poll_time = now
+            self.last_hand_vel = self.api.get_joint_speed()
+            time.sleep(0.002)
+
+    def run(self):
+        if self.sdk_v == 1:
+            self.sleep_time = 0.009
+        # Position commands have priority over synchronous state reads.
+        self._apply_pending_commands()
+        self._poll_state_if_due()
+        if self.cmd_lock == False:
+            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.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()
+                # 发布矩阵压感合值浮点数组
+                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):
+        """发布矩阵数据合值，顺序为 thumb, index, middle, ring, little，单位 g。"""
+        msg = Float32MultiArray()
+        msg.data = [
+            float(sum(sum(row) for row in dic["thumb_matrix"])),
+            float(sum(sum(row) for row in dic["index_matrix"])),
+            float(sum(sum(row) for row in dic["middle_matrix"])),
+            float(sum(sum(row) for row in dic["ring_matrix"])),
+            float(sum(sum(row) for row in dic["little_matrix"])),
+        ]
+        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 or []
+        hand_joint = node.hand_joint.upper()
+        if len(embedded_version) == 3 or hand_joint in {"O6", "L6", "G20"}:
+            ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+            node.sdk_v = 2
+        elif len(embedded_version) == 6 and 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("程序已退出。")

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("程序已退出。")

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py

@@ -0,0 +1,251 @@
+#!/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'/l20_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/l20_{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'/l20_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/l20_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{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'/l20_{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()
+            # 发布矩阵压感合值浮点数组
+            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):
+        """发布矩阵数据合值，顺序为 thumb, index, middle, ring, little，单位 g。"""
+        msg = Float32MultiArray()
+        msg.data = [
+            float(sum(sum(row) for row in dic["thumb_matrix"])),
+            float(sum(sum(row) for row in dic["index_matrix"])),
+            float(sum(sum(row) for row in dic["middle_matrix"])),
+            float(sum(sum(row) for row in dic["ring_matrix"])),
+            float(sum(sum(row) for row in dic["little_matrix"])),
+        ]
+        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):
+    '''
+    本节点用于收集手指状态和压感数据。
+    '/l20_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+    /l20_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz
+    '/l20_{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("程序已退出。")

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py

@@ -0,0 +1,239 @@
+#!/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'/l20_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/l20_{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'/l20_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/l20_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{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'/l20_{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()
+            # 发布矩阵压感合值浮点数组
+            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):
+        """发布矩阵数据合值，顺序为 thumb, index, middle, ring, little，单位 g。"""
+        msg = Float32MultiArray()
+        msg.data = [
+            float(sum(sum(row) for row in dic["thumb_matrix"])),
+            float(sum(sum(row) for row in dic["index_matrix"])),
+            float(sum(sum(row) for row in dic["middle_matrix"])),
+            float(sum(sum(row) for row in dic["ring_matrix"])),
+            float(sum(sum(row) for row in dic["little_matrix"])),
+        ]
+        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):
+    '''
+    本节点用于收集手指状态和压感数据。
+    '/l20_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+    /l20_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz
+    '/l20_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz
+    '/l20_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{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("程序已退出。")

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py

@@ -0,0 +1,244 @@
+#!/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'/l20_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/l20_{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'/l20_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/l20_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{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'/l20_{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()
+            # 发布矩阵压感合值浮点数组
+            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):
+        """发布矩阵数据合值，顺序为 thumb, index, middle, ring, little，单位 g。"""
+        msg = Float32MultiArray()
+        msg.data = [
+            float(sum(sum(row) for row in dic["thumb_matrix"])),
+            float(sum(sum(row) for row in dic["index_matrix"])),
+            float(sum(sum(row) for row in dic["middle_matrix"])),
+            float(sum(sum(row) for row in dic["ring_matrix"])),
+            float(sum(sum(row) for row in dic["little_matrix"])),
+        ]
+        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):
+    '''
+    本节点用于收集手指状态和压感数据。
+    '/l20_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+    /l20_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz
+    '/l20_{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("程序已退出。")

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py

@@ -0,0 +1,274 @@
+#!/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'/l20_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/l20_{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'/l20_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/l20_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{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'/l20_{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()
+            # 发布矩阵压感合值浮点数组
+            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):
+        """发布矩阵数据合值，顺序为 thumb, index, middle, ring, little，单位 g。"""
+        msg = Float32MultiArray()
+        msg.data = [
+            float(sum(sum(row) for row in dic["thumb_matrix"])),
+            float(sum(sum(row) for row in dic["index_matrix"])),
+            float(sum(sum(row) for row in dic["middle_matrix"])),
+            float(sum(sum(row) for row in dic["ring_matrix"])),
+            float(sum(sum(row) for row in dic["little_matrix"])),
+        ]
+        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):
+    '''
+    本节点用于收集手指状态和压感数据。
+    '/l20_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+    /l20_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz
+    '/l20_{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("程序已退出。")

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py

@@ -0,0 +1,245 @@
+#!/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'/l20_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/l20_{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'/l20_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/l20_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{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'/l20_{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()
+            # 发布矩阵压感合值浮点数组
+            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):
+        """发布矩阵数据合值，顺序为 thumb, index, middle, ring, little，单位 g。"""
+        msg = Float32MultiArray()
+        msg.data = [
+            float(sum(sum(row) for row in dic["thumb_matrix"])),
+            float(sum(sum(row) for row in dic["index_matrix"])),
+            float(sum(sum(row) for row in dic["middle_matrix"])),
+            float(sum(sum(row) for row in dic["ring_matrix"])),
+            float(sum(sum(row) for row in dic["little_matrix"])),
+        ]
+        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):
+    '''
+    本节点用于收集手指状态和压感数据。
+    '/l20_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+    /l20_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 50Hz
+    '/l20_{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("程序已退出。")

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py

@@ -0,0 +1,260 @@
+#!/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'/l20_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_state_pub = self.create_publisher(JointState, f'/l20_{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'/l20_{self.hand_type}_hand_matrix_touch', 10)
+                #self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/l20_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(Float32MultiArray, f'/l20_{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'/l20_{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()
+            # 发布矩阵压感合值浮点数组
+            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):
+        """发布矩阵数据合值，顺序为 thumb, index, middle, ring, little，单位 g。"""
+        msg = Float32MultiArray()
+        msg.data = [
+            float(sum(self.api.hand.thumb_matrix_palm_mass)),
+            float(sum(self.api.hand.index_matrix_palm_mass)),
+            float(sum(self.api.hand.middle_matrix_palm_mass)),
+            float(sum(self.api.hand.ring_matrix_palm_mass)),
+            float(sum(self.api.hand.little_matrix_palm_mass)),
+        ]
+        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):
+    '''
+    本节点用于收集手指状态和压感数据。
+    '/l20_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+    /l20_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz
+    '/l20_{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("程序已退出。")

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/o6_joint_curve.py

@@ -0,0 +1,331 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""Record O6 joint positions and plot angle/derived-velocity curves.
+
+The O6 ``0x05`` value exposed by the SDK is a configured speed, not a measured
+joint velocity.  This recorder therefore always derives velocity from the
+position samples and their monotonic timestamps.
+"""
+
+import argparse
+import csv
+from datetime import datetime
+import json
+import math
+from pathlib import Path
+import sys
+import time
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.utilities import remove_ros_args
+from sensor_msgs.msg import JointState
+
+
+O6_JOINT_NAMES = (
+    "thumb_cmc_pitch",
+    "thumb_cmc_yaw",
+    "index_mcp_pitch",
+    "middle_mcp_pitch",
+    "ring_mcp_pitch",
+    "pinky_mcp_pitch",
+)
+
+# Nominal SDK ranges.  A hand-specific lookup JSON should be used for formal
+# measurements because individual hands can differ from these values.
+O6_NOMINAL_MAX_DEG = dict(
+    zip(O6_JOINT_NAMES, np.rad2deg([0.58, 1.36, 1.6, 1.6, 1.6, 1.6]))
+)
+
+
+class AngleMapper:
+    """Convert O6 uint8 positions to degrees using lookup tables or nominal ranges."""
+
+    def __init__(self, mapping_json=None):
+        self.tables = {}
+        self.source = "SDK nominal range"
+        if mapping_json:
+            self._load(Path(mapping_json).expanduser())
+
+    def _load(self, path):
+        with path.open("r", encoding="utf-8") as stream:
+            data = json.load(stream)
+
+        joints = data.get("joints", data)
+        if not isinstance(joints, dict):
+            raise ValueError("mapping JSON must contain an object named 'joints'")
+
+        for joint_name, values in joints.items():
+            if not isinstance(values, dict):
+                continue
+            joint_u8 = values.get("joint_u8")
+            joint_real_deg = values.get("joint_real_deg")
+            if joint_u8 is None or joint_real_deg is None:
+                continue
+            if len(joint_u8) != len(joint_real_deg) or len(joint_u8) < 2:
+                raise ValueError(
+                    f"{joint_name}: joint_u8 and joint_real_deg need equal lengths >= 2"
+                )
+
+            x = np.asarray(joint_u8, dtype=float)
+            y = np.asarray(joint_real_deg, dtype=float)
+            if not np.all(np.isfinite(x)) or not np.all(np.isfinite(y)):
+                raise ValueError(f"{joint_name}: mapping contains a non-finite value")
+            order = np.argsort(x)
+            x = x[order]
+            y = y[order]
+            if np.any(np.diff(x) <= 0):
+                raise ValueError(f"{joint_name}: joint_u8 values must be unique")
+            self.tables[joint_name] = (x, y)
+
+        if not self.tables:
+            raise ValueError(
+                "mapping JSON has no joint with joint_u8 and joint_real_deg arrays"
+            )
+        self.source = str(path)
+
+    def to_degrees(self, joint_name, position_u8):
+        if joint_name in self.tables:
+            x, y = self.tables[joint_name]
+            return float(np.interp(position_u8, x, y))
+
+        if joint_name not in O6_NOMINAL_MAX_DEG:
+            raise KeyError(f"no O6 nominal range or lookup table for {joint_name}")
+        position_u8 = float(np.clip(position_u8, 0.0, 255.0))
+        return (255.0 - position_u8) * O6_NOMINAL_MAX_DEG[joint_name] / 255.0
+
+
+def derive_velocity(time_s, angle_deg, cutoff_hz=5.0):
+    """Differentiate angle samples and apply a first-order low-pass filter."""
+    time_s = np.asarray(time_s, dtype=float)
+    angle_deg = np.asarray(angle_deg, dtype=float)
+    if len(time_s) != len(angle_deg):
+        raise ValueError("time and angle arrays must have the same length")
+    if len(time_s) < 2:
+        return np.zeros_like(angle_deg)
+    if np.any(np.diff(time_s) <= 0):
+        raise ValueError("timestamps must be strictly increasing")
+
+    velocity = np.gradient(angle_deg, time_s)
+    if cutoff_hz <= 0.0:
+        return velocity
+
+    filtered = np.empty_like(velocity)
+    filtered[0] = velocity[0]
+    rc = 1.0 / (2.0 * math.pi * cutoff_hz)
+    for index in range(1, len(velocity)):
+        dt = time_s[index] - time_s[index - 1]
+        alpha = dt / (rc + dt)
+        filtered[index] = filtered[index - 1] + alpha * (
+            velocity[index] - filtered[index - 1]
+        )
+    return filtered
+
+
+class O6JointCurveRecorder(Node):
+    def __init__(
+        self,
+        hand_type,
+        duration,
+        output_dir,
+        joint_name,
+        mapping_json,
+        filter_hz,
+    ):
+        super().__init__("o6_joint_curve")
+        self.hand_type = hand_type
+        self.duration = duration
+        self.output_dir = Path(output_dir).expanduser()
+        self.joint_name = joint_name
+        self.filter_hz = filter_hz
+        self.mapper = AngleMapper(mapping_json)
+        self.samples = []
+        self.start_time = None
+        self.saved_paths = None
+        self.finished = False
+
+        topic = f"/l20_{hand_type}_hand_state"
+        self.subscription = self.create_subscription(
+            JointState, topic, self._state_callback, 100
+        )
+        self.stop_timer = self.create_timer(0.05, self._check_duration)
+        self.get_logger().info(
+            f"Recording {topic}; duration={duration:.3f}s; angle map={self.mapper.source}"
+        )
+
+    def _state_callback(self, msg):
+        if self.finished or not msg.position:
+            return
+
+        names = tuple(msg.name) if len(msg.name) == len(msg.position) else O6_JOINT_NAMES
+        if len(msg.position) != len(names):
+            self.get_logger().warning(
+                f"Ignoring JointState with {len(msg.position)} positions and "
+                f"{len(msg.name)} names"
+            )
+            return
+
+        now = time.perf_counter()
+        if self.start_time is None:
+            self.start_time = now
+        relative_time = now - self.start_time
+        for name, position in zip(names, msg.position):
+            if self.joint_name != "all" and name != self.joint_name:
+                continue
+            try:
+                angle_deg = self.mapper.to_degrees(name, position)
+            except KeyError as error:
+                self.get_logger().warning(str(error))
+                continue
+            self.samples.append((relative_time, name, float(position), angle_deg))
+
+    def _check_duration(self):
+        if self.finished or self.start_time is None or self.duration <= 0.0:
+            return
+        if time.perf_counter() - self.start_time >= self.duration:
+            self.finish()
+            rclpy.shutdown()
+
+    def finish(self):
+        if self.finished:
+            return self.saved_paths
+        self.finished = True
+        if not self.samples:
+            self.get_logger().warning("No joint-state sample received; no files were written")
+            return None
+        self.saved_paths = export_curves(
+            samples=self.samples,
+            output_dir=self.output_dir,
+            hand_type=self.hand_type,
+            mapping_source=self.mapper.source,
+            cutoff_hz=self.filter_hz,
+        )
+        csv_path, plot_path = self.saved_paths
+        self.get_logger().info(f"Saved samples: {csv_path}")
+        self.get_logger().info(f"Saved curves: {plot_path}")
+        return self.saved_paths
+
+
+def export_curves(samples, output_dir, hand_type, mapping_source, cutoff_hz):
+    """Export long-form CSV and a two-panel PNG for all recorded joints."""
+    import matplotlib
+
+    matplotlib.use("Agg")
+    import matplotlib.pyplot as plt
+
+    output_dir = Path(output_dir).expanduser()
+    output_dir.mkdir(parents=True, exist_ok=True)
+    stem = f"o6_{hand_type}_{datetime.now().strftime('%Y%m%d_%H%M%S')}"
+    csv_path = output_dir / f"{stem}.csv"
+    plot_path = output_dir / f"{stem}.png"
+
+    joint_samples = {}
+    for sample in samples:
+        joint_samples.setdefault(sample[1], []).append(sample)
+
+    processed = {}
+    for joint_name, values in joint_samples.items():
+        time_s = np.asarray([value[0] for value in values], dtype=float)
+        position_u8 = np.asarray([value[2] for value in values], dtype=float)
+        angle_deg = np.asarray([value[3] for value in values], dtype=float)
+        velocity_deg_s = derive_velocity(time_s, angle_deg, cutoff_hz)
+        processed[joint_name] = (time_s, position_u8, angle_deg, velocity_deg_s)
+
+    with csv_path.open("w", encoding="utf-8", newline="") as stream:
+        writer = csv.writer(stream)
+        writer.writerow(
+            ["t_s", "joint_name", "position_u8", "angle_deg", "velocity_deg_s"]
+        )
+        for joint_name, arrays in processed.items():
+            for row in zip(*arrays):
+                writer.writerow(
+                    [
+                        f"{row[0]:.9f}",
+                        joint_name,
+                        f"{row[1]:.6f}",
+                        f"{row[2]:.6f}",
+                        f"{row[3]:.6f}",
+                    ]
+                )
+
+    figure, axes = plt.subplots(2, 1, sharex=True, figsize=(12, 8))
+    for joint_name, (time_s, _position_u8, angle_deg, velocity_deg_s) in processed.items():
+        axes[0].plot(time_s, angle_deg, label=joint_name)
+        axes[1].plot(time_s, velocity_deg_s, label=joint_name)
+
+    axes[0].set_ylabel("Angle (deg)")
+    axes[0].set_title(f"O6 {hand_type} joint curves\nangle map: {mapping_source}")
+    axes[1].set_ylabel("Velocity (deg/s)")
+    axes[1].set_xlabel("Time (s)")
+    axes[1].set_title(f"Derived velocity, first-order low-pass {cutoff_hz:g} Hz")
+    for axis in axes:
+        axis.grid(True, alpha=0.3)
+        axis.legend(loc="best", fontsize="small")
+    figure.tight_layout()
+    figure.savefig(plot_path, dpi=160)
+    plt.close(figure)
+    return csv_path, plot_path
+
+
+def _parse_args(argv):
+    parser = argparse.ArgumentParser(
+        description="Record O6 angle and angle-derived velocity curves"
+    )
+    parser.add_argument("--hand-type", choices=("left", "right"), default="right")
+    parser.add_argument(
+        "--duration",
+        type=float,
+        default=10.0,
+        help="recording seconds; <= 0 records until Ctrl+C (default: 10)",
+    )
+    parser.add_argument(
+        "--output-dir", default="logs/O6/curves", help="CSV/PNG output directory"
+    )
+    parser.add_argument(
+        "--joint",
+        default="all",
+        choices=("all",) + O6_JOINT_NAMES,
+        help="plot one joint or all six (default: all)",
+    )
+    parser.add_argument(
+        "--mapping-json",
+        help="optional hand_<SN>.json containing joints.<name>.joint_u8 and joint_real_deg",
+    )
+    parser.add_argument(
+        "--filter-hz",
+        type=float,
+        default=5.0,
+        help="velocity low-pass cutoff; <= 0 disables filtering (default: 5)",
+    )
+    parsed = parser.parse_args(remove_ros_args(args=argv)[1:])
+    if parsed.duration == 0.0:
+        parsed.duration = -1.0
+    return parsed
+
+
+def main(args=None):
+    argv = sys.argv if args is None else [sys.argv[0], *args]
+    parsed = _parse_args(argv)
+    rclpy.init(args=args)
+    node = O6JointCurveRecorder(
+        hand_type=parsed.hand_type,
+        duration=parsed.duration,
+        output_dir=parsed.output_dir,
+        joint_name=parsed.joint,
+        mapping_json=parsed.mapping_json,
+        filter_hz=parsed.filter_hz,
+    )
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("Stopping on Ctrl+C")
+    finally:
+        node.finish()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/linker_hand_ros2_sdk/package.xml

@@ -0,0 +1,24 @@
+<?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>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-matplotlib</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/linker_hand_ros2_sdk/pyproject.toml

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

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

src/linker_hand_ros2_sdk/setup.py

@@ -0,0 +1,55 @@
+#!/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}.*"]),
+    package_data={
+        package_name: ['LinkerHand/config/*.yaml'],
+    },
+    include_package_data=True,
+    data_files=data_files,
+    install_requires=['setuptools'],
+    zip_safe=False,
+    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',
+            'o6_joint_curve = linker_hand_ros2_sdk.o6_joint_curve: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',
+        ],
+    },
+)

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'

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)

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'

src/linker_hand_ros2_sdk/test/test_topic_prefix.py

@@ -0,0 +1,36 @@
+from rclpy.qos import HistoryPolicy
+
+from linker_hand_ros2_sdk.linker_hand import (
+    COMMAND_QOS,
+    LinkerHand,
+    command_changed,
+    state_poll_due,
+)
+
+
+def test_empty_topic_prefix_preserves_legacy_topics():
+    assert LinkerHand.normalize_topic_prefix("") == ""
+    assert LinkerHand.normalize_topic_prefix("/") == ""
+
+
+def test_topic_prefix_is_absolute_and_has_no_trailing_slash():
+    assert LinkerHand.normalize_topic_prefix("o6") == "/o6"
+    assert LinkerHand.normalize_topic_prefix("/g20/") == "/g20"
+
+
+def test_command_qos_keeps_only_the_latest_sample():
+    assert COMMAND_QOS.history == HistoryPolicy.KEEP_LAST
+    assert COMMAND_QOS.depth == 1
+
+
+def test_identical_commands_are_not_reapplied():
+    assert command_changed(None, [60, 60])
+    assert not command_changed([60, 60], [60, 60])
+    assert command_changed([60, 60], [60, 61])
+    assert not command_changed([60, 60], [])
+
+
+def test_state_polling_is_throttled_without_missing_deadline():
+    assert state_poll_due(None, 10.0, 0.1)
+    assert not state_poll_due(10.0, 10.09, 0.1)
+    assert state_poll_due(10.0, 10.1, 0.1)

src/officer_sdk/lbot_arm_interfaces/CMakeLists.txt

@@ -0,0 +1,72 @@
+cmake_minimum_required(VERSION 3.5)
+project(lbot_arm_interfaces)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+    set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+    # srv
+    "srv/MoveJ.srv"
+    "srv/MoveL.srv"
+    "srv/MoveC.srv"
+    "srv/MoveJP.srv"
+    "srv/InverseKinematics.srv"
+    "srv/ForwardKinematics.srv"
+    "srv/SetFrame.srv"
+    "srv/SetString.srv"
+    "srv/GetFrame.srv"
+    "srv/GetCurrentFrame.srv"
+    "srv/ChangeFrame.srv"
+    "srv/DeleteFrame.srv"
+    "srv/GetAllFrames.srv"
+    "srv/SetZero.srv"
+    "srv/SetEmergency.srv"
+    "srv/SetEnable.srv"
+    # msg
+    "msg/ArmState.msg"
+    "msg/LbotPose.msg"
+    "msg/LbotFrame.msg"
+    "msg/FollowJoint.msg"
+    "msg/SystemError.msg"
+    
+    DEPENDENCIES std_msgs geometry_msgs sensor_msgs
+ )
+ 
+ ament_export_dependencies(rosidl_default_runtime)
+
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    # the following line skips the linter which checks for copyrights
+    # uncomment the line when a copyright and license is not present in all source files
+    #set(ament_cmake_copyright_FOUND TRUE)
+    # the following line skips cpplint (only works in a git repo)
+    # uncomment the line when this package is not in a git repo
+    #set(ament_cmake_cpplint_FOUND TRUE)
+    ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/officer_sdk/lbot_arm_interfaces/msg/ArmState.msg

@@ -0,0 +1,3 @@
+float32[] joints
+geometry_msgs/Vector3 euler
+geometry_msgs/Pose pose

src/officer_sdk/lbot_arm_interfaces/msg/FollowJoint.msg

@@ -0,0 +1,2 @@
+float32[] joints
+bool follow

src/officer_sdk/lbot_arm_interfaces/msg/LbotFrame.msg

@@ -0,0 +1,3 @@
+string name
+geometry_msgs/Vector3 euler
+geometry_msgs/Vector3 position

src/officer_sdk/lbot_arm_interfaces/msg/LbotPose.msg

@@ -0,0 +1,2 @@
+geometry_msgs/Vector3 euler
+geometry_msgs/Vector3 position

src/officer_sdk/lbot_arm_interfaces/msg/SystemError.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+int32 error_code
+string error_msg
+bool connected

src/officer_sdk/lbot_arm_interfaces/package.xml

@@ -0,0 +1,25 @@
+<?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>lbot_arm_interfaces</name>
+    <version>0.0.0</version>
+    <description>TODO: Package description</description>
+    <maintainer email="mengfanjiwork@163.com">Ross</maintainer>
+    <license>TODO: License declaration</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
+
+    <build_depend>rosidl_default_generators</build_depend>
+    <exec_depend>rosidl_default_runtime</exec_depend>
+    <member_of_group>rosidl_interface_packages</member_of_group>
+
+    <test_depend>ament_lint_auto</test_depend>
+    <test_depend>ament_lint_common</test_depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+    <depend>std_msgs</depend>
+    <depend>geometry_msgs</depend>
+    <depend>sensor_msgs</depend>
+</package>

src/officer_sdk/lbot_arm_interfaces/srv/ChangeFrame.srv

@@ -0,0 +1,3 @@
+string name
+---
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/DeleteFrame.srv

@@ -0,0 +1,3 @@
+string name
+---
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/ForwardKinematics.srv

@@ -0,0 +1,7 @@
+float32[] joints
+
+---
+geometry_msgs/Vector3 position
+geometry_msgs/Vector3 euler
+
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/GetAllFrames.srv

@@ -0,0 +1,4 @@
+
+---
+string[] names
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/GetCurrentFrame.srv

@@ -0,0 +1,5 @@
+---
+string name
+lbot_arm_interfaces/LbotFrame frame
+bool success
+

src/officer_sdk/lbot_arm_interfaces/srv/GetFrame.srv

@@ -0,0 +1,4 @@
+string name
+---
+lbot_arm_interfaces/LbotFrame frame
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/InverseKinematics.srv

@@ -0,0 +1,6 @@
+float32[] joints                      # 此关节角度不设置会默认从机械臂读取当前角度，如果设置则基于此值为初始角度进行逆解
+geometry_msgs/Vector3 position
+geometry_msgs/Vector3 euler
+---
+float32[] joints
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/MoveC.srv

@@ -0,0 +1,9 @@
+geometry_msgs/Vector3 position
+geometry_msgs/Vector3 euler
+float32 speed
+float32 acce
+bool block
+
+---
+
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/MoveJ.srv

@@ -0,0 +1,7 @@
+float32[] joints
+float32 speed
+float32 acce
+bool block
+
+---
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/MoveJP.srv

@@ -0,0 +1,9 @@
+geometry_msgs/Vector3 position
+geometry_msgs/Vector3 euler
+float32 speed
+float32 acce
+bool block
+
+---
+
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/MoveL.srv

@@ -0,0 +1,9 @@
+geometry_msgs/Vector3 position
+geometry_msgs/Vector3 euler
+float32 speed
+float32 acce
+bool block
+
+---
+
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/SetEmergency.srv

@@ -0,0 +1,5 @@
+bool emergency
+
+---
+
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/SetEnable.srv

@@ -0,0 +1,5 @@
+bool enable
+
+---
+
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/SetFrame.srv

@@ -0,0 +1,3 @@
+lbot_arm_interfaces/LbotFrame frame
+---
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/SetString.srv

@@ -0,0 +1,3 @@
+string name
+---
+bool success

src/officer_sdk/lbot_arm_interfaces/srv/SetZero.srv

@@ -0,0 +1,3 @@
+
+---
+bool success

src/officer_sdk/lbot_driver/CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 3.8)
+project(lbot_driver)
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+add_compile_options(-Wall -Wextra -Wpedantic)
+
+# ROS2 dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(lbot_arm_interfaces REQUIRED)
+
+# include directories
+include_directories(
+  ${PROJECT_SOURCE_DIR}/include
+  ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}
+)
+
+# library directory
+link_directories(${PROJECT_SOURCE_DIR}/lib)
+
+# executable
+add_executable(lbot_driver src/lbot_driver.cpp)
+
+# link the API library (name only! no lib prefix, no .so suffix)
+target_link_libraries(lbot_driver
+  lbot_api_cpp
+)
+
+# ros deps
+ament_target_dependencies(lbot_driver
+  rclcpp
+  std_msgs
+  std_srvs
+  sensor_msgs
+  geometry_msgs
+  tf2
+  tf2_geometry_msgs
+  lbot_arm_interfaces
+)
+
+# install binary
+install(TARGETS lbot_driver
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# install launch & config
+install(DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# install .so libraries
+install(DIRECTORY lib/
+  DESTINATION lib
+)
+
+ament_package()

src/officer_sdk/lbot_driver/config/lbot_config.yaml

@@ -0,0 +1,4 @@
+lbot_driver:
+  ros__parameters:
+    #robot param
+    arm_ip: "192.168.10.21"        #设置TCP连接时的IP

src/officer_sdk/lbot_driver/include/lbot_driver/lbot_api.h

@@ -0,0 +1,485 @@
+/**
+ * @file lbot_api.h
+ * @brief LBot机器人控制API接口
+ * @date 2026.1.19
+ * @copyright 灵心巧手科技有限公司
+ */
+#ifndef LBOT_API_H
+#define LBOT_API_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// 跨平台导出宏
+#ifdef _WIN32
+    #ifdef LBOT_API_EXPORTS
+        #define LBOT_API __declspec(dllexport)
+    #else
+        #define LBOT_API __declspec(dllimport)
+    #endif
+#else
+    #define LBOT_API __attribute__((visibility("default")))
+#endif
+
+#include "lbot_types.h"
+#include "lbot_version.h"
+
+// ==============================================
+// API初始化和清理函数
+// ==============================================
+/**
+ * @brief 初始化LBot API连接
+ * @param tcp_host TCP服务器地址格式："192.168.10.21"
+ * @return lbot_handle_t 连接句柄，连接成功句柄ID>0，失败返回NULL
+ */
+LBOT_API lbot_handle_t *lbot_init(const char* tcp_host);
+
+/**
+ * @brief 断开指定连接
+ * @param handle 机械臂句柄
+ * @return true 断开成功，false 断开失败
+ */
+LBOT_API bool lbot_disconnect(lbot_handle_t *handle);
+
+/**
+ * @brief 清理API资源，断开连接
+ */
+LBOT_API void lbot_cleanup();
+
+/**
+ * @brief 获取API版本信息
+ * @return 版本字符串
+ */
+LBOT_API const char* lbot_get_api_version();
+// ==============================================
+// 系统信息获取函数
+// ==============================================
+/**
+ * @brief 获取控制器信息
+ * @param handle 机械臂句柄
+ * @param robot_model 返回的机器人型号字符串（需要调用者释放）
+ * @param controller_version 返回的控制器版本字符串（需要调用者释放）
+ * @return true 获取成功，false 获取失败
+ */
+LBOT_API bool lbot_get_controller_info(lbot_handle_t *handle, char** robot_model, char** controller_version);
+
+// ==============================================
+// 状态监控和管理函数
+// ==============================================
+/**
+ * @brief 启动状态监控
+ * @param state_cb 状态回调函数，当机器人状态更新时调用
+ * @param error_cb 错误回调函数，当发生错误时调用
+ * @return true 启动成功，false 启动失败
+ */
+LBOT_API bool lbot_start_state_monitor(lbot_state_callback_t state_cb, lbot_error_callback_t error_cb);
+
+/**
+ * @brief 停止状态监控
+ */
+LBOT_API void lbot_stop_state_monitor();
+
+/**
+ * @brief 获取当前机器人完整状态，此接口需要在启动状态监控后才能调用
+ * @param handle 机械臂句柄
+ * @param state 返回的机器人状态结构体指针
+ * @return true 获取成功，false 获取失败
+ */
+LBOT_API bool lbot_get_current_state(lbot_handle_t *handle, lbot_full_state_t* state);
+
+// ==============================================
+// 运动控制函数
+// ==============================================
+/**
+ * @brief 关节空间运动
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param joints 7个关节的目标角度（弧度）
+ * @param speed 运动速度（0.0~20.0）单位是rad/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+ * @param accel 加速度（0.0~20.0） 单位是rad/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+ * @param block 是否阻塞执行：true 等待运动完成，false 立即返回
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_move_joint(lbot_handle_t *handle, lbot_arm_t arm, const double joints[7], double speed, double accel, bool block);
+
+/**
+ * @brief 笛卡尔空间姿态运动（关节插值）
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param position 目标位置（x, y, z，单位：米）
+ * @param euler 机械臂末端目标欧拉角（roll, pitch, yaw，单位：弧度）
+ * @param speed 机械臂末端运动速度（0.0~20.0）单位m/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+ * @param accel 机械臂末端加速度（0.0~20.0）单位m/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+ * @param block 是否阻塞执行：true 等待运动完成，false 立即返回
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_move_pose(lbot_handle_t *handle, lbot_arm_t arm, const lbot_position_t* position, const lbot_euler_t* euler, 
+                   double speed, double accel, bool block);
+
+/**
+ * @brief 笛卡尔空间直线运动（直线插值）
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param position 目标位置（x, y, z，单位：米）
+ * @param euler 目标欧拉角（roll, pitch, yaw，单位：弧度）
+ * @param speed 关节运动速度（0.0~20.0）单位是rad/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+ * @param accel 关节运动加速度（0.0~20.0） 单位是rad/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+ * @param block 是否阻塞执行：true 等待运动完成，false 立即返回
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_move_linear(lbot_handle_t *handle, lbot_arm_t arm, const lbot_position_t* position, const lbot_euler_t* euler, 
+                     double speed, double accel, bool block);
+
+// ==============================================
+// 关节跟随函数（用于遥操作）
+// ==============================================
+/**
+ * @brief 关节跟随控制
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param joints 7个关节的目标角度（弧度）
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_joint_follow(lbot_handle_t *handle, lbot_arm_t arm, const double joints[7]);
+
+/**
+ * @brief 笛卡尔空间姿态跟随运动
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param pos 目标位置（x, y, z，单位：米）
+ * @param eul 目标欧拉角（roll, pitch, yaw，单位：弧度）
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_pose_follow(lbot_handle_t *handle, lbot_arm_t arm, lbot_position_t pos, lbot_euler_t eul);
+// ==============================================
+// l6 手控制接口
+// ==============================================
+/**
+ * @brief 设置L6手的位置控制
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param position 6个手指的目标位置（0~255）
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_l6_set_position(lbot_handle_t *handle, lbot_arm_t arm, const uint8_t position[6]);
+
+/**
+ * @brief 设置L6手的速度控制
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param velocity 6个手指的目标速度（0~255）
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_l6_set_velocity(lbot_handle_t *handle, lbot_arm_t arm, const uint8_t velocity[6]);
+
+/**
+ * @brief 设置L6手的力矩控制
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param torque 6个手指的目标力矩（0~255）
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_l6_set_effort(lbot_handle_t *handle, lbot_arm_t arm, const uint8_t torque[6]);
+
+// ==============================================
+// l10 手控制接口（10个自由度）
+// ==============================================
+/**
+ * @brief 设置L10手的位置控制
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param position 10个手指的目标位置（0~255）
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_l10_set_position(lbot_handle_t *handle, lbot_arm_t arm, const uint8_t position[10]);
+
+/**
+ * @brief 设置L10手的速度控制
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param velocity 10个手指的目标速度（0~255）
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_l10_set_velocity(lbot_handle_t *handle, lbot_arm_t arm, const uint8_t velocity[10]);
+
+/**
+ * @brief 设置L10手的力矩控制
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param torque 10个手指的目标力矩（0~255）
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_l10_set_effort(lbot_handle_t *handle, lbot_arm_t arm, const uint8_t torque[10]);
+
+// ==============================================
+// r20 手控制接口
+// ==============================================
+/**
+ * @brief 设置R20手各手指位置控制
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param cmd 目标位置命令
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_l20_set_series_position(lbot_handle_t *handle, lbot_arm_t arm, const lbot_l20_series_cmd_t* cmd);
+/**
+ * @brief 设置R20手所有自由度位置控制
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param cmd 目标位置命令, 16个自由度的目标位置(度)，分别为：拇指指根（0~120）、
+ *      拇指指尖（0~150）、拇指侧摆（0~180）、拇指旋转（0~130）、食指侧摆（-30~30）、
+ *      食指指根（0~180）、食指指尖（0~180）、中指侧摆（-30~30）、中指指根（0~180）、
+ *      中指指尖（0~180）、无名指侧摆（-20~20）、无名指指根（0~180）、无名指指尖（0~180）、
+ *      小指侧摆（-20~20）、小指指根（0~180）、小指指尖（0~180）
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_l20_set_all_position(lbot_handle_t *handle, lbot_arm_t arm, const int cmd[16]);
+
+
+// ==============================================
+// 运动学计算函数
+// ==============================================
+/**
+ * @brief 正运动学计算
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param joints 7个关节角度（弧度）
+ * @param position 返回的末端位置（x, y, z，单位：米）
+ * @param euler 返回的末端欧拉角（roll, pitch, yaw，单位：弧度）
+ * @return true 计算成功，false 计算失败
+ */
+LBOT_API bool lbot_forward_kinematics(lbot_handle_t *handle, lbot_arm_t arm, const double joints[7], 
+                            lbot_position_t* position, lbot_euler_t* euler);
+
+/**
+ * @brief 逆运动学计算
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param initial_joints 初始关节角度（弧度），用于求解器迭代
+ * @param position 目标位置（x, y, z，单位：米）
+ * @param euler 目标欧拉角（roll, pitch, yaw，单位：弧度）
+ * @param result_joints 返回的7个关节角度解（弧度）
+ * @return true 求解成功，false 求解失败
+ */
+LBOT_API bool lbot_inverse_kinematics(lbot_handle_t *handle, lbot_arm_t arm, const double initial_joints[7], 
+                            const lbot_position_t* position, const lbot_euler_t* euler, 
+                            double result_joints[7]);
+
+// ==============================================
+// 工具坐标系管理函数
+// ==============================================
+/**
+ * @brief 设置工具坐标系
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param name 工具坐标系名称（最大32字符）
+ * @param position 工具坐标系相对于法兰盘的位置偏移（x, y, z，单位：米）
+ * @param euler 工具坐标系相对于法兰盘的欧拉角偏移（roll, pitch, yaw，单位：弧度）
+ * @return true 设置成功，false 设置失败
+ */
+LBOT_API bool lbot_set_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, 
+                        const lbot_position_t* position, const lbot_euler_t* euler);
+
+/**
+ * @brief 获取工具坐标系参数
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param name 工具坐标系名称
+ * @param position 返回的工具坐标系位置偏移
+ * @param euler 返回的工具坐标系欧拉角偏移
+ * @return true 获取成功，false 获取失败
+ */
+LBOT_API bool lbot_get_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, 
+                        lbot_position_t* position, lbot_euler_t* euler);
+
+/**
+ * @brief 获取当前使用的工具坐标系
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param name 返回的当前工具坐标系名称（需要调用者释放）
+ * @param position 返回的当前工具坐标系位置偏移
+ * @param euler 返回的当前工具坐标系欧拉角偏移
+ * @return true 获取成功，false 获取失败
+ */
+LBOT_API bool lbot_get_current_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, 
+                                char** name, 
+                                lbot_position_t* position, 
+                                lbot_euler_t* euler);
+
+/**
+ * @brief 切换当前工具坐标系
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param name 要切换到的工具坐标系名称
+ * @return true 切换成功，false 切换失败
+ */
+LBOT_API bool lbot_change_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name);
+
+/**
+ * @brief 删除工具坐标系
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param name 要删除的工具坐标系名称
+ * @return true 删除成功，false 删除失败
+ */
+LBOT_API bool lbot_delete_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name);
+ 
+/**
+ * @brief 获取所有工具坐标系名称
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param names 返回的工具坐标系名称数组（需要调用lbot_free_string_array释放）
+ * @param count 返回的工具坐标系数量
+ * @return true 获取成功，false 获取失败
+ */
+LBOT_API bool lbot_get_all_tool_frames(lbot_handle_t *handle, lbot_arm_t arm, char*** names, int* count);
+
+// ==============================================
+// 工作坐标系管理函数
+// ==============================================
+/**
+ * @brief 设置工作坐标系
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param name 工作坐标系名称（最大32字符）
+ * @param position 工作坐标系相对于基坐标系的位置偏移（x, y, z，单位：米）
+ * @param euler 工作坐标系相对于基坐标系的欧拉角偏移（roll, pitch, yaw，单位：弧度）
+ * @return true 设置成功，false 设置失败
+ */
+LBOT_API bool lbot_set_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, 
+                        const lbot_position_t* position, const lbot_euler_t* euler);
+
+/**
+ * @brief 获取工作坐标系参数
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param name 工作坐标系名称
+ * @param position 返回的工作坐标系位置偏移
+ * @param euler 返回的工作坐标系欧拉角偏移
+ * @return true 获取成功，false 获取失败
+ */
+LBOT_API bool lbot_get_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, 
+                        lbot_position_t* position, lbot_euler_t* euler);
+
+/**
+ * @brief 切换当前工作坐标系
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param name 要切换到的工作坐标系名称
+ * @return true 切换成功，false 切换失败
+ */
+LBOT_API bool lbot_change_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name);
+
+/**
+ * @brief 删除工作坐标系
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param name 要删除的工作坐标系名称
+ * @return true 删除成功，false 删除失败
+ */
+LBOT_API bool lbot_delete_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name);
+
+/**
+ * @brief 获取所有工作坐标系名称
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param names 返回的工作坐标系名称数组（需要调用lbot_free_string_array释放）
+ * @param count 返回的工作坐标系数量
+ * @return true 获取成功，false 获取失败
+ */
+LBOT_API bool lbot_get_all_work_frames(lbot_handle_t *handle, lbot_arm_t arm, char*** names, int* count);
+
+// ==============================================
+// 系统功能函数
+// ==============================================
+/**
+ * @brief 重新标定电机零位，设置当前位置为零位
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @return true 设置成功，false 设置失败
+ */
+LBOT_API bool lbot_set_zero(lbot_handle_t *handle, lbot_arm_t arm);
+
+/**
+ * @brief 使能/掉使能机械臂
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param enable true 使能，false 掉使能
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_enable_arm(lbot_handle_t *handle, lbot_arm_t arm, bool enable);
+
+/**
+ * @brief 紧急停止/恢复机械臂运行
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param enable true 紧急停止，false 恢复运行
+ * @return true 指令发送成功，false 发送失败
+ */
+LBOT_API bool lbot_emergency_stop(lbot_handle_t *handle, lbot_arm_t arm, bool enable);
+
+/**
+ * @brief 清除所有错误
+ * @param handle 机械臂句柄
+ * @return true 清除成功，false 清除失败
+ */
+LBOT_API bool lbot_clear_errors(lbot_handle_t *handle);
+
+/**
+ * @brief 设置机械臂关节限位
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param upper_joint_limit 最大关节限位参数
+ * @param lower_joint_limit 最小关节限位参数
+ * @return true 设置成功，false 设置失败
+ */
+LBOT_API bool lbot_set_joint_limit(lbot_handle_t *handle, lbot_arm_t arm, const double upper_joint_limit[7], const double lower_joint_limit[7]);
+/**
+ * @brief 获取机械臂关节限位参数
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param upper_joint_limit 返回的最大关节限位参数
+ * @param lower_joint_limit 返回的最小关节限位参数
+ * @return true 获取成功，false 获取失败
+ */
+LBOT_API bool lbot_get_joint_limit(lbot_handle_t *handle, lbot_arm_t arm, double upper_joint_limit[7], double lower_joint_limit[7]);
+/**
+ * @brief 恢复默认关节限位参数
+ * @param handle 机械臂句柄
+ * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+ * @param upper_joint_limit 返回的默认最大关节限位参数
+ * @param lower_joint_limit 返回的默认最小关节限位参数
+ * @return true 恢复成功，false 恢复失败
+ */
+LBOT_API bool lbot_get_default_joint_limit(lbot_handle_t *handle, lbot_arm_t arm, double upper_joint_limit[7], double lower_joint_limit[7]);
+// ==============================================
+// 内存管理辅助函数
+// ==============================================
+/**
+ * @brief 释放字符串数组内存
+ * @param array 要释放的字符串数组
+ * @param count 数组元素数量
+ */
+LBOT_API void lbot_free_string_array(char** array, int count);
+
+// ==============================================
+// 工具函数
+// ==============================================
+/**
+ * @brief 获取最后一次错误信息
+ * @return 错误信息字符串指针
+ */
+LBOT_API const char* lbot_get_last_error(lbot_handle_t *handle);
+
+/**
+ * @brief 设置日志级别
+ * @param level 日志级别：0-ERROR, 1-WARN, 2-INFO, 3-DEBUG
+ */
+LBOT_API void lbot_set_log_level(int level);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LBOT_API_H

src/officer_sdk/lbot_driver/include/lbot_driver/lbot_api_cpp.h

@@ -0,0 +1,660 @@
+/**
+ * @file lbot_api_cpp.h
+ * @brief 灵心巧手机械臂控制API C++封装头文件
+ * @date 2026.1.19
+ * @copyright 灵心巧手科技有限公司
+ */
+#ifndef LBOT_API_CPP_H
+#define LBOT_API_CPP_H
+
+#include "lbot_api.h"
+#include <string>
+#include <vector>
+#include <functional>
+
+/**
+ * @brief 灵心巧手机械臂控制API C++封装类
+ * @details 提供C++友好的接口封装，使用std::string和std::vector简化内存管理
+ */
+namespace lbot {
+
+class LbotApi {
+public:
+    // ==============================================
+    // 构造函数和析构函数
+    // ==============================================
+    /**
+     * @brief 构造函数
+     */
+    LbotApi();
+    
+    /**
+     * @brief 析构函数，自动清理资源
+     */
+    ~LbotApi();
+
+    // ==============================================
+    // API初始化和清理函数
+    // ==============================================
+    /**
+     * @brief 初始化LBot API连接
+     * @param tcp_host TCP服务器地址格式："192.168.10.21"
+     * @return lbot_handle_t 连接句柄，连接成功句柄ID>0，失败返回NULL
+     */
+    LBOT_API lbot_handle_t *lbot_init(const char* tcp_host);
+        
+    /**
+     * @brief 断开指定连接
+     * @param handle 机械臂句柄
+     * @return true 断开成功，false 断开失败
+     */
+    LBOT_API bool lbot_disconnect(lbot_handle_t *handle);
+
+    /**
+     * @brief 清理API资源，断开连接
+     */
+    LBOT_API void lbot_cleanup();
+
+    /**
+     * @brief 获取API版本信息
+     * @return 版本字符串
+     */
+    LBOT_API std::string lbot_get_api_version();
+
+    // ==============================================
+    // 系统信息获取函数
+    // ==============================================
+    /**
+     * @brief 获取控制器信息
+     * @param handle 机械臂句柄
+     * @param robot_model 返回的机器人型号字符串
+     * @param controller_version 返回的控制器版本字符串
+     * @return true 获取成功，false 获取失败
+     */
+    LBOT_API bool lbot_get_controller_info(lbot_handle_t *handle, std::string& robot_model, std::string& controller_version);
+    
+    // ==============================================
+    // 状态监控和管理函数
+    // ==============================================
+    /**
+     * @brief 启动状态监控
+     * @param state_cb 状态回调函数，当机器人状态更新时调用
+     * @param error_cb 错误回调函数，当发生错误时调用
+     * @return true 启动成功，false 启动失败
+     */
+    LBOT_API bool lbot_start_state_monitor(lbot_state_callback_t state_cb, lbot_error_callback_t error_cb);
+    
+    /**
+     * @brief 停止状态监控
+     */
+    LBOT_API void lbot_stop_state_monitor();
+    
+    /**
+     * @brief 获取当前机器人完整状态，此接口需要在启动状态监控后才能调用
+     * @param handle 机械臂句柄
+     * @param state 返回的机器人状态结构体指针
+     * @return true 获取成功，false 获取失败
+     */
+    LBOT_API bool lbot_get_current_state(lbot_handle_t *handle, lbot_full_state_t* state);
+
+    // ==============================================
+    // 运动控制函数
+    // ==============================================
+    /**
+     * @brief 关节空间运动
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param joints 7个关节的目标角度（弧度）
+     * @param speed 运动速度（0.0~20.0）单位是rad/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+     * @param accel 加速度（0.0~20.0）单位是rad/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+     * @param block 是否阻塞执行：true 等待运动完成，false 立即返回
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_move_joint(lbot_handle_t *handle, lbot_arm_t arm, const double joints[7], double speed, double accel, bool block);
+    
+    /**
+     * @brief 关节空间运动（使用vector）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param joints 7个关节的目标角度（弧度）向量
+     * @param speed 运动速度（0.0~20.0）单位是rad/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+     * @param accel 加速度（0.0~20.0）单位是rad/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+     * @param block 是否阻塞执行：true 等待运动完成，false 立即返回
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_move_joint(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<double>& joints, double speed, double accel, bool block);
+    
+    /**
+     * @brief 笛卡尔空间姿态运动（关节插值）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param position 目标位置（x, y, z，单位：米）
+     * @param euler 机械臂末端目标欧拉角（roll, pitch, yaw，单位：弧度）
+     * @param speed 机械臂末端运动速度（0.0~20.0）单位m/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+     * @param accel 机械臂末端加速度（0.0~20.0）单位m/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+     * @param block 是否阻塞执行：true 等待运动完成，false 立即返回
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_move_pose(lbot_handle_t *handle, lbot_arm_t arm, const lbot_position_t* position, const lbot_euler_t* euler, 
+                       double speed, double accel, bool block);
+                       
+    /**
+     * @brief 笛卡尔空间姿态运动（使用向量）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param position 目标位置（x, y, z，单位：米）向量
+     * @param euler 机械臂末端目标欧拉角（roll, pitch, yaw，单位：弧度）向量
+     * @param speed 机械臂末端运动速度（0.0~20.0）单位m/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+     * @param accel 机械臂末端加速度（0.0~20.0）单位m/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+     * @param block 是否阻塞执行：true 等待运动完成，false 立即返回
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_move_pose(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<double>& position, const std::vector<double>& euler,
+                       double speed, double accel, bool block);
+    
+    /**
+     * @brief 笛卡尔空间直线运动（直线插值）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param position 目标位置（x, y, z，单位：米）
+     * @param euler 目标欧拉角（roll, pitch, yaw，单位：弧度）
+     * @param speed 关节运动速度（0.0~20.0）单位是rad/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+     * @param accel 关节运动加速度（0.0~20.0）单位是rad/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+     * @param block 是否阻塞执行：true 等待运动完成，false 立即返回
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_move_linear(lbot_handle_t *handle, lbot_arm_t arm, const lbot_position_t* position, const lbot_euler_t* euler, 
+                         double speed, double accel, bool block);
+                         
+    /**
+     * @brief 笛卡尔空间直线运动（使用向量）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param position 目标位置（x, y, z，单位：米）向量
+     * @param euler 目标欧拉角（roll, pitch, yaw，单位：弧度）向量
+     * @param speed 关节运动速度（0.0~20.0）单位是rad/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+     * @param accel 关节运动加速度（0.0~20.0）单位是rad/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高
+     * @param block 是否阻塞执行：true 等待运动完成，false 立即返回
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_move_linear(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<double>& position, const std::vector<double>& euler,
+                         double speed, double accel, bool block);
+
+    // ==============================================
+    // 关节跟随函数（用于遥操作）
+    // ==============================================
+    /**
+     * @brief 关节跟随控制
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param joints 包含7个关节目标角度（弧度）的向量
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_joint_follow(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<double>& joints);
+        
+    // ==============================================
+    // 姿态跟随函数 
+    // ==============================================
+    /**
+     * @brief 笛卡尔空间姿态跟随运动
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param pos 目标位置（x, y, z，单位：米）
+     * @param eul 目标欧拉角（roll, pitch, yaw，单位：弧度）
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_pose_follow(lbot_handle_t *handle, lbot_arm_t arm, lbot_position_t pos, lbot_euler_t eul);
+    
+    // ==============================================
+    // L6手控制接口
+    // ==============================================
+    /**
+     * @brief 设置L6手的位置控制
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param position 6个手指的目标位置（0~255）向量
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_l6_set_position(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<uint8_t>& position);
+    
+    /**
+     * @brief 设置L6手的速度控制
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param velocity 6个手指的目标速度（0~255）向量
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_l6_set_velocity(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<uint8_t>& velocity);
+    
+    /**
+     * @brief 设置L6手的力矩控制
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param torque 6个手指的目标力矩（0~255）向量
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_l6_set_effort(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<uint8_t>& torque);
+    
+    // ==============================================
+    // L10手控制接口（10个自由度）
+    // ==============================================
+    /**
+     * @brief 设置L10手的位置控制
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param position 10个手指的目标位置（0~255）向量
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_l10_set_position(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<uint8_t>& position);
+    
+    /**
+     * @brief 设置L10手的速度控制
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param velocity 10个手指的目标速度（0~255）向量
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_l10_set_velocity(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<uint8_t>& velocity);
+    
+    /**
+     * @brief 设置L10手的力矩控制
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param torque 10个手指的目标力矩（0~255）向量
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_l10_set_effort(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<uint8_t>& torque);
+
+    // ==============================================
+    // L20手控制接口（20个自由度）
+    // ==============================================
+    /**
+     * @brief 设置R20手的串联位置控制
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param cmd 目标位置命令，包含手指标识和6个电机位置
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_l20_set_series_position(lbot_handle_t *handle, lbot_arm_t arm, const lbot_l20_series_cmd_t* cmd);
+    
+    /**
+     * @brief 设置R20手所有自由度位置控制
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param position 16个自由度的目标位置（度）向量
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_l20_set_all_position(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<int>& position);
+    
+    // ==============================================
+    // 运动学计算函数
+    // ==============================================
+    /**
+     * @brief 正运动学计算
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param joints 7个关节角度（弧度）
+     * @param position 返回的末端位置（x, y, z，单位：米）
+     * @param euler 返回的末端欧拉角（roll, pitch, yaw，单位：弧度）
+     * @return true 计算成功，false 计算失败
+     */
+    LBOT_API bool lbot_forward_kinematics(lbot_handle_t *handle, lbot_arm_t arm, const double joints[7], 
+                                lbot_position_t* position, lbot_euler_t* euler);
+    
+    /**
+     * @brief 正运动学计算（使用向量）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param joints 7个关节角度（弧度）向量
+     * @param position 返回的末端位置（x, y, z，单位：米）向量
+     * @param euler 返回的末端欧拉角（roll, pitch, yaw，单位：弧度）向量
+     * @return true 计算成功，false 计算失败
+     */
+    LBOT_API bool lbot_forward_kinematics(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<double>& joints,
+                                std::vector<double>& position, std::vector<double>& euler);
+    
+    /**
+     * @brief 逆运动学计算
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param initial_joints 初始关节角度（弧度），用于求解器迭代
+     * @param position 目标位置（x, y, z，单位：米）
+     * @param euler 目标欧拉角（roll, pitch, yaw，单位：弧度）
+     * @param result_joints 返回的7个关节角度解（弧度）
+     * @return true 求解成功，false 求解失败
+     */
+    LBOT_API bool lbot_inverse_kinematics(lbot_handle_t *handle, lbot_arm_t arm, const double initial_joints[7], 
+                                const lbot_position_t* position, const lbot_euler_t* euler, 
+                                double result_joints[7]);
+                                
+    /**
+     * @brief 逆运动学计算（使用向量）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param initial_joints 初始关节角度（弧度）向量，用于求解器迭代
+     * @param position 目标位置（x, y, z，单位：米）向量
+     * @param euler 目标欧拉角（roll, pitch, yaw，单位：弧度）向量
+     * @param result_joints 返回的7个关节角度解（弧度）向量
+     * @return true 求解成功，false 求解失败
+     */
+    LBOT_API bool lbot_inverse_kinematics(lbot_handle_t *handle, lbot_arm_t arm, const std::vector<double>& initial_joints,
+                                const std::vector<double>& position, const std::vector<double>& euler,
+                                std::vector<double>& result_joints);
+
+    // ==============================================
+    // 工具坐标系管理函数
+    // ==============================================
+    /**
+     * @brief 设置工具坐标系
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 工具坐标系名称（最大32字符）
+     * @param position 工具坐标系相对于法兰盘的位置偏移（x, y, z，单位：米）
+     * @param euler 工具坐标系相对于法兰盘的欧拉角偏移（roll, pitch, yaw，单位：弧度）
+     * @return true 设置成功，false 设置失败
+     */
+    LBOT_API bool lbot_set_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, 
+                            const lbot_position_t* position, const lbot_euler_t* euler);
+    
+    /**
+     * @brief 设置工具坐标系（使用向量）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 工具坐标系名称（最大32字符）
+     * @param position 工具坐标系相对于法兰盘的位置偏移（x, y, z，单位：米）向量
+     * @param euler 工具坐标系相对于法兰盘的欧拉角偏移（roll, pitch, yaw，单位：弧度）向量
+     * @return true 设置成功，false 设置失败
+     */
+    LBOT_API bool lbot_set_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name,
+                            const std::vector<double>& position, const std::vector<double>& euler);
+    
+    /**
+     * @brief 获取工具坐标系参数
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 工具坐标系名称
+     * @param position 返回的工具坐标系位置偏移
+     * @param euler 返回的工具坐标系欧拉角偏移
+     * @return true 获取成功，false 获取失败
+     */
+    LBOT_API bool lbot_get_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, 
+                            lbot_position_t* position, lbot_euler_t* euler);
+    
+    /**
+     * @brief 获取工具坐标系参数（使用向量）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 工具坐标系名称
+     * @param position 返回的工具坐标系位置偏移向量
+     * @param euler 返回的工具坐标系欧拉角偏移向量
+     * @return true 获取成功，false 获取失败
+     */
+    LBOT_API bool lbot_get_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name,
+                            std::vector<double>& position, std::vector<double>& euler);
+    
+    /**
+     * @brief 获取当前使用的工具坐标系
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 返回的当前工具坐标系名称
+     * @param position 返回的当前工具坐标系位置偏移
+     * @param euler 返回的当前工具坐标系欧拉角偏移
+     * @return true 获取成功，false 获取失败
+     */
+    LBOT_API bool lbot_get_current_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, 
+                                    std::string& name, 
+                                    lbot_position_t& position, 
+                                    lbot_euler_t& euler);
+    
+    /**
+     * @brief 获取当前使用的工具坐标系（使用向量）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 返回的当前工具坐标系名称
+     * @param position 返回的当前工具坐标系位置偏移向量
+     * @param euler 返回的当前工具坐标系欧拉角偏移向量
+     * @return true 获取成功，false 获取失败
+     */
+    LBOT_API bool lbot_get_current_tool_frame(lbot_handle_t *handle, lbot_arm_t arm,                                     std::string& name,
+                                    std::vector<double>& position,
+                                    std::vector<double>& euler);
+    
+    /**
+     * @brief 切换当前工具坐标系
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 要切换到的工具坐标系名称
+     * @return true 切换成功，false 切换失败
+     */
+    LBOT_API bool lbot_change_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name);
+    
+    /**
+     * @brief 切换当前工具坐标系（使用string）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 要切换到的工具坐标系名称
+     * @return true 切换成功，false 切换失败
+     */
+    LBOT_API bool lbot_change_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name);
+    
+    /**
+     * @brief 删除工具坐标系
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 要删除的工具坐标系名称
+     * @return true 删除成功，false 删除失败
+     */
+    LBOT_API bool lbot_delete_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name);
+    
+    /**
+     * @brief 删除工具坐标系（使用string）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 要删除的工具坐标系名称
+     * @return true 删除成功，false 删除失败
+     */
+    LBOT_API bool lbot_delete_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name);
+    
+    /**
+     * @brief 获取所有工具坐标系名称
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param names 返回的工具坐标系名称向量
+     * @return true 获取成功，false 获取失败
+     */
+    LBOT_API bool lbot_get_all_tool_frames(lbot_handle_t *handle, lbot_arm_t arm, std::vector<std::string>& names);
+    
+    // ==============================================
+    // 工作坐标系管理函数
+    // ==============================================
+    /**
+     * @brief 设置工作坐标系
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 工作坐标系名称（最大32字符）
+     * @param position 工作坐标系相对于基坐标系的位置偏移（x, y, z，单位：米）
+     * @param euler 工作坐标系相对于基坐标系的欧拉角偏移（roll, pitch, yaw，单位：弧度）
+     * @return true 设置成功，false 设置失败
+     */
+    LBOT_API bool lbot_set_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, 
+                            const lbot_position_t* position, const lbot_euler_t* euler);
+    
+    /**
+     * @brief 设置工作坐标系（使用向量）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 工作坐标系名称（最大32字符）
+     * @param position 工作坐标系相对于基坐标系的位置偏移（x, y, z，单位：米）向量
+     * @param euler 工作坐标系相对于基坐标系的欧拉角偏移（roll, pitch, yaw，单位：弧度）向量
+     * @return true 设置成功，false 设置失败
+     */
+    LBOT_API bool lbot_set_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name,
+                            const std::vector<double>& position, const std::vector<double>& euler);
+    
+    /**
+     * @brief 获取工作坐标系参数
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 工作坐标系名称
+     * @param position 返回的工作坐标系位置偏移
+     * @param euler 返回的工作坐标系欧拉角偏移
+     * @return true 获取成功，false 获取失败
+     */
+    LBOT_API bool lbot_get_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, 
+                            lbot_position_t* position, lbot_euler_t* euler);
+    
+    /**
+     * @brief 获取工作坐标系参数（使用向量）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 工作坐标系名称
+     * @param position 返回的工作坐标系位置偏移向量
+     * @param euler 返回的工作坐标系欧拉角偏移向量
+     * @return true 获取成功，false 获取失败
+     */
+    LBOT_API bool lbot_get_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name,
+                            std::vector<double>& position, std::vector<double>& euler);
+    
+    /**
+     * @brief 切换当前工作坐标系
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 要切换到的工作坐标系名称
+     * @return true 切换成功，false 切换失败
+     */
+    LBOT_API bool lbot_change_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name);
+    
+    /**
+     * @brief 切换当前工作坐标系（使用string）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 要切换到的工作坐标系名称
+     * @return true 切换成功，false 切换失败
+     */
+    LBOT_API bool lbot_change_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name);
+    
+    /**
+     * @brief 删除工作坐标系
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 要删除的工作坐标系名称
+     * @return true 删除成功，false 删除失败
+     */
+    LBOT_API bool lbot_delete_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name);
+    
+    /**
+     * @brief 删除工作坐标系（使用string）
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param name 要删除的工作坐标系名称
+     * @return true 删除成功，false 删除失败
+     */
+    LBOT_API bool lbot_delete_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name);
+    
+    /**
+     * @brief 获取所有工作坐标系名称
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param names 返回的工作坐标系名称向量
+     * @return true 获取成功，false 获取失败
+     */
+    LBOT_API bool lbot_get_all_work_frames(lbot_handle_t *handle, lbot_arm_t arm, std::vector<std::string>& names);
+    
+    // ==============================================
+    // 系统功能函数
+    // ==============================================
+    /**
+     * @brief 重新标定电机零位，设置当前位置为零位
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @return true 设置成功，false 设置失败
+     */
+    LBOT_API bool lbot_set_zero(lbot_handle_t *handle, lbot_arm_t arm);
+    
+    /**
+     * @brief 使能/掉使能机械臂
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param enable true 使能，false 掉使能
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_enable_arm(lbot_handle_t *handle, lbot_arm_t arm, bool enable);
+    
+    /**
+     * @brief 紧急停止/恢复
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param enable true 紧急停止，false 恢复运行
+     * @return true 指令发送成功，false 发送失败
+     */
+    LBOT_API bool lbot_emergency_stop(lbot_handle_t *handle, lbot_arm_t arm, bool enable);
+    
+    /**
+     * @brief 清除所有错误
+     * @param handle 机械臂句柄
+     * @return true 清除成功，false 清除失败
+     */
+    LBOT_API bool lbot_clear_errors(lbot_handle_t *handle);
+
+    /**
+     * @brief 设置机械臂关节限位
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param upper_joint_limit 最大关节限位参数
+     * @param lower_joint_limit 最小关节限位参数
+     * @return true 设置成功，false 设置失败
+     */
+    LBOT_API bool lbot_set_joint_limit(lbot_handle_t *handle, lbot_arm_t arm, const double upper_joint_limit[7], const double lower_joint_limit[7]);
+    /**
+     * @brief 获取机械臂关节限位参数
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param upper_joint_limit 返回的最大关节限位参数
+     * @param lower_joint_limit 返回的最小关节限位参数
+     * @return true 获取成功，false 获取失败
+     */
+    LBOT_API bool lbot_get_joint_limit(lbot_handle_t *handle, lbot_arm_t arm, double upper_joint_limit[7], double lower_joint_limit[7]);
+    /**
+     * @brief 获取机械臂默认关节限位参数
+     * @param handle 机械臂句柄
+     * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM
+     * @param upper_joint_limit 返回的最大关节限位参数
+     * @param lower_joint_limit 返回的最小关节限位参数
+     * @return true 获取成功，false 获取失败
+     */
+    LBOT_API bool lbot_get_default_joint_limit(lbot_handle_t *handle, lbot_arm_t arm, double upper_joint_limit[7], double lower_joint_limit[7]);
+
+    // ==============================================
+    // 工具函数
+    // ==============================================
+    /**
+     * @brief 获取最后一次错误信息
+     * @return 错误信息字符串
+     */
+    LBOT_API std::string lbot_get_last_error(lbot_handle_t *handle);
+    
+    /**
+     * @brief 设置日志级别
+     * @param level 日志级别：0-ERROR, 1-WARN, 2-INFO, 3-DEBUG
+     */
+    LBOT_API void lbot_set_log_level(int level);
+
+    // ==============================================
+    // 内存管理辅助函数
+    // ==============================================
+    /**
+     * @brief 释放字符串数组内存
+     * @param array 要释放的字符串数组
+     * @param count 数组元素数量
+     */
+    LBOT_API void lbot_free_string_array(char** array, int count);
+
+private:
+    // 禁用拷贝构造和赋值操作
+    LbotApi(const LbotApi&) = delete;
+    LbotApi& operator=(const LbotApi&) = delete;
+};
+
+} // namespace lbot
+
+#endif // LBOT_API_CPP_H

src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h

@@ -0,0 +1,430 @@
+// Copyright (c) 2025  LingSmart Tech
+//
+// 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.
+
+#ifndef LBOT_DRIVER_H
+#define LBOT_DRIVER_H
+
+#include <iostream>
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/clock.hpp" 
+#include <memory>
+#include <string>
+#include <thread>
+#include <chrono>
+#include <functional>
+#include <atomic>
+#include <unistd.h>             
+#include <signal.h>             
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <sys/ioctl.h>
+#include <sys/time.h>
+#include <sys/select.h>
+#include <fcntl.h>
+#include <rmw/qos_profiles.h>
+
+#include "lbot_api_cpp.h"
+
+// ROS2 标准消息类型
+#include <std_msgs/msg/empty.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <std_srvs/srv/empty.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include "std_msgs/msg/u_int8_multi_array.hpp"
+#include "std_msgs/msg/int32_multi_array.hpp"
+
+// 自定义 Message 类型
+#include "lbot_arm_interfaces/msg/arm_state.hpp"
+#include "lbot_arm_interfaces/msg/lbot_pose.hpp"
+#include "lbot_arm_interfaces/msg/lbot_frame.hpp"
+#include "lbot_arm_interfaces/msg/follow_joint.hpp"
+#include "lbot_arm_interfaces/msg/system_error.hpp"
+
+// 自定义 Service 类型
+#include "lbot_arm_interfaces/srv/change_frame.hpp"
+#include "lbot_arm_interfaces/srv/delete_frame.hpp"
+#include "lbot_arm_interfaces/srv/forward_kinematics.hpp"
+#include "lbot_arm_interfaces/srv/inverse_kinematics.hpp"
+#include "lbot_arm_interfaces/srv/move_c.hpp"
+#include "lbot_arm_interfaces/srv/move_j.hpp"
+#include "lbot_arm_interfaces/srv/move_jp.hpp"
+#include "lbot_arm_interfaces/srv/move_l.hpp"
+#include "lbot_arm_interfaces/srv/set_frame.hpp"
+#include "lbot_arm_interfaces/srv/set_string.hpp"
+#include "lbot_arm_interfaces/srv/set_zero.hpp"
+#include "lbot_arm_interfaces/srv/set_enable.hpp"
+#include "lbot_arm_interfaces/srv/set_emergency.hpp"
+#include "lbot_arm_interfaces/srv/get_frame.hpp"
+#include "lbot_arm_interfaces/srv/get_current_frame.hpp"
+#include "lbot_arm_interfaces/srv/get_all_frames.hpp"
+
+#define RAD_DEGREE 57.295791433
+#define DEGREE_RAD 0.01745329252
+
+using namespace std::chrono_literals;
+
+// 状态回调函数
+void lbot_state_callback_wrapper(const lbot_full_state_t* state);
+void lbot_error_callback_wrapper(int error_code, const char* error_msg);
+
+// 全局连接状态枚举
+enum class GlobalConnState {
+    DISCONNECTED,
+    CONNECTING,
+    CONNECTED
+};
+
+// 全局变量
+extern bool lbot_ctrl_flag;
+extern lbot::LbotApi lbot_api;
+extern lbot_handle_t *lbot_handle;
+extern std::atomic<GlobalConnState> g_conn_state;  // 全局连接状态
+
+namespace lbot_driver {
+
+// 主节点 - 负责连接管理、状态发布、心跳重连、关节跟随
+class LBot: public rclcpp::Node
+{
+public:
+    LBot(const std::string& node_name = "lbot_main_node");
+    ~LBot();
+
+    // 线程安全的状态缓存
+    std::mutex state_mutex_, conn_mutex_;
+    lbot_full_state_t current_lbot_state_;
+    // 节点关闭状态变量
+    std::atomic<bool> shutting_down_{false};
+    // 单例类指针
+    static LBot* g_instance;
+
+    // 重连机制相关线程与变量
+    std::thread reconnect_thread_;
+    std::atomic<bool> reconnect_thread_running_{false};
+    std::mutex reconnect_thread_mutex_;
+
+    void disconnect_robot();
+    void publish_system_error(int error_code, const char* error_msg);
+
+private:
+    // 初始化和连接相关
+    bool connect_robot();
+    void get_robot_info();
+    void state_publish_timer_callback();
+
+    // 心跳与重连机制回调函数
+    void heartbeat_timer_callback();
+    void reconnect_timer_callback();
+
+    // 关节跟随订阅回调函数（移到主节点，避免阻塞）
+    void left_joint_follow_callback(const lbot_arm_interfaces::msg::FollowJoint::SharedPtr msg);
+    void right_joint_follow_callback(const lbot_arm_interfaces::msg::FollowJoint::SharedPtr msg);
+
+    /****************************** 发布器 ******************************/
+    
+    // 状态发布器 (50Hz)
+    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr left_joint_pub_;
+    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr right_joint_pub_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr left_pose_pub_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr right_pose_pub_;
+    rclcpp::Publisher<lbot_arm_interfaces::msg::SystemError>::SharedPtr system_error_pub_;
+
+    /****************************** 订阅器 ******************************/
+    
+    // 关节跟随订阅器 (用于遥操作) - 移到主节点
+    rclcpp::Subscription<lbot_arm_interfaces::msg::FollowJoint>::SharedPtr left_joint_follow_sub_;
+    rclcpp::Subscription<lbot_arm_interfaces::msg::FollowJoint>::SharedPtr right_joint_follow_sub_;
+
+    /****************************** 定时器 ******************************/
+    
+    // 状态发布定时器 (50Hz)
+    rclcpp::TimerBase::SharedPtr state_publish_timer_;
+    rclcpp::TimerBase::SharedPtr heartbeat_timer_;
+    rclcpp::TimerBase::SharedPtr reconnect_timer_;
+
+    // 参数
+    std::string arm_ip_ = "192.168.10.21";
+
+    // 回调组
+    rclcpp::CallbackGroup::SharedPtr callback_group_timer_;
+    rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_;
+
+    // 连接状态
+    bool is_state_monitor_started_ = false;
+
+    GlobalConnState conn_state_ = GlobalConnState::DISCONNECTED;
+
+    // 心跳计数器（连续失败次数）
+    int heartbeat_fail_count_ = 0;
+};
+
+// 左臂服务节点 - 负责左臂所有服务
+class LeftArmServiceNode : public rclcpp::Node
+{
+public:
+    LeftArmServiceNode(const std::string& node_name = "lbot_left_arm_node");
+    ~LeftArmServiceNode() = default;
+
+private:
+    rclcpp::CallbackGroup::SharedPtr callback_group_, callback_group_subscribers_;
+    
+    void create_services();
+
+    // 灵巧手设置回调函数
+    void left_hand_l6_set_joint_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
+    void left_hand_l6_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
+    void left_hand_l6_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
+    void left_hand_l10_set_joint_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
+    void left_hand_l10_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
+    void left_hand_l10_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
+    void left_hand_l20_set_joint_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg);
+    
+    /****************************** 连接状态检查函数 ******************************/
+    
+    bool check_connection_state(const std::string& service_name) {
+        if (g_conn_state.load() != GlobalConnState::CONNECTED) {
+            RCLCPP_WARN(this->get_logger(), "%s: Robot not connected, service rejected", service_name.c_str());
+            return false;
+        }
+        return true;
+    }
+
+    /****************************** 左臂服务回调函数 ******************************/
+    
+    // 运动控制
+    void move_joint_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::MoveJ::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::MoveJ::Response> response);
+    
+    void move_pose_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::MoveJP::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::MoveJP::Response> response);
+    
+    void move_linear_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::MoveL::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::MoveL::Response> response);
+
+    // 运动学计算
+    void forward_kinematics_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::ForwardKinematics::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::ForwardKinematics::Response> response);
+    
+    void inverse_kinematics_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::InverseKinematics::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::InverseKinematics::Response> response);
+
+    // 工具坐标系管理
+    void set_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetFrame::Response> response);
+    
+    void get_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::GetFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::GetFrame::Response> response);
+
+    void get_current_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::GetCurrentFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::GetCurrentFrame::Response> response);
+    
+    void change_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::ChangeFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::ChangeFrame::Response> response);
+    
+    void delete_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::DeleteFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::DeleteFrame::Response> response);
+    
+    void get_all_tool_frames_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::GetAllFrames::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::GetAllFrames::Response> response);
+
+    // 系统设置
+    void set_zero_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetZero::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetZero::Response> response);
+    void set_enable_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetEnable::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetEnable::Response> response);
+    void set_emergency_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetEmergency::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetEmergency::Response> response);
+
+    /****************************** 服务器 ******************************/
+    
+    // 运动控制服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::MoveJ>::SharedPtr move_joint_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::MoveJP>::SharedPtr move_pose_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::MoveL>::SharedPtr move_linear_service_;
+
+    // 运动学计算服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::ForwardKinematics>::SharedPtr forward_kinematics_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::InverseKinematics>::SharedPtr inverse_kinematics_service_;
+
+    // 工具坐标系管理服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::SetFrame>::SharedPtr set_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::GetFrame>::SharedPtr get_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::GetCurrentFrame>::SharedPtr get_current_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::ChangeFrame>::SharedPtr change_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::DeleteFrame>::SharedPtr delete_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::GetAllFrames>::SharedPtr get_all_tool_frames_service_;
+
+    // 系统设置服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::SetZero>::SharedPtr set_zero_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::SetEnable>::SharedPtr set_enable_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::SetEmergency>::SharedPtr set_emergency_service_;
+
+    /****************************** 灵巧手Topic ******************************/
+
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l6_joint_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l6_force_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l6_speed_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l10_joint_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l10_force_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l10_speed_sub_;
+    rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr left_hand_l20_joint_sub_;
+};
+
+// 右臂服务节点 - 负责右臂所有服务
+class RightArmServiceNode : public rclcpp::Node
+{
+public:
+    RightArmServiceNode(const std::string& node_name = "lbot_right_arm_node");
+    ~RightArmServiceNode() = default;
+
+private:
+    rclcpp::CallbackGroup::SharedPtr callback_group_, callback_group_subscribers_;
+    
+    void create_services();
+
+    // 灵巧手设置回调函数
+    void right_hand_l6_set_joint_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
+    void right_hand_l6_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
+    void right_hand_l6_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
+    void right_hand_l10_set_joint_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
+    void right_hand_l10_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
+    void right_hand_l10_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
+    void right_hand_l20_set_joint_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg);
+    
+    /****************************** 连接状态检查函数 ******************************/
+    
+    bool check_connection_state(const std::string& service_name) {
+        if (g_conn_state.load() != GlobalConnState::CONNECTED) {
+            RCLCPP_WARN(this->get_logger(), "%s: Robot not connected, service rejected", service_name.c_str());
+            return false;
+        }
+        return true;
+    }
+
+    /****************************** 右臂服务回调函数 ******************************/
+    
+    // 运动控制
+    void move_joint_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::MoveJ::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::MoveJ::Response> response);
+    
+    void move_pose_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::MoveJP::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::MoveJP::Response> response);
+    
+    void move_linear_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::MoveL::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::MoveL::Response> response);
+
+    // 运动学计算
+    void forward_kinematics_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::ForwardKinematics::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::ForwardKinematics::Response> response);
+    
+    void inverse_kinematics_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::InverseKinematics::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::InverseKinematics::Response> response);
+
+    // 工具坐标系管理
+    void set_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetFrame::Response> response);
+    
+    void get_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::GetFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::GetFrame::Response> response);
+
+    void get_current_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::GetCurrentFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::GetCurrentFrame::Response> response);
+    
+    void change_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::ChangeFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::ChangeFrame::Response> response);
+    
+    void delete_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::DeleteFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::DeleteFrame::Response> response);
+    
+    void get_all_tool_frames_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::GetAllFrames::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::GetAllFrames::Response> response);
+
+    // 系统设置
+    void set_zero_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetZero::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetZero::Response> response);
+    void set_enable_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetEnable::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetEnable::Response> response);
+    void set_emergency_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetEmergency::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetEmergency::Response> response);
+
+    /****************************** 服务器 ******************************/
+    
+    // 运动控制服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::MoveJ>::SharedPtr move_joint_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::MoveJP>::SharedPtr move_pose_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::MoveL>::SharedPtr move_linear_service_;
+
+    // 运动学计算服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::ForwardKinematics>::SharedPtr forward_kinematics_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::InverseKinematics>::SharedPtr inverse_kinematics_service_;
+
+    // 工具坐标系管理服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::SetFrame>::SharedPtr set_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::GetFrame>::SharedPtr get_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::GetCurrentFrame>::SharedPtr get_current_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::ChangeFrame>::SharedPtr change_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::DeleteFrame>::SharedPtr delete_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::GetAllFrames>::SharedPtr get_all_tool_frames_service_;
+
+    // 系统设置服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::SetZero>::SharedPtr set_zero_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::SetEnable>::SharedPtr set_enable_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::SetEmergency>::SharedPtr set_emergency_service_;
+
+    /****************************** 灵巧手Topic ******************************/
+
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l6_joint_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l6_force_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l6_speed_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l10_joint_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l10_force_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l10_speed_sub_;
+    rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr right_hand_l20_joint_sub_;
+};
+
+}  // namespace lbot_driver
+
+#endif // LBOT_DRIVER_H

src/officer_sdk/lbot_driver/include/lbot_driver/lbot_types.h

@@ -0,0 +1,99 @@
+/**
+ * @file lbot_types.h
+ * @brief 该文件定义了结构和枚举定义
+ * @date 2026.1.19
+ * @copyright 灵心巧手科技有限公司
+ */
+#ifndef LBOT_TYPES_H
+#define LBOT_TYPES_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+
+// 机械臂类型枚举
+typedef enum {
+    LBOT_LEFT_ARM = 0,
+    LBOT_RIGHT_ARM = 1
+} lbot_arm_t;
+
+// 机械臂控制句柄
+typedef struct {
+    uint64_t id;         // 句柄ID，连接成功返回>0的值，0表示无效句柄
+}lbot_handle_t;
+
+// 运动类型枚举
+typedef enum {
+    LBOT_MOVE_JOINT = 0,      // 关节空间运动
+    LBOT_MOVE_POSE = 1,       // 笛卡尔空间点到点
+    LBOT_MOVE_LINEAR = 2      // 笛卡尔空间直线运动
+} lbot_move_type_t;
+
+// 坐标系结构体
+typedef struct {
+    double x, y, z;
+} lbot_position_t;
+
+typedef struct {
+    double x, y, z, w;
+} lbot_orientation_t;
+
+typedef struct {
+    double x, y, z;
+} lbot_euler_t;
+
+// 关节状态结构体
+typedef struct {
+    // 关节数据
+    char name[7][32];         // 7个关节名称
+    double joint_position[7]; // 7个关节位置
+    double velocity[7];       // 7个关节速度
+    double effort[7];         // 7个关节力矩
+    double temperature[7];    // 7个关节温度
+    
+    // 时间戳
+    int32_t sec;              // 秒
+    uint32_t nanosec;         // 纳秒
+    char frame_id[64];        // 工作坐标系
+    
+    // 末端状态
+    lbot_position_t end_effector_position; // 末端位置
+    lbot_euler_t euler;                    // 欧拉角
+    lbot_orientation_t orientation;        // 四元数姿态
+} lbot_arm_state_t;
+
+// 机械臂完整状态结构体
+typedef struct {
+    lbot_arm_state_t left_arm;
+    lbot_arm_state_t right_arm;
+    uint64_t system_timestamp; // 系统时间戳（纳秒）
+    char arm_ip[16];           // 机械臂IP地址
+} lbot_full_state_t;
+
+// 回调函数类型定义
+typedef void (*lbot_state_callback_t)(const lbot_full_state_t* state);
+typedef void (*lbot_error_callback_t)(int error_code, const char* error_msg);
+
+
+typedef enum {
+    LBOT_FINGER_THUMB = 0,    // 大拇指
+    LBOT_FINGER_INDEX = 1,    // 食指
+    LBOT_FINGER_MID = 2,      // 中指
+    LBOT_FINGER_RING = 3,     // 无名指
+    LBOT_FINGER_LITTLE = 4    // 小指
+} lbot_finger_type_t;
+
+typedef struct {
+    uint8_t data[6];          // 单根手指各电机位置（指根、指尖、侧摆、旋转）
+    lbot_finger_type_t finger;        // 手指标识，0~4表示thumb,index,mid,ring,little
+} lbot_l20_series_cmd_t;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LBOT_TYPES_H

src/officer_sdk/lbot_driver/include/lbot_driver/lbot_version.h

@@ -0,0 +1,23 @@
+/**
+ * @file lbot_version.h
+ * @brief 该文件指定API版本号
+ * @date 2026.1.19
+ * @copyright 灵心巧手科技有限公司
+ */
+#ifndef LBOT_VERSION_H
+#define LBOT_VERSION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define SDK_VERSION ("1.0.5")
+#define SDK_BUILD_TIME ("2026.4.30")
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+                        

src/officer_sdk/lbot_driver/launch/lbot_start_driver.launch.py

@@ -0,0 +1,41 @@
+import launch
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    # YAML 默认模板文件
+    base_yaml_file = os.path.join(
+        get_package_share_directory('lbot_driver'),
+        'config', 'lbot_config.yaml'
+    )
+
+    # 定义机器人列表，每个机器人名字和 IP
+    robots = [
+        {"name": "robot1", "arm_ip": "192.168.10.21"},
+        # {"name": "robot2", "arm_ip": "192.168.10.22"},
+    ]
+
+    nodes = []
+
+    for robot in robots:
+        # 每个机器人只需要启动一个 lbot_driver 可执行文件
+        # 这个可执行文件内部会创建三个节点：主节点、左臂服务节点、右臂服务节点
+        driver_node = Node(
+            package='lbot_driver',
+            executable='lbot_driver',
+            namespace=robot["name"],        # 设置 namespace
+            parameters=[
+                base_yaml_file,             # 默认 YAML 文件
+                {                           # 覆盖参数
+                    "arm_ip": robot["arm_ip"]
+                }
+            ],
+            output='screen',
+            emulate_tty=True,              # 更好的日志输出格式
+        )
+        nodes.append(driver_node)
+
+    return LaunchDescription(nodes)

src/officer_sdk/lbot_driver/lib/lib_install.sh

@@ -0,0 +1,36 @@
+#!/bin/bash
+set -e
+
+# 功能包 lib 目录（脚本所在目录就是 lib）
+LIB_DIR="$(cd "$(dirname "$0")" && pwd)"
+
+echo "Target lib directory: $LIB_DIR"
+
+# 根据系统架构选择对应文件夹
+ARCH=$(uname -m)
+if [ "$ARCH" = "x86_64" ]; then
+    SRC_DIR="$LIB_DIR/linux_x64"
+elif [ "$ARCH" = "aarch64" ] || [ "$ARCH" = "arm64" ]; then
+    SRC_DIR="$LIB_DIR/linux_arm64"
+else
+    echo "Unsupported architecture: $ARCH"
+    exit 1
+fi
+
+TARGET_SO="$LIB_DIR/liblbot_api_cpp.so.1.0.0"
+
+echo "Using source directory: $SRC_DIR"
+echo "Removing old files..."
+rm -f "$LIB_DIR/liblbot_api_cpp.so" \
+      "$LIB_DIR/liblbot_api_cpp.so.1" \
+      "$TARGET_SO"
+
+echo "Copying..."
+cp "$SRC_DIR/liblbot_api_cpp.so" "$TARGET_SO"
+
+echo "Creating symlinks..."
+ln -s "liblbot_api_cpp.so.1.0.0" "$LIB_DIR/liblbot_api_cpp.so.1"
+ln -s "liblbot_api_cpp.so.1.0.0" "$LIB_DIR/liblbot_api_cpp.so"
+
+echo "[SUCCESS] Installed."
+

src/officer_sdk/lbot_driver/lib/liblbot_api_cpp.so

@@ -0,0 +1 @@
+liblbot_api_cpp.so.1.0.0

src/officer_sdk/lbot_driver/lib/liblbot_api_cpp.so.1

@@ -0,0 +1 @@
+liblbot_api_cpp.so.1.0.0