# Qnbot 外骨骼远程控制器 Demo

这个包提供了通过 WebSocket 接收外骨骼数据并发布到专用话题的解决方案。

## 🚀 快速开始

### 一键启动（推荐）

同时启动 WebSocket 监听和 RViz 可视化：

```bash
# 构建并激活环境
cd ~/Robotics/qnbot_demo
colcon build
source install/setup.bash

# 一键启动完整 Demo
ros2 launch qnbot_teleoperator demo_full.launch.py
```

这将同时启动：
- **WebSocket 服务器**: `0.0.0.0:19091` - 接收外骨骼数据
- **RViz 可视化**: 显示外骨骼模型

### RViz 可视化启动

要单独启动外骨骼的 RViz 可视化界面：

```bash
# 启动 RViz 显示外骨骼模型（使用外骨骼命令数据）
ros2 launch qnbot_teleoperator exoskeleton_display.launch.py

# 启动 RViz 并使用 GUI 滑条手动控制关节
# （不需要连接外骨骼，适合测试）
ros2 launch qnbot_teleoperator exoskeleton_display.launch.py source:=gui

# 禁用 RViz（仅启动数据处理）
ros2 launch qnbot_teleoperator exoskeleton_display.launch.py rviz:=false
```

**启动参数说明：**
| 参数 | 默认值 | 说明 |
|------|--------|------|
| `rviz` | `true` | 是否启动 RViz 可视化 |
| `source` | `exo_command` | 数据源: `gui`(手动控制) 或 `exo_command`(外骨骼命令) |
| `namespace` | `exoskeleton` | ROS名空间，避免与机器人话题冲突 |

**RViz 配置文件位置：** `config/exoskeleton_display.rviz`

---

## 🔧 如何修改适配自己的机器人

本 Demo 包设计为通用模板，您可以通过以下步骤适配自己的机器人：

### 步骤 1：创建配置文件

复制示例配置文件并重命名：

```bash
cd config/
cp retargeting_DEMO.yaml retargeting_<您的机器人名称>.yaml
```

### 步骤 2：修改配置参数

编辑新创建的配置文件，修改以下内容：

```yaml
# 1. 机器人基本信息
robot_info:
  name: "您的机器人名称"
  description: "您的机器人描述"

# 2. 目标机器人关节名称（必须与URDF一致）
target_joint_mapping:
  left_arm:
    joint_names:
      - 'your_left_shoulder_pitch'  # 替换为您的关节名
      - 'your_left_shoulder_roll'
      # ...

# 3. Retargeting 参数（根据实际测试调整）
retargeting_params:
  # 缩放因子（方向和比例）
  scaling_factors:
    left_arm:  [1.0, -1.0, 1.0, 1.0, 1.0]  # 负值表示方向相反
    
  # 偏移角度（补偿零位差异）
  offset_angles:
    left_arm:  [-1.57, 0.0, 0.0, 0.0, 0.0]  # 弧度
    
  # 关节限位（从URDF获取）
  joint_limits:
    left_arm:
      lower: [-2.96, -0.61, -2.61, -2.19, -2.61]
      upper: [2.79, 2.35, 2.61, 0.73, 2.61]
```

### 步骤 3：更新 setup.py

在 `setup.py` 中添加您的配置文件：

```python
('share/' + package_name + '/config', [
    'config/qnbot_teleoperator_config.yaml',
    'config/exoskeleton_display.rviz',
    'config/retargeting_DEMO.yaml',
    'config/retargeting_<您的机器人名称>.yaml',  # 添加这行
]),
```

### 步骤 4：启动时指定机器人类型

```bash
# 启动重定向节点时指定机器人类型
ros2 launch qnbot_teleoperator exo_retargeting.launch.py robot_type:=<您的机器人名称>
```

### 步骤 5：验证配置

```bash
# 1. 查看配置是否正确加载
ros2 topic echo /exo_retargeting/status

# 2. 检查重定向后的关节数据
ros2 topic echo /left_arm/joint_command
ros2 topic echo /right_arm/joint_command

# 3. 用 RViz 可视化验证运动是否正确
```

### 配置文件关键参数说明

| 参数 | 说明 | 如何获取 |
|------|------|----------|
| `target_joint_mapping` | 目标机器人关节名称 | 从URDF文件中获取 |
| `scaling_factors` | 关节方向和比例 | 实际测试调整 |
| `offset_angles` | 零位偏移角度 | 比较外骨骼和机器人零位差异 |
| `joint_limits` | 关节限位 | 从URDF文件中获取 |

---

## 功能架构 🚀

### 数据流架构
```
外骨骼设备 --WebSocket--> websocket_teleoperator --/exo/*话题--> 下游控制系统
                        (数据接收+控制逻辑)                  
```

### 功能说明
- **`websocket_teleoperator`**：数据接收、按钮控制、回零流程、发布到专用话题 `/exo/joint_command` 和 `/exo/gamepad_keys`

## 功能特点

- **100Hz 高频率实时数据发布**：确保数据实时性
- **智能控制逻辑**：按钮启停、回零流程、摇杆数据处理
- **扳机数据作为关节数据**：直接映射到关节命令
- **进程内通信优化**：减少消息复制延迟
- **安全保护机制**：关节限位保护和数据超时保护
- **外骨骼数据标准化**：统一的数据格式发布

## 系统架构

```
外骨骼设备 
    |
    | (WebSocket)
    v
websocket_teleoperator
    | - 按钮控制逻辑
    | - 回零流程控制  
    | - 扳机映射
    | - 安全检查
    | - 发布标准化数据
    |
    | (/exo/joint_command - 16个关节)
    | (/exo/gamepad_keys - 摇杆+控制状态)
    v
下游控制系统
```

## 节点说明

### WebSocket 远程控制器 (`websocket_teleoperator`)
**功能**：接收外骨骼数据，处理控制逻辑，发布到 `/exo/*` 话题

**控制功能**：
- ✅ 按钮点击检测和防抖
- ✅ 左右臂控制启停
- ✅ 双边长按回零功能
- ✅ 扳机数据映射关节
- ✅ 关节限位保护
- ✅ 数据超时保护
- ✅ WebSocket连接管理

**配置参数**：
- `enable_left_arm`: 是否接收左臂数据
- `enable_right_arm`: 是否接收右臂数据
- `enable_left_joystick`: 是否接收左手柄数据
- `enable_right_joystick`: 是否接收右手柄数据
- `enable_vehicle_control`: 是否启用小车和滑台控制

**发布话题**：
- `/exo/joint_command`：外骨骼关节数据（16个关节：左臂7个+右臂7个+左扳机+右扳机）
- `/exo/gamepad_keys`：摇杆数据和控制状态

**数据格式**：
- **关节命令** (`sensor_msgs/JointState`)：
  - `position[0-6]`：左臂关节位置 (弧度)
  - `position[7-13]`：右臂关节位置 (弧度)  
  - `position[14]`：左扳机位置 (0-1)
  - `position[15]`：右扳机位置 (0-1)

- **摇杆数据** (`sensor_msgs/Joy`)：
  - `axes[0-3]`：左右摇杆X/Y轴数据 (-1到1)
  - `buttons[0-3]`：控制状态位

## 启动方法

### 方法 1：分别启动（推荐）

```bash
# 终端1：启动WebSocket远程控制器（数据接收+控制逻辑）
ros2 launch qnbot_teleoperator websocket_teleoperator.launch.py

# 终端2：启动机械臂控制器（数据转发+运动计算）
ros2 launch qnbot_teleoperator websocket_controller.launch.py arm_mode:=both
```

### 方法 2：控制器配置选项

```bash
# 双臂控制（默认）
ros2 launch qnbot_teleoperator websocket_teleoperator.launch.py

# 仅左臂
ros2 launch qnbot_teleoperator websocket_teleoperator.launch.py enable_left_arm:=true enable_right_arm:=false

# 仅右臂  
ros2 launch qnbot_teleoperator websocket_teleoperator.launch.py enable_left_arm:=false enable_right_arm:=true

# 禁用小车和滑台控制
ros2 launch qnbot_teleoperator websocket_teleoperator.launch.py enable_vehicle_control:=false

# 禁用左手柄
ros2 launch qnbot_teleoperator websocket_teleoperator.launch.py enable_left_joystick:=false
```

### 方法 3：手动启动可执行文件

```bash
# 启动WebSocket外骨骼数据接收器
ros2 run qnbot_teleoperator websocket_teleoperator
```

## 数据格式

### `/exo/joint_command` 话题（16个关节）

```yaml
header:
  stamp: {当前时间戳}
  frame_id: ''
name: 
  - 'left_arm_joint_1'     # 索引 0
  - 'left_arm_joint_2'     # 索引 1
  - 'left_arm_joint_3'     # 索引 2
  - 'left_arm_joint_4'     # 索引 3
  - 'left_arm_joint_5'     # 索引 4
  - 'left_arm_joint_6'     # 索引 5
  - 'left_arm_joint_7'     # 索引 6
  - 'right_arm_joint_1'    # 索引 7
  - 'right_arm_joint_2'    # 索引 8
  - 'right_arm_joint_3'    # 索引 9
  - 'right_arm_joint_4'    # 索引 10
  - 'right_arm_joint_5'    # 索引 11
  - 'right_arm_joint_6'    # 索引 12
  - 'right_arm_joint_7'    # 索引 13
  - 'left_trigger_joint'   # 索引 14 (扳机→夹爪)
  - 'right_trigger_joint'  # 索引 15 (扳机→夹爪)
position: [16个关节位置值]
velocity: [16个0值]
effort: [16个0值]
```

### `/exo/gamepad_keys` 话题（新格式）

```yaml
header:
  stamp: {当前时间戳}
  frame_id: ''
axes:
  - {左摇杆X}      # 索引 0 (原始数据，-1.0 ~ 1.0)
  - {左摇杆Y}      # 索引 1 (原始数据，-1.0 ~ 1.0)
  - {右摇杆X}      # 索引 2 (原始数据，-1.0 ~ 1.0)
  - {右摇杆Y}      # 索引 3 (原始数据，-1.0 ~ 1.0)
buttons:
  - {双臂控制状态}     # 索引 0 (1=启用, 0=禁用)
  - {摇杆控制状态}     # 索引 1 (1=启用, 0=禁用)
  - {回零状态}        # 索引 2 (1=回零中, 0=正常)
  - {小车滑台控制权限} # 索引 3 (1=启用, 0=禁用)
```

## 控制逻辑

### 按钮控制
- **双侧同时长按1-10秒松开**：切换双臂控制 + 摇杆控制
- **双侧同时长按>10秒松开**：回零（仅在控制禁用时）
- **防抖保护**：按钮需按下至少50ms
- **初始状态**：所有控制禁用
- **容错机制**：双边松开允许200ms时间差

### 回零功能
- **触发条件**：所有控制禁用时，双边长按超过10秒
- **回零顺序**：关节2→关节1→关节3-7→夹爪
- **时间间隔**：每个关节1秒间隔
- **并行执行**：左右臂同时回零
- **总耗时**：约8秒

### 摇杆控制（由转发器处理）
- **左摇杆Y轴**：小车前进/后退（最大0.3m/s）
- **右摇杆X轴**：小车左转/右转（最大1.0rad/s）
- **右摇杆Y轴**：滑台上升/下降（最大0.15m/s）
- **死区范围**：±0.1（在转发器中应用）
- **权限检查**：基于按钮状态决定是否响应

### 扳机控制
- **数据范围**：700(松开) ~ 2700(夹紧)
- **映射范围**：-0.0120m(张开) ~ 0.0m(闭合)
- **作为关节**：左扳机→索引14，右扳机→索引15

## 安全特性

### 关节限位保护
所有关节都有严格的角度限制：
```
关节1: -135° ~ +135°    关节5: -120° ~ +120°
关节2: -105° ~ +105°    关节6: -90° ~ +90°  
关节3: -120° ~ +120°    关节7: -100° ~ +70°
关节4: -92.5° ~ +92.5°
```

### 夹爪限制
```
夹爪位置: -0.0120m (完全张开) ~ 0.0m (完全闭合)
```

### 数据超时保护
- 当 WebSocket 连接断开或数据超时时，自动发布默认安全位置
- 超时时间：100ms（可配置）
- **只有WebSocket连接时才发布数据**

### 摇杆数据标准化
- 原始摇杆数据范围：-1.0 ~ 1.0
- 防止摇杆漂移的后续处理由下游系统负责

## 调试命令

### 监控数据流
```bash
# 查看话题列表
ros2 topic list

# 监控外骨骼数据
ros2 topic echo /exo/joint_command
ros2 topic echo /exo/gamepad_keys

# 检查节点状态
ros2 node list
ros2 node info /websocket_teleoperator

# 检查话题频率
ros2 topic hz /exo/joint_command
ros2 topic hz /exo/gamepad_keys
```

### 测试数据发布
```bash
# 验证发布的关节数据格式
ros2 topic echo /exo/joint_command --once

# 验证发布的摇杆数据格式  
ros2 topic echo /exo/gamepad_keys --once
```

## 故障排除

### 常见问题

1. **WebSocket连接失败**
   - 检查防火墙设置
   - 确认端口19091未被占用
   - 查看日志：`ros2 topic echo /rosout`

2. **数据发布异常**
   - 检查话题发布：`ros2 topic hz /exo/joint_command`
   - 查看控制状态：`ros2 topic echo /exo/gamepad_keys`

3. **控制被禁用**
   - 按对应摇杆按钮激活控制
   - 检查按钮状态：`ros2 topic echo /exo/gamepad_keys`

4. **下游系统未收到数据**
   - 确认 websocket_teleoperator 正在运行
   - 检查话题连接：`ros2 topic info /exo/joint_command`

## 开发说明

### 架构优势
1. **专注数据接收**：专门负责外骨骼数据的接收和标准化
2. **易于调试**：可以独立验证数据接收功能
3. **高性能**：原始数据传输，最小化处理开销
4. **易扩展**：下游可以连接多个不同的控制系统
5. **标准化输出**：统一的数据格式便于集成

### 添加新功能
1. **控制逻辑**：在 `websocket_teleoperator.py` 中修改
2. **数据格式**：修改发布话题的消息格式
3. **新话题**：添加新的发布话题支持新功能

### 性能优化
- 原始数据传输减少计算开销
- 100Hz高频率发布确保实时性
- 专注数据接收提高系统稳定性