# Qnbot Exoskeleton Teleoperator Demo

This package provides a solution for receiving exoskeleton data via WebSocket and publishing to dedicated ROS2 topics.

## Quick Start

### One-click Launch (Recommended)

Start WebSocket listener and RViz visualization simultaneously:

```bash
# Build and source environment
cd ~/Robotics/qnbot_demo
colcon build
source install/setup.bash

# One-click launch full demo
ros2 launch qnbot_teleoperator demo_full.launch.py
```

This will start:
- **WebSocket Server**: `0.0.0.0:19091` - Receives exoskeleton data
- **RViz Visualization**: Displays exoskeleton model

### RViz Visualization Launch

To launch the exoskeleton RViz visualization independently:

```bash
# Launch RViz with exoskeleton model (using exoskeleton command data)
ros2 launch qnbot_teleoperator exoskeleton_display.launch.py

# Launch RViz with GUI sliders to manually control joints
# (No exoskeleton connection needed, useful for testing)
ros2 launch qnbot_teleoperator exoskeleton_display.launch.py source:=gui

# Disable RViz (data processing only)
ros2 launch qnbot_teleoperator exoskeleton_display.launch.py rviz:=false
```

**Launch Parameters:**
| Parameter | Default | Description |
|-----------|---------|-------------|
| `rviz` | `true` | Whether to launch RViz visualization |
| `source` | `exo_command` | Data source: `gui` (manual control) or `exo_command` (exoskeleton command) |
| `namespace` | `exoskeleton` | ROS namespace to avoid conflicts with robot topics |

**RViz Config File:** `config/exoskeleton_display.rviz`

---

## How to Adapt for Your Robot

This demo package is designed as a universal template. Follow these steps to adapt it for your robot:

### Step 1: Create Configuration File

Copy and rename the sample configuration file:

```bash
cd config/
cp retargeting_DEMO.yaml retargeting_<your_robot_name>.yaml
```

### Step 2: Modify Configuration Parameters

Edit the newly created configuration file:

```yaml
# 1. Robot basic info
robot_info:
  name: "Your Robot Name"
  description: "Your robot description"

# 2. Target robot joint names (must match URDF)
target_joint_mapping:
  left_arm:
    joint_names:
      - 'your_left_shoulder_pitch'  # Replace with your joint names
      - 'your_left_shoulder_roll'
      # ...

# 3. Retargeting parameters (adjust based on actual testing)
retargeting_params:
  # Scaling factors (direction and ratio)
  scaling_factors:
    left_arm:  [1.0, -1.0, 1.0, 1.0, 1.0]  # Negative = opposite direction
    
  # Offset angles (compensate for zero-position differences)
  offset_angles:
    left_arm:  [-1.57, 0.0, 0.0, 0.0, 0.0]  # radians
    
  # Joint limits (from 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]
```

### Step 3: Update setup.py

Add your configuration file to `setup.py`:

```python
('share/' + package_name + '/config', [
    'config/qnbot_teleoperator_config.yaml',
    'config/exoskeleton_display.rviz',
    'config/retargeting_DEMO.yaml',
    'config/retargeting_<your_robot_name>.yaml',  # Add this line
]),
```

### Step 4: Specify Robot Type at Launch

```bash
# Specify robot type when launching retargeting node
ros2 launch qnbot_teleoperator exo_retargeting.launch.py robot_type:=<your_robot_name>
```

### Step 5: Verify Configuration

```bash
# 1. Check if config is loaded correctly
ros2 topic echo /exo_retargeting/status

# 2. Check retargeted joint data
ros2 topic echo /left_arm/joint_command
ros2 topic echo /right_arm/joint_command

# 3. Visualize in RViz to verify motion correctness
```

### Key Configuration Parameters

| Parameter | Description | How to Obtain |
|-----------|-------------|---------------|
| `target_joint_mapping` | Target robot joint names | From URDF file |
| `scaling_factors` | Joint direction and ratio | Adjust through testing |
| `offset_angles` | Zero-position offset angles | Compare exo and robot zero positions |
| `joint_limits` | Joint limits | From URDF file |

---

## Architecture

### Data Flow
```
Exoskeleton Device --WebSocket--> websocket_teleoperator --/exo/* topics--> Downstream Control System
                                  (Data reception + control logic)                  
```

### Features
- **`websocket_teleoperator`**: Data reception, button control, homing sequence, publishes to dedicated topics `/exo/joint_command` and `/exo/gamepad_keys`

## Key Features

- **100Hz high-frequency real-time data publishing**: Ensures data timeliness
- **Intelligent control logic**: Button start/stop, homing sequence, joystick data processing
- **Trigger data as joint data**: Direct mapping to joint commands
- **In-process communication optimization**: Reduces message copy latency
- **Safety protection**: Joint limit protection and data timeout protection
- **Standardized exoskeleton data**: Unified data format publishing

## Node Description

### WebSocket Teleoperator (`websocket_teleoperator`)
**Function**: Receives exoskeleton data, processes control logic, publishes to `/exo/*` topics

**Control Features**:
- Button click detection with debounce
- Left/right arm control start/stop
- Dual-side long-press homing
- Trigger-to-joint mapping
- Joint limit protection
- Data timeout protection
- WebSocket connection management

**Configuration Parameters**:
- `enable_left_arm`: Whether to receive left arm data
- `enable_right_arm`: Whether to receive right arm data
- `enable_left_joystick`: Whether to receive left joystick data
- `enable_right_joystick`: Whether to receive right joystick data
- `enable_vehicle_control`: Whether to enable vehicle and slider control

**Published Topics**:
- `/exo/joint_command`: Exoskeleton joint data (16 joints: left arm 7 + right arm 7 + left trigger + right trigger)
- `/exo/gamepad_keys`: Joystick data and control states

**Data Format**:
- **Joint Command** (`sensor_msgs/JointState`):
  - `position[0-6]`: Left arm joint positions (radians)
  - `position[7-13]`: Right arm joint positions (radians)
  - `position[14]`: Left trigger position (0-1)
  - `position[15]`: Right trigger position (0-1)

- **Joystick Data** (`sensor_msgs/Joy`):
  - `axes[0-3]`: Left/right joystick X/Y axis data (-1 to 1)
  - `buttons[0-3]`: Control state bits

## Launch Methods

### Method 1: Separate Launch (Recommended)

```bash
# Terminal 1: Start WebSocket teleoperator (data reception + control logic)
ros2 launch qnbot_teleoperator websocket_teleoperator.launch.py
```

### Method 2: Configuration Options

```bash
# Dual-arm control (default)
ros2 launch qnbot_teleoperator websocket_teleoperator.launch.py

# Left arm only
ros2 launch qnbot_teleoperator websocket_teleoperator.launch.py enable_left_arm:=true enable_right_arm:=false

# Right arm only
ros2 launch qnbot_teleoperator websocket_teleoperator.launch.py enable_left_arm:=false enable_right_arm:=true

# Disable vehicle and slider control
ros2 launch qnbot_teleoperator websocket_teleoperator.launch.py enable_vehicle_control:=false

# Disable left joystick
ros2 launch qnbot_teleoperator websocket_teleoperator.launch.py enable_left_joystick:=false
```

### Method 3: Run Executable Directly

```bash
# Start WebSocket exoskeleton data receiver
ros2 run qnbot_teleoperator websocket_teleoperator
```

## Control Logic

### Button Control
- **Both sides long-press 1-10s then release**: Toggle dual-arm control + joystick control
- **Both sides long-press >10s then release**: Homing (only when control is disabled)
- **Debounce protection**: Button must be pressed for at least 50ms
- **Initial state**: All controls disabled
- **Tolerance**: 200ms time difference allowed for dual-side release

### Homing Feature
- **Trigger condition**: All controls disabled, both sides long-press > 10 seconds
- **Homing order**: Joint 2 → Joint 1 → Joints 3-7 → Gripper
- **Time interval**: 1 second per joint
- **Parallel execution**: Left and right arms home simultaneously
- **Total duration**: ~8 seconds

### Joystick Control (Handled by Forwarder)
- **Left joystick Y-axis**: Vehicle forward/backward (max 0.3m/s)
- **Right joystick X-axis**: Vehicle left/right turn (max 1.0rad/s)
- **Right joystick Y-axis**: Slider up/down (max 0.15m/s)
- **Deadzone**: ±0.1 (applied in forwarder)
- **Permission check**: Based on button state

### Trigger Control
- **Data range**: 700 (released) ~ 2700 (clamped)
- **Mapped range**: -0.0120m (open) ~ 0.0m (closed)
- **As joint**: Left trigger → index 14, Right trigger → index 15

## Safety Features

### Joint Limit Protection
All joints have strict angle limits:
```
Joint 1: -135° ~ +135°    Joint 5: -120° ~ +120°
Joint 2: -105° ~ +105°    Joint 6: -90° ~ +90°  
Joint 3: -120° ~ +120°    Joint 7: -100° ~ +70°
Joint 4: -92.5° ~ +92.5°
```

### Gripper Limits
```
Gripper position: -0.0120m (fully open) ~ 0.0m (fully closed)
```

### Data Timeout Protection
- When WebSocket connection is lost or data times out, automatically publishes default safe position
- Timeout: 100ms (configurable)
- **Only publishes data when WebSocket is connected**

## Debug Commands

### Monitor Data Flow
```bash
# List topics
ros2 topic list

# Monitor exoskeleton data
ros2 topic echo /exo/joint_command
ros2 topic echo /exo/gamepad_keys

# Check node status
ros2 node list
ros2 node info /websocket_teleoperator

# Check topic frequency
ros2 topic hz /exo/joint_command
ros2 topic hz /exo/gamepad_keys
```

### Test Data Publishing
```bash
# Verify published joint data format
ros2 topic echo /exo/joint_command --once

# Verify published joystick data format
ros2 topic echo /exo/gamepad_keys --once
```

## Troubleshooting

### Common Issues

1. **WebSocket connection failed**
   - Check firewall settings
   - Confirm port 19091 is not occupied
   - Check logs: `ros2 topic echo /rosout`

2. **Data publishing anomaly**
   - Check topic publishing: `ros2 topic hz /exo/joint_command`
   - Check control state: `ros2 topic echo /exo/gamepad_keys`

3. **Control is disabled**
   - Press corresponding joystick button to activate control
   - Check button state: `ros2 topic echo /exo/gamepad_keys`

4. **Downstream system not receiving data**
   - Confirm websocket_teleoperator is running
   - Check topic connection: `ros2 topic info /exo/joint_command`
