# tp_hand_bringup_ros2

**Repository Path**: tp_motion/tp_hand_bringup_ros2

## Basic Information

- **Project Name**: tp_hand_bringup_ros2
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Apache-2.0
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-06-01
- **Last Updated**: 2026-08-01

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

[English](./README.md) | 中文

# TP Hand Bringup ROS2 Node

本软件包提供了一个 ROS2 节点（`tp_hand_node`），用于通过 CAN‑FD（使用 ZLG USB‑CANFD 设备）控制和监控 **TP 系列灵巧手**。该节点通过底层 C API（`interface.h`）与灵巧手通信，发送关节指令，并发布实时的关节状态、传感器遥测和诊断信息。

---

## 功能特性

- **双手支持** – 左手和/或右手可独立启用。
- **实时指令** – 以可配置的速率发送关节位置/速度/力矩指令。
- **完整遥测** – 发布关节状态（位置、速度、力矩）、电机位置/速度、指尖触觉传感器和总线诊断信息。
- **模式切换** – 每个主动关节可独立设置为不同的控制模式（位置、电流、阻抗、零力矩等）。
- **超时保护** – 在可配置的超时后自动将指令置零或切换为零力矩模式。
- **诊断信息** – 总线电压、电流、温度和故障码通过 `/diagnostics` 话题发布。

工作空间包含两个 ROS 2 软件包：

```text
src/
├── tp_hand_bringup/       # 硬件通信、CAN-FD 控制、ROS 话题桥接
└── tp_hand_description/   # URDF 和 STL 网格模型
```

---

## 依赖项

- **ROS 2**（Humble 或更高版本）
- **ZLG CAN‑FD 库** – 提供 `zcan.h` 和共享库（通常为 `libusbcanfd.so`）。
- **C API** – `interface.h` 和 `libtphandapi.so`（包含在软件包中）。
- `robot_state_publisher`（如果使用启动文件）。

---

## 构建说明

```bash
# 将仓库克隆到您的 ROS 工作空间
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
git clone https://github.com/lzh9780/tp_hand_bringup_ros2.git
# 若使用 Gitee，请从以下仓库克隆：
# git clone https://gitee.com/tp_motion/tp_hand_bringup_ros2.git

# 安装依赖（如果有）
rosdep install --from-paths . --ignore-src -r -y

# 构建
cd ~/ros2_ws
colcon build --packages-select tp_hand_bringup tp_hand_description

# 加载环境
source install/setup.bash
```

---

## CAN 设备安装与权限

ZLG 官方文档：[ZLG Linux 驱动](https://manual.zlg.cn/web/#/42/1710)

### 简单安装步骤：
- 从 [https://manual.zlg.cn/web/#/146](https://manual.zlg.cn/web/#/146) 下载驱动
- 安装 libusb-1.0
  ```bash
  sudo apt-get install libusb-1.0-0
  ```
- 解压下载的驱动，进入解压后的文件夹，运行
  ```bash
  sudo cp libusbcanfd.so /lib
  ```

### 权限

如果当前用户为普通用户，执行设备操作需要使用 `sudo` 调用 root 权限，或者可以使用以下命令修改设备访问权限：

```bash
sudo chmod 666 /dev/bus/usb/xxx/yyy
```
其中 `xxx` 对应 `lsusb` 输出中 USB 设备的总线编号，`yyy` 对应设备编号。例如，USBCANFD 的总线编号为 004，设备编号为 057。

要永久修改用户权限，需要修改 udev 配置，添加文件：`/etc/udev/rules.d/50-usbcanfd.rules`，内容如下：

```text
SUBSYSTEMS=="usb", ATTRS{idVendor}=="04cc", ATTRS{idProduct}=="1240", GROUP="users", MODE="0666"
```

使用以下命令重新加载 udev 规则：

```bash
udevadm control --reload
```

最后，插拔设备即可应用新权限。

---

## ROS 2 接口

### 发布的话题

| 话题 | 类型 | 描述 |
|------|------|------|
| `/joint_states` | `sensor_msgs/JointState` | 每只手全部 13 个关节（主动关节 + 关联的 DIP/PIP）的完整关节状态（位置、速度、力矩）。 |
| `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | 总线信息（电压、电流、温度、故障码）以及每个关节的模式/力矩。 |
| `/tp_hand_telemetry/{prefix}tactile` | `std_msgs/Int16MultiArray` | 指尖触觉传感器数值（5 个值）。 |
| `/tp_hand_telemetry/{prefix}motor_position` | `std_msgs/Int16MultiArray` | 8 个主动关节的电机编码器位置。 |
| `/tp_hand_telemetry/{prefix}motor_speed` | `std_msgs/Int16MultiArray` | 电机速度（由位置计算得出）。 |

*`{prefix}` 为 `left_` 或 `right_`。*

### 订阅的话题

| 话题 | 类型 | 描述 |
|------|------|------|
| `/tp_hand_control/command` | `trajectory_msgs/JointTrajectory` | 设定点轨迹（仅使用第一个点）。关节名称必须匹配手的前缀（例如 `left_yaw`）。 |
| `/tp_hand_control/{prefix}modes` | `std_msgs/Int16MultiArray` | 所有 8 个主动关节的控制模式（顺序：yaw, pitch, thumb_mcp, thumb_rot, index_mcp, middle_mcp, ring_mcp, pinky_mcp）。 |
| `/tp_hand_control/{prefix}single_mode` | `std_msgs/Int16MultiArray` | 为单个关节设置模式。格式：`[joint_id (1‑8), mode_value]`。 |

**控制模式取值**（参见 `interface.h`）：

| 模式 | 数值（十进制） |
|------|---------------|
| `GRIP_CTRLMODE_ZERO_TORQUE` | 0 |
| `GRIP_CTRLMODE_CURRENT` | 16 |
| `GRIP_CTRLMODE_HALLPOS` | 32 |
| `GRIP_CTRLMODE_JOINT_POS` | 48 |
| `GRIP_CTRLMODE_IMPEDANCE` | 64 |
| `GRIP_CTRLMODE_CASCADED_PID` | 80 |
| `GRIP_CTRLMODE_MIT_TORQUE` | 96 |
| `GRIP_CTRLMODE_HYBRID` | 112 |
| `GRIP_CTRLMODE_FORCE_POS_HYBRID` | 128 |
| `GRIP_CTRLMODE_EMERGENCY` | 144 |
| `GRIP_CTRLMODE_HM` | 160 |

*示例：将左手所有关节设置为 **joint_position** 模式（48）：*
```
ros2 topic pub /tp_hand_control/left_modes std_msgs/Int16MultiArray "{data: [48,48,48,48,48,48,48,48]}"
```

### 主动关节 ID 与 ROS 关节名称

每只手有 8 个主动控制 ID。ROS 命令使用带有 `left_` 或 `right_` 前缀的关节名称。

| 设备 ID | 基本关节名称 | 左手命令名称 | 右手命令名称 |
|---:|---|---|---|
| 1 | `yaw` | `left_yaw` | `right_yaw` |
| 2 | `pitch` | `left_pitch` | `right_pitch` |
| 3 | `thumb_mcp` | `left_thumb_mcp` | `right_thumb_mcp` |
| 4 | `thumb_rot` | `left_thumb_rot` | `right_thumb_rot` |
| 5 | `index_mcp` | `left_index_mcp` | `right_index_mcp` |
| 6 | `middle_mcp` | `left_middle_mcp` | `right_middle_mcp` |
| 7 | `ring_mcp` | `left_ring_mcp` | `right_ring_mcp` |
| 8 | `pinky_mcp` | `left_pinky_mcp` | `right_pinky_mcp` |

每只手在 `/joint_states` 中发布 13 个关节：

```text
<prefix>pitch
<prefix>yaw
<prefix>thumb_rot
<prefix>thumb_mcp
<prefix>thumb_dip
<prefix>index_mcp
<prefix>index_pip
<prefix>middle_mcp
<prefix>middle_pip
<prefix>ring_mcp
<prefix>ring_pip
<prefix>pinky_mcp
<prefix>pinky_pip
```

从动关节由主动 MCP 关节推导得出：

| 从动关节 | 关系 |
|---|---|
| `thumb_dip` | `thumb_mcp * 1.33` |
| `index_pip` | `index_mcp * 1.166` |
| `middle_pip` | `middle_mcp * 1.166` |
| `ring_pip` | `ring_mcp * 1.166` |
| `pinky_pip` | `pinky_mcp * 1.166` |

---

## 参数

| 名称 | 类型 | 默认值 | 描述 |
|------|------|--------|------|
| `can_type` | int | 33 | ZLG 设备类型（例如 USB‑CANFD‑200U 为 33）。 |
| `can_index` | int | 0 | ZLG 设备索引。 |
| `enable_left` | bool | true | 启用左手。 |
| `enable_right` | bool | false | 启用右手。 |
| `left_hand_id` | int | 1 | 左手的 CAN 标识符。 |
| `left_hand_channel` | int | 0 | 左手的 CAN 通道。 |
| `right_hand_id` | int | 2 | 右手的 CAN 标识符。 |
| `right_hand_channel` | int | 1 | 右手的 CAN 通道。 |
| `command_period_ms` | int | 10 | 指令发送周期（毫秒）。 |
| `state_period_ms` | int | 10 | 状态发布周期（毫秒）。 |
| `command_timeout_sec` | double | 0.5 | 超时时间（若未收到新指令，则置零指令）。 |
| `send_before_first_command` | bool | false | 若为 true，即使在收到第一条轨迹之前也会发送零力矩指令。 |
| `timeout_zero_torque` | bool | true | 若为 true，超时时将类似力矩的模式切换为零力矩。 |

---

## 启动文件

提供了启动文件 `bringup.launch.xml`，用于启动节点以及 `robot_state_publisher`（发布 URDF 用于可视化）。示例用法：

```bash
# 使用默认设置启动（仅左手）
ros2 launch tp_hand_bringup bringup.launch.xml

# 启用双手，设置自定义通道
ros2 launch tp_hand_bringup bringup.launch.xml \
    enable_left:=true \
    enable_right:=true \
    left_hand_channel:=0 \
    right_hand_channel:=1 \
    command_period_ms:=5
```

---

## 命令行控制示例

### 将右手设置为关节位置模式

```bash
ros2 topic pub --once /tp_hand_control/right_modes std_msgs/msg/Int16MultiArray \
"{data: [48, 48, 48, 48, 48, 48, 48, 48]}"
```

### 右手单关节控制

控制右食指 MCP 到 `1.0 rad`：

```bash
ros2 topic pub --once /tp_hand_control/command trajectory_msgs/msg/JointTrajectory \
"{joint_names: ['right_index_mcp'],
points: [{positions: [1.0], velocities: [0.0], effort: [0]}]}"
```

该指令只需发布一次。节点会缓存目标值，并按 `command_period_ms` 固定频率持续发送 CAN 指令。

### 张开右手

```bash
ros2 topic pub --once /tp_hand_control/command trajectory_msgs/msg/JointTrajectory \
"{joint_names: [
'right_yaw',
'right_pitch',
'right_thumb_rot',
'right_thumb_mcp',
'right_index_mcp',
'right_middle_mcp',
'right_ring_mcp',
'right_pinky_mcp'
],
points: [{
positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
effort: [0, 0, 0, 0, 0, 0, 0, 0]
}]}"
```

### 同时控制双手

```bash
ros2 topic pub --once /tp_hand_control/command trajectory_msgs/msg/JointTrajectory \
"{joint_names: [
'left_yaw', 'left_pitch', 'left_thumb_rot', 'left_thumb_mcp',
'left_index_mcp', 'left_middle_mcp', 'left_ring_mcp', 'left_pinky_mcp',
'right_yaw', 'right_pitch', 'right_thumb_rot', 'right_thumb_mcp',
'right_index_mcp', 'right_middle_mcp', 'right_ring_mcp', 'right_pinky_mcp'
],
points: [{
positions: [
0.0, 0.0, 0.6, 0.8, 1.2, 1.2, 1.2, 1.2,
0.0, 0.0, 0.6, 0.8, 1.2, 1.2, 1.2, 1.2
],
velocities: [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
],
effort: [
0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0
]
}]}"
```

### 紧急停止 / 零力矩

右手全部关闭为零力矩：

```bash
ros2 topic pub --once /tp_hand_control/right_modes std_msgs/msg/Int16MultiArray \
"{data: [0, 0, 0, 0, 0, 0, 0, 0]}"
```

左手全部关闭为零力矩：

```bash
ros2 topic pub --once /tp_hand_control/left_modes std_msgs/msg/Int16MultiArray \
"{data: [0, 0, 0, 0, 0, 0, 0, 0]}"
```

---

## 查看状态

```bash
ros2 topic echo /joint_states
```

```bash
ros2 topic echo /tp_hand_telemetry/right_motor_position
```

```bash
ros2 topic echo /tp_hand_telemetry/right_motor_speed
```

```bash
ros2 topic echo /tp_hand_telemetry/right_tactile
```

```bash
ros2 topic echo /diagnostics
```

查看话题列表：

```bash
ros2 topic list
```

查看 TF / 机器人状态：

```bash
ros2 run rviz2 rviz2
```

---

## 故障排除

### 启动失败：无法打开 CAN 设备
检查：
```bash
lsusb
dmesg -w
```
确认：
- CAN-FD 设备已连接；
- `can_type`、`can_index`、`*_hand_channel` 与硬件匹配；
- 当前用户具有 USB 设备权限；
- 可加载厂商 `.so` 库。

### 无关节状态或传感器数据
- 检查 CAN‑FD 设备是否被识别（`ls /dev/zcan*`）。
- 验证 `enable_left` / `enable_right` 是否与物理手匹配。
- 确保手已上电且 CAN 接线正确。
- 查看终端中的错误信息（节点会记录通信失败警告）。

### 超时过早触发
- 如果您的轨迹发布器发送指令慢于超时时间，请增大 `command_timeout_sec`。

### 已发送指令但手不运动
按顺序检查：
1. 是否将关节位置模式设置为 `48`？
2. 指令关节名称是否带有正确前缀：`left_` 或 `right_`？
3. 是否已收到第一条指令？若 `send_before_first_command=false`，则只有在收到指令后才会开始发送。
4. `/diagnostics` 中是否有总线故障？
5. CAN 通道是否正确？
6. 手是否已上电？

---

## 许可证

本软件依照 [Apache License 2.0](LICENSE) 提供。