acRealman_xr/CODEX.md

# CODEX.md

This file provides guidance to Codex, Claude Code, and other coding agents when working with this repository. Higher-priority system or developer instructions override this file.

## Project Overview

This repository is the `src/` layer of a ROS2 Humble workspace for PICO/XR controller teleoperation of left and right RealMan RM75 arms.

The core behavior is relative Cartesian pose streaming:

```text
PICO/XR UDP JSON
  -> xr_rm_input/udp_controller_receiver
  -> /xr/left_controller and /xr/right_controller
  -> xr_rm_teleop/single_arm_velocity_teleop
  -> MockRealManAdapter or RealManAdapter
  -> /xr_rm/<arm_name>/current_pose
  -> /xr_rm/<arm_name>/raw_target_pose
  -> /xr_rm/<arm_name>/target_pose
  -> /xr_rm/<arm_name>/cmd_vel
  -> /xr_rm/<arm_name>/target_clamped
```

When `grip=true`, the first valid frame locks both the XR controller origin and the robot TCP origin. Later controller translation and rotation deltas are converted into target TCP poses and sent through `rm_movep_canfd`. When `grip=false`, `pose_valid=false`, UDP data times out, an adapter exception occurs, or the node shuts down, motion must stop.

## Architecture

The project consists of:

- **Interface package** (`xr_rm_interfaces`): defines the `XrController` message.
- **Input package** (`xr_rm_input`): receives UDP controller JSON, normalizes controller payloads, publishes left/right XR controller topics, and provides `sample_udp_sender` for mock/debug input.
- **Teleop package** (`xr_rm_teleop`): maps relative XR controller motion to RM75 Cartesian target poses and dispatches commands through mock or real adapters.
- **Bringup package** (`xr_rm_bringup`): owns launch files, arm YAML configuration, and the local launcher UI.
- **Unity/PICO sender** (`unity/XR_RM_PICO_UDP_Sender`): PICO 4 Ultra Unity project that sends controller pose, validity, source, tracking status, sequence, and timestamp fields over UDP.

Key control facts:

- Main launch file: `xr_rm_bringup/launch/arm_debug.launch.py`
- Supported launch arm modes: `arm:=left|right|both`
- Supported adapter modes: `use_mock:=true|false`
- ROS2 workspace root: `/home/robot/WS_xr`
- Repository/source root: `/home/robot/WS_xr/src`
- `cmd_vel` is a debug estimate of target-pose change rate, not the real robot command topic.

## Build Commands

Run ROS2 commands from the workspace root:

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
rosdep install --from-paths src -y --ignore-src
colcon build --symlink-install
source install/setup.bash
```

Git inspection commands:

```bash
cd /home/robot/WS_xr/src
git status --short
git diff
git diff --check
```

## Run Commands

### Mock Dual-Arm Debug

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
```

In another terminal:

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 \
  --pattern axis_sweep --seconds 60 --both-mode staggered
```

Use `--rotation-pattern rpy_steps` when checking orientation mapping.

### Real Arm Debug

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
```

Dual-arm real hardware:

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
  left_robot_ip:=192.168.192.18 \
  right_robot_ip:=192.168.192.19
```

Launcher UI:

```bash
python3 src/xr_rm_bringup/tools/launcher_ui.py
```

## Debug Topics

```bash
ros2 topic echo /xr/left_controller
ros2 topic echo /xr/right_controller
ros2 topic echo /xr_rm/left_rm75/target_pose
ros2 topic echo /xr_rm/right_rm75/target_pose
ros2 topic echo /xr_rm/left_rm75/cmd_vel
ros2 topic echo /xr_rm/right_rm75/cmd_vel
```

## UDP Protocol

Preferred Unity packets contain top-level `controllers.left` and `controllers.right` objects plus `t`, `source_time`, `seq`, and `frame_id`.

Each controller payload should include:

- `grip`
- `trigger`
- `pos[3]`
- `quat[4]`
- `pose_valid`
- `pose_source`
- `tracking_state`
- `controller_status`
- `grip_value`
- `axis[2]`
- `buttons`

The receiver also supports single-controller debug packets with `hand`, `pos`, and `quat`, plus aliases such as `position`, `p`, `pose.position`, `orientation`, and `q`. The default quaternion order is `xyzw`; use `quat_order:=wxyz` only when the sender really emits `wxyz`.

When the PICO HUD shows `invalid none`, expect ROS-side `grip` to be forced false. Debug in this order: Unity pose source, `udp_controller_receiver` warnings, then teleop timeout or clamping topics.

## Coordinate Notes

PICO/OpenXR project coordinates are:

- `+X`: right
- `+Y`: up
- `+Z`: back

Current Unity `Project (+Z back)` output must remain in that project coordinate convention. PXR `pxr_predict` native values are converted to:

```text
project.x = native.z
project.y = native.y
project.z = -native.x
```

`Source raw` is only for field comparison and diagnostics. If `/xr/*_controller.pose.position` already matches the expected PICO/OpenXR coordinates but one arm moves in the wrong robot direction, prefer changing only that arm's YAML `xr_to_robot_matrix`.

## Development Workflow

Use the repository rule file as the project-level source of truth for coding-agent behavior:

- Read relevant code and docs before editing.
- Check `git status --short` before edits.
- Treat existing uncommitted changes as user work; do not revert them unless explicitly requested.
- Before writing files, explain the goal understanding, files to touch, implementation plan, risks, and validation path, then wait for user confirmation.
- Skip the confirmation gate only when the user's current request clearly authorizes direct modification, such as "可以直接修改", "无需确认", "直接执行", or equivalent wording.
- Keep edits small and task-scoped.
- Do not automatically run `git add`, commit, push, force push, delete branches, or merge branches.
- After changes, summarize touched files, behavior changes, validation performed, and remaining risks.

## Safety Rules

This is a real robot teleoperation project. Keep all hardware-related changes conservative.

Do not casually modify:

- Left arm IP: `192.168.192.18`
- Right arm IP: `192.168.192.19`
- RM75 TCP port: `8080`
- Workspace limits
- Cylinder limits
- `xr_to_robot_matrix`
- Initial joint or TCP poses
- Speed and acceleration limits
- End-effector peripheral configuration

Preserve stop behavior for:

- `grip=false`
- `pose_valid=false`
- UDP timeout or stale controller data
- Adapter exceptions
- Node shutdown
- PICO app pause, exit, or disabled sending

Additional safety constraints:

- Keep `move_to_initial_pose_on_connect` defaulting to `false`.
- Do not bypass receiver behavior that forces `grip=false` when `pose_valid=false`.
- Validate hardware-related behavior in this order: static checks, mock mode, single real arm, dual real arms.
- Do not claim dual-arm collision detection exists unless code implements it.

## Unity / PICO Notes

- Current Unity project: `unity/XR_RM_PICO_UDP_Sender`
- Current PICO SDK: `unity/PICO-Unity-Integration-SDK-release_3.4.0`
- Treat the PICO SDK as third-party code unless the user explicitly targets it.
- Treat Unity `Library/`, `Builds/`, `Logs/`, `UserSettings/`, APKs, and generated logs as local/generated artifacts.
- The Unity package depends on TextMeshPro.
- PICO panel fonts come from pregenerated `Assets/Resources/Fonts/Roboto-Regular SDF.asset` and `Roboto-Bold SDF.asset`.
- Do not create TMP font assets dynamically at APK runtime.
- The PICO UDP target IP must be the Ubuntu ROS host IPv4 address on the same LAN.
- PICO app pause, exit, or disabled sending must send `grip=false`.
- Invalid pose data must send `pose_valid=false` and allow the ROS receiver to force stop.

## Documentation Rules

- `README.md` is the main project document for humans.
- `CODEX.md` is the coding-agent workflow and safety document.
- Keep `docs/` for focused setup guides and report materials.
- Do not add scattered Markdown files unless the user asks.
- Do not document unimplemented features as complete.
- Keep mock, real-arm, and Unity/PICO paths explicit.

## Testing and Validation

There is no single full-system automated test that proves real-hardware safety. Prefer layered validation:

- Static checks: syntax, imports, formatting-sensitive checks, `git diff --check`.
- ROS build: `colcon build --symlink-install`.
- Mock validation: launch `arm_debug.launch.py` with `use_mock:=true`.
- UDP validation: run `sample_udp_sender` with `axis_sweep` and, when needed, `rpy_steps`.
- Real hardware validation: single arm first, then dual arm.

## Important Notes

- Do not implement or claim the following unless the user explicitly requests and code actually supports it:
  - XRoboToolkit PC-Service bridge as a required runtime path
  - D405/D435 video streaming or data recording
  - Dual-arm collision detection
  - Autonomous picking state machine
  - QP IK, dexterous-hand retargeting, or whole-body tracker support
  - Full time synchronization
  - Robot state feedback to PICO
- If motion direction is wrong, inspect `/xr/*_controller.pose.position` first before changing robot-side YAML.
- The project should stay understandable enough for field debugging; avoid broad refactors during safety-sensitive fixes.