# LinkerHand O6 MuJoCo（ROS2）启动说明

本机跑 MuJoCo 左手仿真，可用本机或另一台电脑通过 ROS2 话题控制。

---

## 环境要求

- Ubuntu + ROS 2 **Jazzy**
- 工作区路径：`~/linker_hand_mujoco_ros2`
- Python 依赖在 `venv` 中（含 `mujoco`、`PyQt5`）
- **注意**：`ros2 launch` 使用系统 `/usr/bin/python3`，仅 `source venv` **不够**，需用下文的启动脚本或设置 `PYTHONPATH`

---

## 一、编译（首次或改代码后）

```bash
cd ~/linker_hand_mujoco_ros2
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install --packages-select linker_hand_mujoco_ros2
source install/setup.bash
```

`venv` 目录已放 `COLCON_IGNORE`，不会被 colcon 误扫。

---

## 二、本机启动仿真（推荐）

```bash
cd ~/linker_hand_mujoco_ros2
./run_sim.sh
```

脚本会自动：

- `source` ROS2 与本工作区
- `ROS_DOMAIN_ID=30`，`ROS_LOCALHOST_ONLY=0`（便于多机）
- 把 `venv` 的 `site-packages` 加入 `PYTHONPATH`（解决找不到 `mujoco`）

成功后应出现：

1. MuJoCo 3D 窗口（左手 O6）
2. Joint 进度条窗口（实时关节角）

### 手动启动（等效）

```bash
cd ~/linker_hand_mujoco_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash
export PYTHONPATH="$PWD/venv/lib/python3.12/site-packages:$PYTHONPATH"
ros2 launch linker_hand_mujoco_ros2 linker_hand_mujoco_ros2.launch.py
```

当前 launch 默认：

| 参数 | 值 |
|------|-----|
| `hand_type` | `left` |
| `hand_joint` | `O6` |
| 控制话题 | `/cb_left_hand_control_cmd` |

---

## 三、控制话题说明

- 话题：`/cb_left_hand_control_cmd`
- 类型：`sensor_msgs/msg/JointState`
- `position`：**6 个数**，范围约 **0–255**

| 索引 | 含义 |
|------|------|
| 0 | 拇指弯曲 |
| 1 | 拇指横摆 |
| 2 | 食指弯曲 |
| 3 | 中指弯曲 |
| 4 | 无名指弯曲 |
| 5 | 小指弯曲 |

约定（当前映射）：约 **255 = 张开，0 = 握紧**。远指节由 mimic 跟随近指节。

### 本机发一帧（握拳示例）

```bash
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=30
export ROS_LOCALHOST_ONLY=0

ros2 topic pub --once /cb_left_hand_control_cmd sensor_msgs/msg/JointState "{
  header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''},
  name: [],
  position: [0.0, 100.0, 0.0, 0.0, 0.0, 0.0],
  velocity: [],
  effort: []
}"
```

张开：

```bash
ros2 topic pub --once /cb_left_hand_control_cmd sensor_msgs/msg/JointState "{
  header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''},
  name: [],
  position: [255.0, 255.0, 255.0, 255.0, 255.0, 255.0],
  velocity: [],
  effort: []
}"
```

---

## 四、两台电脑共用 ROS2

ROS2 **没有** ROS1 的 `roscore`。两边约定同一个 Domain 即可。

### 两边都设置

```bash
export ROS_DOMAIN_ID=30
export ROS_LOCALHOST_ONLY=0
```

### 电脑 A：仿真机

```bash
cd ~/linker_hand_mujoco_ros2
./run_sim.sh
```

### 电脑 B：控制机（发指令 / handretarget / SDK）

```bash
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=30
export ROS_LOCALHOST_ONLY=0

# 确认话题与收发端
ros2 topic list
ros2 topic info /cb_left_hand_control_cmd -v
```

`topic info -v` 中应能看到：

- **Publisher**：例如 `handretarget_node`
- **Subscription**：至少包含 `linker_hand_mujoco_ros2_node`

### 查是否真的在发数据

有 Publisher ≠ 正在发数据。请用：

```bash
ros2 topic hz /cb_left_hand_control_cmd
ros2 topic echo /cb_left_hand_control_cmd
```

- `hz` 无输出 → 对端没在 publish  
- `echo --once` 会一直等到有一帧；无数据时像卡住，优先用上面两条

### 跨机能看见话题但收不到数据时

1. 确认两边 `ROS_DOMAIN_ID`、`ROS_LOCALHOST_ONLY=0` 一致，并重启节点  
2. 互相 `ping` 通；尽量同一网段 / 有线；关闭路由器 **AP 隔离**  
3. 本机 `ufw` 可先确认：`sudo ufw status`（不活动则不是防火墙）  
4. 需要时可配置 CycloneDDS 静态 Peer（两边 IP 写入 `~/cyclonedds.xml`），然后：

```bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=file://$HOME/cyclonedds.xml
```

---

## 五、常用自检命令

```bash
# 节点是否在
ros2 node list

# 话题列表（仿真未启动时可能还没有 /cb_left_hand_control_cmd）
ros2 topic list

# 订阅/发布数量与节点名
ros2 topic info /cb_left_hand_control_cmd -v

# 频率 / 内容
ros2 topic hz /cb_left_hand_control_cmd
ros2 topic echo /cb_left_hand_control_cmd
```

---

## 六、常见问题

### `ModuleNotFoundError: No module named 'mujoco'`

`ros2 launch` 没用到 venv。请用：

```bash
./run_sim.sh
```

或设置 `PYTHONPATH`（见上文「手动启动」）。

### `topic list` 只有 `/rosout`，没有左手话题

仿真节点未运行。先 `./run_sim.sh`，再 `ros2 topic list`。

### 改左右手

编辑：

`src/linkerhand-sim/linker_hand_mujoco_ros2/launch/linker_hand_mujoco_ros2.launch.py`

- 左手：`hand_type: 'left'` → 话题 `/cb_left_hand_control_cmd`
- 右手：`hand_type: 'right'` → 话题 `/cb_right_hand_control_cmd`

改完后若未用 symlink，再执行一次 `colcon build --symlink-install`。

---

## 七、通用曲线录制（仿真 / 真机同一套话题）

**只传一个文件给对端即可：**

`tools/hand_curve_recorder_standalone.py`

两边都要：已 `source` ROS2、`numpy`、`matplotlib`。

```bash
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=30
export ROS_LOCALHOST_ONLY=0

# 仿真端
python3 tools/hand_curve_recorder_standalone.py --hand left --channel 3 --label sim

# 真机端（把该 py 拷过去后）
python3 hand_curve_recorder_standalone.py --hand left --channel 3 --label real
```

对端发 `/cb_left_hand_control_cmd`；本机 **Ctrl+C** → `reports/hand_curves/` 下生成 csv + 三曲线图。

默认还订 `/cb_left_hand_state`（`position`=0～255，`effort` 可放电流）。话题名不同时：

```bash
python3 hand_curve_recorder_standalone.py --state-topic /你的状态话题 --channel 3 --label real
```

通道：`0拇指弯 1拇指横摆 2食指 3中指 4无名指 5小指`。

---

要「ROS 发指令 + 仿真里出 q/v/τ 图」：

```bash
cd ~/linker_hand_mujoco_ros2
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=30
export ROS_LOCALHOST_ONLY=0

# 自测：订话题 + 本地 MuJoCo + 自动发中指慢扫 + 可选 viewer
./venv/bin/python tools/ros_mujoco_record_qvt.py \
  --hand left --finger middle --viewer --self-sweep

# 对端发指令时：去掉 --self-sweep
./venv/bin/python tools/ros_mujoco_record_qvt.py \
  --hand left --finger middle --viewer
# Ctrl+C → reports/O6_middle_ros/*_qvt.png
```

---

## 八、文件与模型位置

| 路径 | 说明 |
|------|------|
| `./run_sim.sh` | 本机一键启动 |
| `src/.../launch/linker_hand_mujoco_ros2.launch.py` | 左右手 / 型号 |
| `src/.../urdf/O6/` | O6 左右手 MJCF（来自 mujoco_testwork 验证模型） |
| `src/.../utils/joint_monitor.py` | Joint 进度条 UI |
| `venv/` | Python 依赖（勿放进 colcon 扫描，已 IGNORE） |