diff --git a/.ci/entrypoint.sh b/.ci/entrypoint.sh index 4d8b584..ad38fbb 100755 --- a/.ci/entrypoint.sh +++ b/.ci/entrypoint.sh @@ -1,44 +1,14 @@ #!/bin/bash +# Exit immediately if a command exits with a non-zero status. set -e -source "/opt/ros/${ROS_DISTRO:-jazzy}/setup.bash" +source "/opt/ros/humble/setup.bash" -# Rebuild ros2_vive_controller if setup.py has changed (e.g. new entry points) -SETUP_FILE="/ros2_ws/src/ros2_vive_controller/setup.py" -SETUP_HASH_FILE="/ros2_ws/.setup_py_hash" -if [ -f "$SETUP_FILE" ]; then - CURRENT_HASH=$(md5sum "$SETUP_FILE" | awk '{print $1}') - STORED_HASH="" - [ -f "$SETUP_HASH_FILE" ] && STORED_HASH=$(cat "$SETUP_HASH_FILE") - if [ "$CURRENT_HASH" != "$STORED_HASH" ]; then - echo "Detected setup.py change, rebuilding ros2_vive_controller..." - cd /ros2_ws - colcon build --symlink-install --packages-select ros2_vive_controller - echo "$CURRENT_HASH" > "$SETUP_HASH_FILE" - fi -fi if [ -f "/ros2_ws/install/setup.bash" ]; then source "/ros2_ws/install/setup.bash" -fi - -export RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION:-rmw_cyclonedds_cpp} -export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1} - -# Setup steamclient symlinks -mkdir -p /root/.steam/sdk64 /root/.steam/sdk32 -ln -sf /home/steam/.local/share/Steam/steamcmd/linux64/steamclient.so /root/.steam/sdk64/steamclient.so -ln -sf /home/steam/.local/share/Steam/steamcmd/linux32/steamclient.so /root/.steam/sdk32/steamclient.so - -# Start vrserver directly (no compositor/monitor needed for tracker-only use) -STEAMVR_PATH="/home/steam/Steam/steamapps/common/SteamVR" -export LD_LIBRARY_PATH="$STEAMVR_PATH/bin/linux64:${LD_LIBRARY_PATH}" - -if [ -d "$STEAMVR_PATH" ]; then - echo "Starting SteamVR vrserver..." - "$STEAMVR_PATH/bin/linux64/vrserver" --keepalive & - sleep 3 - echo "SteamVR vrserver started." + # Optional: Log that the workspace was found + # echo "āœ… Workspace sourced." fi exec "$@" diff --git a/.env.template b/.env.template new file mode 100644 index 0000000..35d4327 --- /dev/null +++ b/.env.template @@ -0,0 +1,14 @@ +STEAM_USER=your_steam_username +STEAM_PASSWORD=your_steam_password +REGISTRY=registry.gitlab.inria.fr/eurobin-horizon/code/ros2-vive-controller +TAG=latest + +# General +ROS_DOMAIN_ID=2 +LINEAR_SCALE=1.75 # Scale factor for linear position, adjust as needed for your setup + +# Hardware Serials +SERIAL_LEFT=LHR-97752221 # Replace with your left controller's serial number +SERIAL_RIGHT=LHR-4BB3817E # Replace with your right controller's serial number +REFERENCE_LIGHTHOUSE_SERIAL=LHB-2E7D2119 # Replace with your reference lighthouse's serial number (The one that will be used as origin) + diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..bdbdf75 --- /dev/null +++ b/Makefile @@ -0,0 +1,75 @@ +# --- Configuration --- +# Load variables from .env if it exists +ifneq ("$(wildcard .env)","") + include .env + export $(shell sed 's/=.*//' .env) +endif + +# Default shell +SHELL := /bin/bash + +# --- Phony Targets --- +.PHONY: help gui-perms build build-force push franka tiago tiago_pro g1 calibrate identify stop clean + +# --- Help --- +help: ## Show this help message + @echo "Vive Controller Docker Management" + @echo "Usage: make " + @echo "" + @echo "Available commands:" + @awk 'BEGIN {FS = ":.*?## "} /^[a-zA-Z_-]+:.*?## / {printf " \033[36m%-15s\033[0m %s\n", $$1, $$2}' $(MAKEFILE_LIST) + +# --- GUI Permissions --- +gui-perms: ## Set X11 permissions for Docker UI (used internally) + @echo "Setting X11 permissions for Docker..." + @xhost + > /dev/null + +# --- Build Targets --- +build: ## Build the 'app' image (cached) + docker compose build + +build-force: ## Build from scratch (no cache) + docker compose build --no-cache + +push: ## Push built images to the registry + @echo "Pushing images to the configured registry..." + docker compose push + +pull: ## Pull the latest pre-built images from the registry + @echo "Pulling latest images..." + docker compose pull + +# --- Robot Missions --- +franka: gui-perms ## Start Franka (Single Right Controller) + docker compose --profile franka up + +tiago: gui-perms ## Start Tiago Dual Arm + docker compose --profile tiago up + +tiago_pro: gui-perms ## Start Tiago Pro Dual Arm + docker compose --profile tiago_pro up + +g1: gui-perms ## Start G1 Dual Arm + docker compose --profile g1 up + +# --- Utilities --- +calibrate: gui-perms ## Run workspace calibration + # Runs calibration inside the franka container context + docker compose --profile franka run --rm franka ros2 launch ros2_vive_controller calibration.launch.py + +identify: ## Vibrate controllers to check IDs + @echo "Vibrating RIGHT controller..." + -docker exec -it ros2_vive_franka /entrypoint.sh ros2 service call /vive/right/identify std_srvs/srv/Trigger || \ + docker exec -it ros2_vive_tiago /entrypoint.sh ros2 service call /vive/right/identify std_srvs/srv/Trigger || \ + docker exec -it ros2_vive_tiago_pro /entrypoint.sh ros2 service call /vive/right/identify std_srvs/srv/Trigger || \ + docker exec -it ros2_vive_g1 /entrypoint.sh ros2 service call /vive/right/identify std_srvs/srv/Trigger + @echo "Vibrating LEFT controller..." + -docker exec -it ros2_vive_tiago_pro /entrypoint.sh ros2 service call /vive/left/identify std_srvs/srv/Trigger || \ + docker exec -it ros2_vive_g1 /entrypoint.sh ros2 service call /vive/left/identify std_srvs/srv/Trigger + +stop: ## Stop all running vive containers + docker compose down + +clean: ## Remove all vive containers and networks + docker compose down --remove-orphans --volumes + diff --git a/README.md b/README.md index f276380..cb12608 100644 --- a/README.md +++ b/README.md @@ -1,265 +1,281 @@ # ROS 2 Vive Controller [![License](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) -[![Ros Version](https://img.shields.io/badge/ROS2-Humble-yellow)](https://docs.ros.org/en/humble/index.html) -[![Python](https://img.shields.io/badge/Python-3.10+-blue.svg)](https://www.python.org/) - -
- ROS 2 Vive Controller Logo -
+A unified ROS 2 driver and teleoperation bridge for HTC Vive hardware (Controllers, Trackers 3.0, and Base Stations). This package provides a standalone, heavily optimized Dockerized workflow for workspace calibration, robot teleoperation, and reliable 6-DOF tracking. -A unified ROS 2 driver and teleoperation bridge for HTC Vive hardware (Trackers, Controllers, and Base Stations). This package provides a standalone Dockerized workflow for workspace calibration and robot teleoperation. - -| **Teleoperation** | **Workspace Calibration** | -| :--------------------------------------------------------------: | :-------------------------------------------------------------------: | -| Teleop | Calibration | +| | **Teleoperation in Action** | +| :---: | :---: | +| Logo | Teleop | --- -## Table of Contents -- [ROS 2 Vive Controller](#ros-2-vive-controller) - - [Table of Contents](#table-of-contents) - - [Prerequisites](#prerequisites) - - [Hardware](#hardware) - - [Software](#software) - - [Installation \& Docker](#installation--docker) - - [1. Building the Image](#1-building-the-image) - - [2. Running the Container](#2-running-the-container) - - [āš™ļø The Vive Driver Node](#ļø-the-vive-driver-node) - - [Key Features](#key-features) - - [šŸš€ Launching the Hardware](#-launching-the-hardware) - - [Usage](#usage) - - [Advanced Launch Configuration](#advanced-launch-configuration) - - [šŸ“Š Data Outputs](#-data-outputs) - - [1. Tracking Data](#1-tracking-data) - - [2. Button Mapping (`joint_states`)](#2-button-mapping-joint_states) - - [šŸ› ļø The Teleop Bridge Node](#ļø-the-teleop-bridge-node) - - [Core Logic: Hybrid Relative/Absolute Positioning](#core-logic-hybrid-relativeabsolute-positioning) - - [Reference Frames \& Alignment](#reference-frames--alignment) - - [Key Features](#key-features-1) - - [šŸ¤– Robot Launch Configurations](#-robot-launch-configurations) - - [1. TIAGo (Mobile Manipulator)](#1-tiago-mobile-manipulator) - - [2. Unitree G1 (Humanoid)](#2-unitree-g1-humanoid) - - [āŒØļø Usage](#ļø-usage) +## šŸ“‹ Table of Contents +- [šŸ› ļø Prerequisites](#ļø-prerequisites) +- [āš™ļø Configuration](#ļø-configuration) +- [šŸš€ Quick Start & Deployment](#-quick-start--deployment) +- [āš™ļø The Vive Driver Node (`vive_node.py`)](#ļø-the-vive-driver-node-vive_nodepy) +- [šŸ› ļø The Teleop Bridge Node (`teleop_bridge_node.py`)](#ļø-the-teleop-bridge-node-teleop_bridge_nodepy) +- [šŸ¤– Adding Your Own Robot (Custom Launch Files)](#-adding-your-own-robot-custom-launch-files) --- -## Prerequisites +## šŸ› ļø Prerequisites ### Hardware * **HTC Vive Base Stations** (Lighthouse): At least one is mandatory for tracking. -* **HTC Vive Controller** or **Tracker**: For teleoperation and calibration. -* **SteamVR Compatible Dongle** (or HMD): Required to connect the devices to the PC. +* **HTC Vive Controllers** or **Vive Trackers (3.0)**. +* **Micro USB to USB-A Cables**: For connecting the controllers to the host machine during setup and calibration. ### Software -* **Linux** (Ubuntu 22.04 recommended) -* **Docker** & **NVIDIA Container Toolkit** -* **Python 3** (for build scripts) -* **Steam Account** (Username and Password required for headless SteamVR installation) +* **Docker** +* **Make** (`sudo apt install make`). +* **Steam Account**: A valid username and password are required to download the headless SteamVR server during the initial Docker build. --- -## Installation & Docker +## āš™ļø Configuration + +This project relies on a `.env` file to map your specific hardware to the Docker containers. -This project uses a **multi-stage Docker build** to separate the heavy SteamVR dependencies from your daily development code. You do not need to install ROS 2 or SteamVR on your host machine. +1. Copy the template file: + ```bash + cp .env.template .env + ``` -### 1. Building the Image -We provide a Python script to handle the multi-stage build arguments automatically. +2. Open `.env` and fill in your details: -1. **Export your Steam Credentials** (Required to download the headless SteamVR server): - ```bash - export STEAM_USER="your_username" - export STEAM_PASSWORD="your_password" - ``` -2. **Run the Build Script**: - ```bash - # Builds the development image (mounts local code) - python3 scripts/build_docker.py --target dev - ``` +```env + # --- 1. Steam Credentials (For Build) --- + STEAM_USER=your_steam_username + STEAM_PASSWORD=your_steam_password + # --- 2. Network & Scale --- + ROS_DOMAIN_ID=2 + LINEAR_SCALE=1.75 # Adjust to scale operator movement to robot movement + # --- 3. Hardware Serials --- + SERIAL_LEFT=LHR-97752221 # Left Controller/Tracker ID + SERIAL_RIGHT=LHR-4BB3817E # Right Controller/Tracker ID + REFERENCE_LIGHTHOUSE_SERIAL=LHB-2E7D2119 # The master Lighthouse used as the TF origin (0,0,0) + ``` + > **Tip:** Don't know your serial numbers? Run `make identify` once the container is running to vibrate the controllers and verify which is which! + > Otherwise, you can run `sudo dmesg -w | grep "LHR"` for the controllers or `sudo dmesg -w | grep "LHB"` for the base stations while plugging them in to see their serial numbers in real time. -### 2. Running the Container +--- -The `run_docker.py` script handles GPU passthrough, USB device mapping, and ROS 2 network configuration (`ROS_DOMAIN_ID`). It also provides **aliases** for common tasks. +## šŸš€ Quick Start & Deployment -**Basic Usage:** +We use a `Makefile` to simplify interacting with the multi-profile `docker-compose.yml`. You never need to install ROS 2 or SteamVR on your host machine. +### 1. Build or Pull the Image (For Internal Usage) +> āš ļø The pull step can only be used by our team members for now, for safety reasons we do not provide images publicly, since they require steam credentials to build. ```bash -python3 scripts/run_docker.py [COMMAND] --domain [ID] +# Option A (Internal): Pull the pre-built image from the registry +make pull +# Option B (Standard): Build it locally (requires STEAM_USER / STEAM_PASSWORD in .env) +make build ``` -**Available Commands:** -| Command | Description | -| :---------- | :-------------------------------------------------------- | -| `calibrate` | Launches the workspace calibration tool with RViz. | -| `teleop` | Launches the main teleoperation bridge (driver + bridge). | -| `g1` | Launches the G1 Humanoid dual-arm teleop configuration. | -| `tiago` | Launches the Tiago dual-arm teleop configuration. | -| *(empty)* | Starts a generic bash shell inside the container. | - -**Example:** - +### 2. Launch a Robot Mission +The `docker-compose.yml` is divided into specific robot profiles. Simply run the make command for your target hardware. This automatically passes X11 permissions for RViz, injects your `.env` variables, and sets the CycloneDDS network interfaces. ```bash -# Run teleoperation on Domain ID 1 -python3 scripts/run_docker.py teleop --domain 1 - +make franka # Start Franka (Single Right Arm) +make tiago # Start TIAGo (Dual Arm) +make tiago_pro # Start TIAGo Pro (Dual Arm) +make g1 # Start Unitree G1 (Dual Arm Humanoid) ``` ---- - -## āš™ļø The Vive Driver Node - -The driver handles three critical tasks: - -1. **Haptic Safety (The Virtual Fence):** It monitors the controller's position relative to the calibrated `workspace` parameters. If the controller enters the "padding" zone near a wall, it triggers a haptic vibration pulse (2ms) to warn the user. -2. **Jitter Reduction:** It uses a **OneEuro Filter** to smooth out the tracking data, significantly reducing high-frequency jitter in the robot's motion. -3. **Haptic & Logic Separation:** It treats the hardware as a `JointState` source (buttons) and a `PoseStamped` source (tracking). - -### Key Features - -* **OneEuro Filter:** Balances low-latency response with high-speed smoothing using three parameters: `mincutoff`, `beta`, and `dcutoff`. -* **Workspace Markers:** Publishes a semi-transparent red cube to RViz representing the "Safe Zone" defined during calibration. -* **Serialized Hardware:** Binds to specific controllers using their unique hardware serial numbers (e.g., `LHR-4BB3817E`). - ---- - -## šŸš€ Launching the Hardware - -The driver is typically launched via `vive_teleop.launch.py`. This launch file is designed to be robust against OpenVR initialization race conditions. - -### Usage - +### 3. Utilities +Manage the lifecycle and hardware with these helper commands: ```bash -# Run with default serials -python3 scripts/run_docker.py teleop - -# Run with specific hardware serials -python3 scripts/run_docker.py teleop --serial_right "LHR-12345678" - +make identify # Vibrates the connected controllers to verify left/right bindings +make calibrate # Launches the workspace calibration tool with RViz +make stop # Safely shuts down running containers +make clean # Removes all containers and orphans ``` -### Advanced Launch Configuration - -The launch file includes a **2-second TimerAction delay** between starting the left and right controller drivers. This prevents Inter-Process Communication (IPC) conflicts within the OpenVR runtime when multiple nodes attempt to initialize the driver simultaneously. +--- -| Argument | Default | Description | -| -------------------- | -------------- | ----------------------------------------------------- | -| `rviz` | `true` | Automatically launches RViz with a predefined config. | -| `serial_left` | `LHR-97752221` | Hardware ID for the left-hand controller. | -| `serial_right` | `LHR-4BB3817E` | Hardware ID for the right-hand controller. | -| `tracking_reference` | `LHB-DFA5BD2C` | The Base Station used as the world origin. | -I have added a detailed breakdown of the `joint_states` topic to the **Data Outputs** section. This explains exactly which index corresponds to which physical button, matching the code you provided. +## āš™ļø The Vive Driver Node (`vive_node.py`) +The driver is the core interface between the OpenVR runtime and ROS 2. It handles hardware tracking, filtering, safety limits, and coordinate transformations. ---- +### šŸŒŸ Key Features -### šŸ“Š Data Outputs +* **Dynamic Lighthouse Origin (Stable TF Tree):** Standard SteamVR uses an invisible, arbitrary "Standing Universe" origin. This node allows you to specify a `reference_lighthouse_serial`. If found, the node automatically computes the math to make that physical Lighthouse the exact $(0,0,0)$ origin of your TF tree. It even applies an automatic 180Ā° Z-axis flip to correct standard Lighthouse hardware alignments. +* **Hot-Plug Polling:** Uses continuous OpenVR event polling. If a tracker or lighthouse is turned on *after* the Docker container starts, the node will gracefully detect it, establish the TF tree, and begin vibrating/tracking without needing a restart. +* **Virtual Safety Fence (Haptics):** Monitors the controller's position against calibrated workspace bounds (`workspace.x_min`, etc.). If the hardware enters the padding zone, it triggers immediate 2ms haptic pulses to physically warn the operator. +* **OneEuro Filtering:** Implements a high-speed, low-latency OneEuro filter to eliminate high-frequency hand jitter before it reaches the robot arm. -Each driver node (Left/Right) publishes to its own namespace. +### šŸ“Š Data Outputs & TF Tree -#### 1. Tracking Data +The driver automatically broadcasts a standard `tf2` tree: +`vive_world` āž” `lighthouse_origin` (if configured) āž” `vive_right_link` -| Topic | Type | Description | -| ---------------------------- | --------------------------- | ---------------------------------------- | -| `vive/left/pose` | `geometry_msgs/PoseStamped` | Filtered 6-DOF position and orientation. | -| `vive/left/workspace_marker` | `visualization_msgs/Marker` | The visual boundary box in RViz. | +It also publishes the following topics in its designated namespace (e.g., `/vive/right/`): -#### 2. Button Mapping (`joint_states`) +| Topic | Type | Description | +| :--- | :--- | :--- | +| `pose` | `geometry_msgs/PoseStamped` | Filtered 6-DOF position and orientation. Frame ID matches the Lighthouse if configured. | +| `joint_states` | `sensor_msgs/JointState` | The raw monolithic array of all analog/digital button inputs. | +| `workspace_marker` | `visualization_msgs/Marker` | A semi-transparent red cube for RViz representing the safe tracking zone. | -The driver publishes button inputs as a `sensor_msgs/JointState` message to `vive/left/joint_states`. The `position` array contains the values for the following keys: +*(Note: You can trigger a massive haptic vibration via the `/vive/right/identify` service to figure out which controller you are holding).* -| Index | Name | Type | Range | Description | -| ----- | ------------------ | ------- | -------------- | -------------------------------------------------- | -| **0** | `trigger` | Analog | `0.0` - `1.0` | The index finger trigger. Used for the **Clutch**. | -| **1** | `trackpad_x` | Analog | `-1.0` - `1.0` | Horizontal touch position on the round pad. | -| **2** | `trackpad_y` | Analog | `-1.0` - `1.0` | Vertical touch position on the round pad. | -| **3** | `grip` | Digital | `0.0` / `1.0` | The side grip buttons (squeezing the handle). | -| **4** | `menu` | Digital | `0.0` / `1.0` | The small button above the trackpad. | -| **5** | `trackpad_touched` | Digital | `0.0` / `1.0` | True if the thumb is touching the pad. | -| **6** | `trackpad_pressed` | Digital | `0.0` / `1.0` | True if the trackpad is physically clicked down. | +### šŸ•¹ļø Button Mapping (`joint_states`) +The driver publishes button inputs directly from OpenVR as a standard `JointState` array. The `position` array contains the values for the following keys: +| Index | Name | Type | Range | Description | +| :---: | :--- | :--- | :--- | :--- | +| **0** | `trigger` | Analog | `0.0` - `1.0` | The index finger trigger. Used for the **Clutch**. | +| **1** | `trackpad_x` | Analog | `-1.0` - `1.0` | Horizontal touch position on the round pad. | +| **2** | `trackpad_y` | Analog | `-1.0` - `1.0` | Vertical touch position on the round pad. | +| **3** | `grip` | Digital | `0.0` / `1.0` | The side grip buttons (squeezing the handle). | +| **4** | `menu` | Digital | `0.0` / `1.0` | The small button above the trackpad. | +| **5** | `trackpad_touched` | Digital | `0.0` / `1.0` | True if the thumb is touching the pad. | +| **6** | `trackpad_pressed` | Digital | `0.0` / `1.0` | True if the trackpad is physically clicked down. | -| Vive Button Map | -| -------------------------------------------------------------------------------- | -| Vive Controller Frames | +
+ Vive Controller Button Map +
> **Note:** Digital buttons are published as floats (`0.0` for False, `1.0` for True) to maintain consistency within the `JointState` message standard. --- -## šŸ› ļø The Teleop Bridge Node -This node implements a **Clutch Mechanism** (Deadman Switch) to allow for safe, intuitive teleoperation by separating translation and rotation logic. +## šŸ› ļø The Teleop Bridge Node (`teleop_bridge_node.py`) -### Core Logic: Hybrid Relative/Absolute Positioning +This node sits between the raw Vive Driver and your robot's Cartesian controllers. It implements a **Clutch Mechanism** (Deadman Switch) to allow for safe, intuitive teleoperation by separating translation and rotation logic. -To provide the most intuitive experience for the operator, the bridge treats position and orientation differently: +### šŸ§  Core Logic: Hybrid Positioning -1. **Relative Translation (The Mouse Metaphor):** Position is calculated as a delta () from the moment the clutch is engaged. This allows you to "ratchet" the robot's position, moving it large distances through multiple small controller strokes. -2. **Absolute Orientation (The Mirror Metaphor):** Rotation is **not relative**. For intuitive control, the robot's end-effector orientation is mapped to match the controller's orientation directly. This ensures that if you tilt the controller 45Ā°, the robot hand tilts 45Ā°, maintaining a consistent mental map for the operator. +To provide the most intuitive experience, the bridge treats position and orientation differently: -### Reference Frames & Alignment +1. **Relative Translation (The Mouse Metaphor):** Position is calculated as a delta from the moment the clutch (the `trigger` button) is engaged. This allows you to "ratchet" the robot's position, moving it large distances through multiple small hand strokes while staying in a comfortable physical stance. +2. **Absolute Orientation (The Mirror Metaphor):** Rotation is **not relative**. For intuitive control, the robot's end-effector orientation directly mirrors the controller's orientation. +3. **Axis Realignment (`rotation_offset`):** Because a VR controller's physical axes rarely match a robot gripper's axes, the node supports a custom `[Roll, Pitch, Yaw]` offset parameter. It applies this rotation locally using Scipy matrix math so the robot moves exactly how your brain expects it to. -To ensure the robot moves in the direction you expect, the controller's internal axes must be understood. The image below shows the coordinate system of the Vive Controller used by the driver: +### šŸ“ Reference Frames & Alignment +To properly map the VR controller to your robot using `rotation_offset`, you must understand the controller's default OpenVR axes: -| Vive Coordinate System | -| -------------------------------------------------------------------------------- | -| Vive Controller Frames | +
+ Vive Controller Axes +
* **Z-axis:** Points "out" from the controller tip. * **X-axis:** Points to the right side of the controller. * **Y-axis:** Points "up" through the trackpad. -**Teleoperation Workflow:** +### šŸŽ® Button Demultiplexing (`PointStamped`) -1. **Idle State:** The bridge ignores controller movement. -2. **Activation (Clutch):** Press the `trigger`. The node saves the current EE position as a reference point. -3. **Execution:** Move the controller. The robot translates based on your hand's displacement and rotates to mirror your hand's orientation. -4. **Repositioning:** Release the trigger. Your hand can move freely while the robot stays locked in its current pose. +Standard ROS 2 robot controllers rarely accept monolithic `JointState` arrays for simple commands like opening a gripper. -### Key Features +The Teleop Bridge automatically "demultiplexes" the VR buttons. You can map any Vive button to its own dedicated topic in the launch file. The bridge publishes these as `geometry_msgs/PointStamped` messages (storing the analog/digital value in the `point.x` field), which is the standard format expected by many ROS 2 action servers and bridges. -* **PointStamped Button Mapping:** All analog and digital button data is published as `geometry_msgs/PointStamped` (with the value in the `.x` field). This allows for high-compatibility with diverse robot control stacks. -* **TF2 Integration:** Uses standard ROS 2 transform lookups to anchor movement to the robot's coordinate system (e.g., `base_link` or `pelvis`). -* **Frequency Control:** Can be set to a fixed rate (e.g., 30 Hz) or event-driven mode (`-1.0`) where it publishes only when new data arrives. - ---- +### šŸ›”ļø Trackpad Safety -## šŸ¤– Robot Launch Configurations - -The package includes pre-configured launch files for complex platforms, demonstrating how to remap topics and frames for specific hardware. - -### 1. TIAGo (Mobile Manipulator) - -Designed for the dual-arm TIAGo robot. It maps VR buttons to the TIAGo gripper actions. - -* **Reference Frame:** `base_link` -* **Target Frames:** `gripper_left_grasping_frame` / `gripper_right_grasping_frame` -* **Logic:** Maps the `menu` button directly to the gripper command topics. - -### 2. Unitree G1 (Humanoid) - -A specialized setup for high-speed humanoid teleoperation. - -* **Reference Frame:** `pelvis` -* **Target Frames:** `left_hand_point_contact` / `right_hand_point_contact` -* **Logic:** Uses **Event-Driven publishing** (Frequency: `-1.0`) to minimize latency for the humanoid's whole-body controller. -* **Custom Serials:** Overrides the default hardware IDs directly in the launch file to match specific lab hardware. +The node includes a `trackpad_pressed_required` parameter. If set to `true`, lightly brushing your thumb over the trackpad will output `0.0`. It will only output the X/Y coordinates to your robot if you physically *click down* on the trackpad. This is critical for preventing accidental movement commands on mobile bases or humanoids. --- -## āŒØļø Usage +## šŸ¤– Adding Your Own Robot (Custom Launch Files) + +The `ros2_vive_controller` is designed to be completely robot-agnostic. To control a new robot, you do **not** need to modify the Python source code. You only need to create a new launch file (e.g., `my_robot.launch.py`) that includes the base Vive driver and configures the `teleop_bridge_node` to match your robot's TF tree and topic names. + +### The Anatomy of a Robot Launch File + +A complete robot launch file consists of two main parts: + +#### 1. The Hardware Driver Include +You must include `vive_teleop.launch.py`. This starts the hardware communication and RViz. +* **`only_right`**: Set to `'true'` for single-arm robots (like Franka), or `'false'` for dual-arm robots (like TIAGo/G1). +* **`linear_scale`**: A multiplier for physical movement. (e.g., A scale of `2.0` means moving the controller 10cm moves the robot 20cm). + +#### 2. The Teleop Bridge Node(s) +You must instantiate one `teleop_bridge_node` per arm. This is where the magic happens. You need to configure the following critical parameters to match your robot's URDF/controllers: + +* **`reference_frame`**: The root frame of the robot (e.g., `base_link`, `pelvis`). The bridge uses this to calculate relative deltas. +* **`target_frame`**: The end-effector frame of the robot (e.g., `panda_hand_tcp`, `gripper_grasping_link`). +* **`rotation_offset`**: A `[Roll, Pitch, Yaw]` array (in degrees). VR controllers rarely align perfectly with robot grippers. Use this to permanently rotate the output orientation so that pointing the controller forward actually points the robot hand forward. +* **Button Topics**: Demultiplex the monolithic VR array into specific `PointStamped` topics. For example, map `'menu_topic'` to `'/my_robot/gripper_command'` to easily hook up an action server later. + +### šŸ“ Template: `custom_robot.launch.py` + +Here is a streamlined template based on a dual-arm setup (like TIAGo Pro) that you can copy and adapt for your own robot: +```python +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + pkg_share = get_package_share_directory('ros2_vive_controller') + hardware_launch_path = os.path.join(pkg_share, 'launch', 'vive_teleop.launch.py') + + # 1. Start the Base Hardware Driver (Dual Arm Mode) + include_vive_teleop = IncludeLaunchDescription( + PythonLaunchDescriptionSource(hardware_launch_path), + launch_arguments={ + 'rviz': 'true', + 'serial_right': LaunchConfiguration('serial_right', default='LHR-XXXXX'), + 'serial_left': LaunchConfiguration('serial_left', default='LHR-YYYYY'), + 'only_right': 'false', + 'linear_scale': '1.0' + }.items() + ) + + # 2. Configure the Right Arm Teleop Bridge + teleop_bridge_right = Node( + package='ros2_vive_controller', + executable='teleop_bridge_node', + name='teleop_bridge_right', + output='screen', + parameters=[{ + # Inputs from Driver + 'pose_topic': '/vive/right/pose', + 'button_state_topic': '/vive/right/joint_states', + + # Output to your Robot's Cartesian Controller + 'output_topic': '/my_robot/right_arm/target_pose', + 'publish_frequency': 30.0, + + # TF Alignment + 'reference_frame': 'base_link', + 'target_frame': 'right_gripper_link', + 'rotation_offset': [180.0, 0.0, 0.0], # Adjust if gripper moves backwards! + + # Button Remapping (PointStamped Outputs) + 'trigger_topic': '/vive/right/clutch_trigger', + 'menu_topic': '/my_robot/right_gripper/toggle', + # ... add other buttons as needed + }] + ) + + # (Repeat for Left Arm if necessary...) + + return LaunchDescription([ + include_vive_teleop, + teleop_bridge_right + ]) +``` -To launch a specific robot configuration inside the Docker container: +### šŸ³ Integrating with Docker and Make +Once you have written your `my_robot.launch.py` and saved it in the `/launch` folder: +1. Open `docker-compose.yml`. +2. Copy an existing robot block (like `tiago:`), rename it, and change the `command:` to run your new launch file. +3. Open `Makefile` and add a quick launch rule (e.g., `make my_robot: gui-perms \n docker compose --profile my_robot up`). -```bash -# For TIAGo -python3 scripts/run_docker.py tiago +## šŸ‘„ Maintainers & Contributors -# For Unitree G1 -python3 scripts/run_docker.py g1 +This package is actively developed by the **[HuCeBot Team](https://github.com/hucebot)** at Inria / Loria. -``` +**Authors & Core Contributors:** +* Maintainers: **[Dionis Totsila](https://github.com/dtotsila)**, **[Jean-Baptiste Mouret](https://github.com/jbmouret)** +* Active contributors: **[Hippolyte Henry](https://github.com/hippolyte-bleu)** +* Other contributors: **[Clemente Donoso](https://github.com/cdonoso)** \ No newline at end of file diff --git a/assets/hardware/stl/gripper_tiago.stl b/assets/hardware/stl/gripper_tiago.stl deleted file mode 100644 index b35635f..0000000 Binary files a/assets/hardware/stl/gripper_tiago.stl and /dev/null differ diff --git a/assets/hardware/stl/vive_support.stl b/assets/hardware/stl/vive_support.stl deleted file mode 100644 index af458dd..0000000 Binary files a/assets/hardware/stl/vive_support.stl and /dev/null differ diff --git a/assets/hardware/vive_support.FCStd b/assets/hardware/vive_support.FCStd deleted file mode 100644 index 3370174..0000000 Binary files a/assets/hardware/vive_support.FCStd and /dev/null differ diff --git a/assets/images/vive_controller_logo.png b/assets/images/vive_controller_logo.png index 5be3e45..aeb7b39 100644 Binary files a/assets/images/vive_controller_logo.png and b/assets/images/vive_controller_logo.png differ diff --git a/config/config.yaml b/config/config.yaml index fdcdf14..a0cfd49 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,12 +1,12 @@ general: angular_scale: 1.0 csv_path: /ros_ws/src/ros1_vive_controller/data/ - frame_id: /ci/world + frame_id: /opensot/world left_gripper_topic: /vive/left/gripper left_marker_topic: /vive/left/marker left_position_topic: /vive/left/pose linear_scale: 1.0 - link_name: /ci/world + link_name: /opensot/world move_base: true move_base_angular_topic: /vive/base/angular move_base_linear_x_topic: /vive/base/linear_x diff --git a/config/cyclonedds.xml b/config/cyclonedds.xml index 0020c7e..409cee8 100644 --- a/config/cyclonedds.xml +++ b/config/cyclonedds.xml @@ -1,30 +1,23 @@ - - + + + - false - 1400B - - + + true - auto - - - + + + + - 500 + auto + 200 - - - 2000kB - - - - config - ${HOME}/.ros/log/cdds.log - \ No newline at end of file diff --git a/config/franka_cyclonedds.xml b/config/franka_cyclonedds.xml new file mode 100644 index 0000000..409cee8 --- /dev/null +++ b/config/franka_cyclonedds.xml @@ -0,0 +1,23 @@ + + + + + + + + true + + + + + + + + + auto + 200 + + + \ No newline at end of file diff --git a/config/tiago_cyclonedds.xml b/config/tiago_cyclonedds.xml new file mode 100644 index 0000000..6e2c37b --- /dev/null +++ b/config/tiago_cyclonedds.xml @@ -0,0 +1,21 @@ + + + + + + + + 1400B + + + true + + + + + + auto + 500 + + + \ No newline at end of file diff --git a/docker-compose.yml b/docker-compose.yml index 479c40a..eef31a3 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -1,5 +1,16 @@ -services: +# ā”€ā”€ RULE: Common Environment Variables ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ +x-common-env: &common_env + DISPLAY: ${DISPLAY} + NVIDIA_DRIVER_CAPABILITIES: all + XDG_RUNTIME_DIR: ${XDG_RUNTIME_DIR} + QT_X11_NO_MITSHM: 1 + +# ā”€ā”€ RULE: ROS Environment Variables ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ +x-ros-env: &ros_env + ROS_DOMAIN_ID: ${ROS_DOMAIN_ID:-1} + RMW_IMPLEMENTATION: rmw_cyclonedds_cpp +services: # ā”€ā”€ bimanual: bimanual tracker (left + right) ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ bimanual_vive_tracker: profiles: [bimanual] @@ -17,14 +28,9 @@ services: pid: host privileged: true environment: - - DISPLAY=${DISPLAY} - - NVIDIA_DRIVER_CAPABILITIES=all - - XDG_RUNTIME_DIR=${XDG_RUNTIME_DIR} - - QT_X11_NO_MITSHM=1 - - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1} - - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp - - HAND_OFFSET_LEFT=${HAND_OFFSET_LEFT:-[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]} - - HAND_OFFSET_RIGHT=${HAND_OFFSET_RIGHT:-[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]} + <<: [*common_env, *ros_env] + HAND_OFFSET_LEFT: ${HAND_OFFSET_LEFT:-[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]} + HAND_OFFSET_RIGHT: ${HAND_OFFSET_RIGHT:-[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]} volumes: - /tmp/.X11-unix:/tmp/.X11-unix - /dev:/dev @@ -73,13 +79,8 @@ services: pid: host privileged: true environment: - - DISPLAY=${DISPLAY} - - NVIDIA_DRIVER_CAPABILITIES=all - - XDG_RUNTIME_DIR=${XDG_RUNTIME_DIR} - - QT_X11_NO_MITSHM=1 - - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1} - - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp - - HAND_OFFSET=${HAND_OFFSET:-[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]} + <<: [*common_env, *ros_env] + HAND_OFFSET: ${HAND_OFFSET:-[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]} volumes: - /tmp/.X11-unix:/tmp/.X11-unix - /dev:/dev @@ -109,3 +110,105 @@ services: - driver: nvidia count: all capabilities: [gpu] + + # ā”€ā”€ COMMON BASE: for controllers ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ + base_robot: &base_robot + image: ${REGISTRY:-registry.gitlab.inria.fr/eurobin-horizon/code/vive_controller_tiago}/vive-controller:${TAG:-latest} + build: + context: . + dockerfile: .ci/Dockerfile + target: app + args: + STEAM_USER: ${STEAM_USER} + STEAM_PASSWORD: ${STEAM_PASSWORD} + network_mode: host + ipc: host + pid: host + privileged: true + environment: + <<: [*common_env, *ros_env] + CYCLONEDDS_URI: file:///ros2_ws/src/ros2_vive_controller/config/cyclonedds.xml + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix + - /dev:/dev + - /run/udev:/run/udev + - ./config/steamvr_config/openvrpaths.vrpath:/root/.config/openvr/openvrpaths.vrpath + - ./config/steamvr_config/default_driver_null:/home/steam/Steam/steamapps/common/SteamVR/drivers/null/resources/settings/default.vrsettings + - ./config/steamvr_config/default_resources:/home/steam/Steam/steamapps/common/SteamVR/resources/settings/default.vrsettings + - ./ros2_vive_controller:/ros2_ws/src/ros2_vive_controller/ros2_vive_controller + - ./launch:/ros2_ws/src/ros2_vive_controller/launch + - ./config:/ros2_ws/src/ros2_vive_controller/config + - ./rviz:/ros2_ws/src/ros2_vive_controller/rviz + - ./assets:/ros2_ws/src/ros2_vive_controller/assets + - ./setup.py:/ros2_ws/src/ros2_vive_controller/setup.py + working_dir: /ros2_ws + entrypoint: ["/entrypoint.sh"] + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: all + capabilities: [gpu] + + # ā”€ā”€ FRANKA: (Single Arm) ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ + franka: + <<: *base_robot + profiles: [franka] + container_name: ros2_vive_franka + env_file: + - .env + environment: + <<: [*common_env, *ros_env] + CYCLONEDDS_URI: file:///ros2_ws/src/ros2_vive_controller/config/franka_cyclonedds.xml + command: > + ros2 launch ros2_vive_controller franka_single.launch.py + serial_right:=${SERIAL_RIGHT:-LHR-9ABF6D66} + reference_lighthouse_serial:=${REFERENCE_LIGHTHOUSE_SERIAL:-} + only_right:=true + linear_scale:=${LINEAR_SCALE:-2.0} + + # ā”€ā”€ TIAGO (Dual Arm) ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ + tiago: + <<: *base_robot + profiles: [tiago] + container_name: ros2_vive_tiago + env_file: .env + environment: + <<: [*common_env, *ros_env] + CYCLONEDDS_URI: file:///ros2_ws/src/ros2_vive_controller/config/tiago_cyclonedds.xml + command: > + ros2 launch ros2_vive_controller tiago_dual.launch.py + serial_left:=${SERIAL_LEFT:-LHR-97752221} + serial_right:=${SERIAL_RIGHT:-LHR-9ABF6D66} + linear_scale:=${LINEAR_SCALE:-1.0} + + # ā”€ā”€ TIAGO (Dual Arm) ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ + tiago_pro: + <<: *base_robot + profiles: [tiago_pro] + container_name: ros2_vive_tiago_pro + env_file: .env + environment: + <<: [*common_env, *ros_env] + CYCLONEDDS_URI: file:///ros2_ws/src/ros2_vive_controller/config/tiago_cyclonedds.xml + command: > + ros2 launch ros2_vive_controller tiago_pro.launch.py + serial_left:=${SERIAL_LEFT:-LHR-97752221} + serial_right:=${SERIAL_RIGHT:-LHR-9ABF6D66} + linear_scale:=${LINEAR_SCALE:-1.0} + + + # ā”€ā”€ G1 (Dual Arm) ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ + g1: + <<: *base_robot + profiles: [g1] + container_name: ros2_vive_g1 + environment: + <<: [*common_env, *ros_env] + CYCLONEDDS_URI: file:///ros2_ws/src/ros2_vive_controller/config/cyclonedds.xml + command: > + ros2 launch ros2_vive_controller g1_dual.launch.py + serial_left:=${SERIAL_LEFT:-LHR-97752221} + serial_right:=${SERIAL_RIGHT:-LHR-9ABF6D66} + linear_scale:=${LINEAR_SCALE:-1.0} \ No newline at end of file diff --git a/launch/franka_single.launch.py b/launch/franka_single.launch.py index c6c8354..6bd8bea 100644 --- a/launch/franka_single.launch.py +++ b/launch/franka_single.launch.py @@ -7,37 +7,56 @@ from launch_ros.actions import Node def generate_launch_description(): - # 1. Locate the vive_teleop.launch.py file pkg_share = get_package_share_directory('ros2_vive_controller') included_launch_path = os.path.join(pkg_share, 'launch', 'vive_teleop.launch.py') - # 2. Define the Include action for hardware drivers - include_vive_teleop = IncludeLaunchDescription( - PythonLaunchDescriptionSource(included_launch_path), - launch_arguments={'rviz': 'true'}.items() + # --- General Launch Arguments --- + serial_right_arg = DeclareLaunchArgument( + 'serial_right', default_value='LHR-9ABF6D66', description='Serial number for the right controller' + ) + + serial_left_arg = DeclareLaunchArgument( + 'serial_left', default_value='LHR-97752221', description='Serial number for the left controller' + ) + + reference_lighthouse_serial_arg = DeclareLaunchArgument( + 'reference_lighthouse_serial', default_value='', description='Serial number for the reference lighthouse' + ) + + only_right_arg = DeclareLaunchArgument( + 'only_right', default_value='true', description="If set, only the right controller will be active." + ) + + linear_scale_arg = DeclareLaunchArgument( + 'linear_scale', default_value='1.0', description='Scaling factor for controller translation.' ) - # 3. General Launch Arguments publish_frequency_arg = DeclareLaunchArgument( - 'publish_frequency', - default_value='30.0', - description='Publishing frequency in Hz.' + 'publish_frequency', default_value='30.0', description='Publishing frequency in Hz.' ) reference_frame_arg = DeclareLaunchArgument( - 'reference_frame', - default_value='panda_link0', - description='Reference frame for transform lookup' + 'reference_frame', default_value='panda_link0', description='Reference frame for transform lookup' ) - target_frame_franka_tcp_arg = DeclareLaunchArgument( - 'target_frame_franka_tcp', - default_value='panda_hand_tcp', - description='Target frame for Franka TCP controller' + 'target_frame_franka_tcp', default_value='panda_hand_tcp', description='Target frame for Franka TCP controller' + ) + + # --- Hardware Driver Inclusion --- + include_vive_teleop = IncludeLaunchDescription( + PythonLaunchDescriptionSource(included_launch_path), + launch_arguments={ + 'rviz': 'true', + 'serial_right': LaunchConfiguration('serial_right'), + 'serial_left': LaunchConfiguration('serial_left'), + 'reference_lighthouse_serial': LaunchConfiguration('reference_lighthouse_serial'), # --- NEW: Pass it down --- + 'only_right': LaunchConfiguration('only_right'), + 'linear_scale': LaunchConfiguration('linear_scale') + }.items() ) - # 5. Teleop bridge node - RIGHT + # --- Teleop Bridge Node --- teleop_bridge_franka_tcp = Node( package='ros2_vive_controller', executable='teleop_bridge_node', @@ -50,24 +69,26 @@ def generate_launch_description(): 'publish_frequency': LaunchConfiguration('publish_frequency'), 'target_frame': LaunchConfiguration('target_frame_franka_tcp'), 'reference_frame': LaunchConfiguration('reference_frame'), - - # --- ADD THE ROTATION OFFSET HERE --- 'rotation_offset': [180.0, 0.0, 0.0], - - # --- MANUAL BUTTON TOPIC MAPPING (RIGHT) --- 'trigger_topic': '/vive/right/trigger', 'trackpad_x_topic': '/vive/right/trackpad_x', 'trackpad_y_topic': '/vive/right/trackpad_y', - 'grip_topic': '/panda_gripper/gripper_command', - 'menu_topic': '/vive/right/menu', + 'grip_topic': '/vive/right/grip_button', + 'menu_topic': '/panda_gripper/gripper_command', 'trackpad_touched_topic': '/vive/right/trackpad_touched', 'trackpad_pressed_topic': '/vive/right/trackpad_pressed', }] ) + return LaunchDescription([ - include_vive_teleop, + serial_right_arg, + serial_left_arg, + reference_lighthouse_serial_arg, + only_right_arg, + linear_scale_arg, publish_frequency_arg, reference_frame_arg, target_frame_franka_tcp_arg, + include_vive_teleop, teleop_bridge_franka_tcp, ]) \ No newline at end of file diff --git a/launch/tiago_dual.launch.py b/launch/tiago_dual.launch.py index 234a4e3..1207c7e 100644 --- a/launch/tiago_dual.launch.py +++ b/launch/tiago_dual.launch.py @@ -7,42 +7,43 @@ from launch_ros.actions import Node def generate_launch_description(): - # 1. Locate the vive_teleop.launch.py file + # Locate paths pkg_share = get_package_share_directory('ros2_vive_controller') included_launch_path = os.path.join(pkg_share, 'launch', 'vive_teleop.launch.py') - # 2. Define the Include action for hardware drivers - include_vive_teleop = IncludeLaunchDescription( - PythonLaunchDescriptionSource(included_launch_path), - launch_arguments={'rviz': 'true'}.items() + # --- General Launch Arguments --- + serial_right_arg = DeclareLaunchArgument( + 'serial_right', default_value='LHR-9ABF6D66', + description='Serial number for the right controller' + ) + serial_left_arg = DeclareLaunchArgument( + 'serial_left', default_value='LHR-97752221', + description='Serial number for the left controller' + ) + linear_scale_arg = DeclareLaunchArgument( + 'linear_scale', default_value='1.0', + description='Linear scaling for both controllers' ) - - # 3. General Launch Arguments publish_frequency_arg = DeclareLaunchArgument( - 'publish_frequency', - default_value='30.0', - description='Publishing frequency in Hz.' + 'publish_frequency', default_value='30.0' ) - reference_frame_arg = DeclareLaunchArgument( - 'reference_frame', - default_value='ci/base_link', - description='Reference frame for transform lookup' + 'reference_frame', default_value='opensot/base_link' ) - target_frame_left_arg = DeclareLaunchArgument( - 'target_frame_left', - default_value='ci/gripper_left_grasping_frame', - description='Target frame for left controller' - ) - - target_frame_right_arg = DeclareLaunchArgument( - 'target_frame_right', - default_value='ci/gripper_right_grasping_frame', - description='Target frame for right controller' + # --- Hardware Driver Inclusion (Dual Mode) --- + include_vive_teleop = IncludeLaunchDescription( + PythonLaunchDescriptionSource(included_launch_path), + launch_arguments={ + 'rviz': 'true', + 'serial_right': LaunchConfiguration('serial_right'), + 'serial_left': LaunchConfiguration('serial_left'), + 'only_right': 'false', # Force dual-controller mode for Tiago + 'linear_scale': LaunchConfiguration('linear_scale') + }.items() ) - # 4. Teleop bridge node - LEFT + # --- Teleop Bridge - LEFT --- teleop_bridge_left = Node( package='ros2_vive_controller', executable='teleop_bridge_node', @@ -53,10 +54,10 @@ def generate_launch_description(): 'button_state_topic': '/vive/left/joint_states', 'output_topic': '/vive/left/output_pose', 'publish_frequency': LaunchConfiguration('publish_frequency'), - 'target_frame': LaunchConfiguration('target_frame_left'), + 'target_frame': LaunchConfiguration('target_frame_left', default='opensot/gripper_left_grasping_frame'), 'reference_frame': LaunchConfiguration('reference_frame'), + 'rotation_offset': [0.0, 0.0, 0.0], # Standard Tiago mapping - # --- MANUAL BUTTON TOPIC MAPPING (LEFT) --- 'trigger_topic': '/vive/left/trigger', 'trackpad_x_topic': '/vive/left/trackpad_x', 'trackpad_y_topic': '/vive/left/trackpad_y', @@ -67,7 +68,7 @@ def generate_launch_description(): }] ) - # 5. Teleop bridge node - RIGHT + # --- Teleop Bridge - RIGHT --- teleop_bridge_right = Node( package='ros2_vive_controller', executable='teleop_bridge_node', @@ -78,10 +79,10 @@ def generate_launch_description(): 'button_state_topic': '/vive/right/joint_states', 'output_topic': '/vive/right/output_pose', 'publish_frequency': LaunchConfiguration('publish_frequency'), - 'target_frame': LaunchConfiguration('target_frame_right'), + 'target_frame': LaunchConfiguration('target_frame_right', default='opensot/gripper_right_grasping_frame'), 'reference_frame': LaunchConfiguration('reference_frame'), + 'rotation_offset': [0.0, 0.0, 0.0], # Standard Tiago mapping - # --- MANUAL BUTTON TOPIC MAPPING (RIGHT) --- 'trigger_topic': '/vive/right/trigger', 'trackpad_x_topic': '/vive/right/trackpad_x', 'trackpad_y_topic': '/vive/right/trackpad_y', @@ -93,11 +94,12 @@ def generate_launch_description(): ) return LaunchDescription([ - include_vive_teleop, + serial_right_arg, + serial_left_arg, + linear_scale_arg, publish_frequency_arg, reference_frame_arg, - target_frame_left_arg, - target_frame_right_arg, + include_vive_teleop, teleop_bridge_left, teleop_bridge_right, ]) \ No newline at end of file diff --git a/launch/tiago_pro.launch.py b/launch/tiago_pro.launch.py new file mode 100644 index 0000000..e31f47b --- /dev/null +++ b/launch/tiago_pro.launch.py @@ -0,0 +1,103 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + # Locate paths + pkg_share = get_package_share_directory('ros2_vive_controller') + included_launch_path = os.path.join(pkg_share, 'launch', 'vive_teleop.launch.py') + + # --- General Launch Arguments --- + serial_right_arg = DeclareLaunchArgument( + 'serial_right', default_value='LHR-9ABF6D66', + description='Serial number for the right controller' + ) + serial_left_arg = DeclareLaunchArgument( + 'serial_left', default_value='LHR-97752221', + description='Serial number for the left controller' + ) + linear_scale_arg = DeclareLaunchArgument( + 'linear_scale', default_value='0.5o', + description='Linear scaling for both controllers' + ) + publish_frequency_arg = DeclareLaunchArgument( + 'publish_frequency', default_value='30.0' + ) + reference_frame_arg = DeclareLaunchArgument( + 'reference_frame', default_value='opensot/base_link' + ) + + # --- Hardware Driver Inclusion (Dual Mode) --- + include_vive_teleop = IncludeLaunchDescription( + PythonLaunchDescriptionSource(included_launch_path), + launch_arguments={ + 'rviz': 'true', + 'serial_right': LaunchConfiguration('serial_right'), + 'serial_left': LaunchConfiguration('serial_left'), + 'only_right': 'false', # Force dual-controller mode for Tiago + 'linear_scale': LaunchConfiguration('linear_scale') + }.items() + ) + + # --- Teleop Bridge - LEFT --- + teleop_bridge_left = Node( + package='ros2_vive_controller', + executable='teleop_bridge_node', + name='teleop_bridge_left', + output='screen', + parameters=[{ + 'pose_topic': '/vive/left/pose', + 'button_state_topic': '/vive/left/joint_states', + 'output_topic': '/vive/left/output_pose', + 'publish_frequency': LaunchConfiguration('publish_frequency'), + 'target_frame': LaunchConfiguration('target_frame_left', default='opensot/gripper_left_grasping_link'), + 'reference_frame': LaunchConfiguration('reference_frame'), + 'rotation_offset': [180.0, 0.0, 0.0], + 'trigger_topic': '/vive/left/trigger', + 'trackpad_x_topic': '/vive/left/trackpad_x', + 'trackpad_y_topic': '/vive/left/trackpad_y', + 'grip_topic': '/vive/left/grip', + 'menu_topic': '/vive/left/gripper', + 'trackpad_touched_topic': '/vive/left/trackpad_touched', + 'trackpad_pressed_topic': '/vive/left/trackpad_pressed', + }] + ) + + # --- Teleop Bridge - RIGHT --- + teleop_bridge_right = Node( + package='ros2_vive_controller', + executable='teleop_bridge_node', + name='teleop_bridge_right', + output='screen', + parameters=[{ + 'pose_topic': '/vive/right/pose', + 'button_state_topic': '/vive/right/joint_states', + 'output_topic': '/vive/right/output_pose', + 'publish_frequency': LaunchConfiguration('publish_frequency'), + 'target_frame': LaunchConfiguration('target_frame_right', default='opensot/gripper_right_grasping_link'), + 'reference_frame': LaunchConfiguration('reference_frame'), + 'rotation_offset': [180.0, 0.0, 0.0], + 'trigger_topic': '/vive/right/trigger', + 'trackpad_x_topic': '/vive/right/trackpad_x', + 'trackpad_y_topic': '/vive/right/trackpad_y', + 'grip_topic': '/vive/right/grip', + 'menu_topic': '/vive/right/gripper', + 'trackpad_touched_topic': '/vive/right/trackpad_touched', + 'trackpad_pressed_topic': '/vive/right/trackpad_pressed', + }] + ) + + return LaunchDescription([ + serial_right_arg, + serial_left_arg, + linear_scale_arg, + publish_frequency_arg, + reference_frame_arg, + include_vive_teleop, + teleop_bridge_left, + teleop_bridge_right, + ]) \ No newline at end of file diff --git a/launch/vive_teleop.launch.py b/launch/vive_teleop.launch.py index c946d41..6dff50e 100644 --- a/launch/vive_teleop.launch.py +++ b/launch/vive_teleop.launch.py @@ -1,58 +1,35 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, TimerAction, LogInfo -from launch.conditions import IfCondition +from launch.actions import DeclareLaunchArgument, TimerAction +from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): pkg_share = get_package_share_directory('ros2_vive_controller') - # Check if we should use 'vive.rviz' or 'view_vive.rviz' based on your file tree rviz_filename = 'view_vive.rviz' rviz_config = os.path.join(pkg_share, 'rviz', rviz_filename) vive_params_file = os.path.join(pkg_share, 'config', 'vive.params.yaml') - # --- DEBUG LOGGING --- - print(f"\n[DEBUG] Package Share Path: {pkg_share}") - print(f"[DEBUG] Looking for RViz file at: {rviz_config}") - - if os.path.exists(rviz_config): - print(f"[DEBUG] āœ… File FOUND.\n") - else: - print(f"[DEBUG] āŒ File NOT FOUND.") - print(f"[DEBUG] Contents of {os.path.join(pkg_share, 'rviz')}:") - try: - print(os.listdir(os.path.join(pkg_share, 'rviz'))) - except FileNotFoundError: - print(" (The 'rviz' directory itself was not found in share. Check setup.py data_files!)") - print("\n") - # --------------------- - return LaunchDescription([ - DeclareLaunchArgument( - 'rviz', - default_value='true', - description='Open RViz to visualize' - ), + DeclareLaunchArgument('rviz', default_value='true'), + DeclareLaunchArgument('serial_left', default_value='LHR-97752221'), + DeclareLaunchArgument('serial_right', default_value='LHR-9ABF6D66'), - DeclareLaunchArgument( - 'serial_left', - default_value='LHR-97752221', - description='Serial number for the left controller' - ), + # --- CHANGED: Name matches parent launch and node parameter --- + DeclareLaunchArgument('reference_lighthouse_serial', default_value=''), DeclareLaunchArgument( - 'serial_right', - default_value='LHR-9ABF6D66', - description='Serial number for the right controller' + 'only_right', + default_value='false', + description='If true, the left controller node is skipped.' ), - DeclareLaunchArgument( - 'tracking_reference', - default_value='LHB-DFA5BD2C', - description='Serial number for the tracking reference (base station)' + 'linear_scale', + default_value='1.0', + description='Translation scaling factor.' ), # --- LEFT HAND DRIVER --- @@ -62,11 +39,13 @@ def generate_launch_description(): name='driver_left', namespace='vive/left', output='screen', + condition=UnlessCondition(LaunchConfiguration('only_right')), parameters=[ vive_params_file, {'side': 'left', + 'linear_scale': LaunchConfiguration('linear_scale'), 'serial': LaunchConfiguration('serial_left'), - 'htc_vive.tracking_reference': LaunchConfiguration('tracking_reference')} + 'reference_lighthouse_serial': LaunchConfiguration('reference_lighthouse_serial')} # <--- UPDATED ] ), @@ -81,9 +60,9 @@ def generate_launch_description(): parameters=[ vive_params_file, {'side': 'right', + 'linear_scale': LaunchConfiguration('linear_scale'), 'serial': LaunchConfiguration('serial_right'), - 'htc_vive.tracking_reference': LaunchConfiguration('tracking_reference') - } + 'reference_lighthouse_serial': LaunchConfiguration('reference_lighthouse_serial')} # <--- UPDATED ] ), ]), diff --git a/ros2_vive_controller/list_devices.py b/ros2_vive_controller/list_devices.py new file mode 100644 index 0000000..eb805cf --- /dev/null +++ b/ros2_vive_controller/list_devices.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python3 +from ros2_vive_controller.openvr_class.openvr_class import triad_openvr + +def list_all_devices(): + try: + vr = triad_openvr() + except Exception as e: + print(f"Failed to initialize OpenVR: {e}") + return + + print("--- Connected Vive Devices ---") + for name, device in vr.devices.items(): + serial = device.get_serial() + device_class = device.device_class + + # Highlight lighthouses + if device_class == "TrackingReference": + print(f"šŸŸ¢ LIGHTHOUSE | Name: {name:<15} | Serial: {serial}") + elif device_class == "Controller": + print(f"šŸŽ® CONTROLLER | Name: {name:<15} | Serial: {serial}") + else: + print(f"āšŖ OTHER | Name: {name:<15} | Serial: {serial} (Class: {device_class})") + +if __name__ == "__main__": + list_all_devices() \ No newline at end of file diff --git a/ros2_vive_controller/openvr_class/openvr_class.py b/ros2_vive_controller/openvr_class/openvr_class.py index 3ec9234..aa87c4b 100644 --- a/ros2_vive_controller/openvr_class/openvr_class.py +++ b/ros2_vive_controller/openvr_class/openvr_class.py @@ -117,7 +117,7 @@ def sample(self,num_samples,sample_rate): if sleep_time>0: time.sleep(sleep_time) return rtn - + def get_pose_quaternion(self, pose=None): if pose == None: pose = get_pose(self.vr) @@ -203,7 +203,7 @@ def __init__(self, configfile_path=None): self.device_index_map = {} poses = self.vr.getDeviceToAbsoluteTrackingPose(openvr.TrackingUniverseStanding, 0, openvr.k_unMaxTrackedDeviceCount) - + for i in range(openvr.k_unMaxTrackedDeviceCount): if poses[i].bDeviceIsConnected: self.add_tracked_device(i) @@ -247,7 +247,7 @@ def reorder_tracking_references(self, desired_serial): if self.devices[dev_name].get_serial() == desired_serial: device_with_desired_serial = dev_name break - + if device_with_desired_serial: references.remove(device_with_desired_serial) references.insert(0, device_with_desired_serial) diff --git a/ros2_vive_controller/vive_node.py b/ros2_vive_controller/vive_node.py index 72d4e58..6036f4c 100644 --- a/ros2_vive_controller/vive_node.py +++ b/ros2_vive_controller/vive_node.py @@ -1,16 +1,18 @@ #!/usr/bin/env python3 import time import rclpy +import numpy as np +from scipy.spatial.transform import Rotation as R from rclpy.node import Node -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseStamped, TransformStamped from std_msgs.msg import Bool from sensor_msgs.msg import JointState from visualization_msgs.msg import Marker +from std_srvs.srv import Trigger +from tf2_ros import TransformBroadcaster from OneEuroFilter import OneEuroFilter -# Ensure your import matches your folder structure from ros2_vive_controller.openvr_class.openvr_class import triad_openvr - class ViveDriverNode(Node): def __init__(self): super().__init__('vive_driver') @@ -21,13 +23,13 @@ def __init__(self): parameters=[ ('side', 'right'), ('serial', ''), + ('reference_lighthouse_serial', ''), ('frame_id', 'vive_world'), - # Workspace Limits (meters) + ('linear_scale', 1.0), ('workspace.x_min', -1.0), ('workspace.x_max', 1.0), ('workspace.y_min', -1.0), ('workspace.y_max', 1.0), ('workspace.z_min', 0.0), ('workspace.z_max', 2.0), - ('workspace.padding', 0.1), # Vibration buffer zone - # Smoothing + ('workspace.padding', 0.1), ('filter.mincutoff', 1.0), ('filter.beta', 0.007), ('filter.dcutoff', 1.0) @@ -36,9 +38,10 @@ def __init__(self): self.side = self.get_parameter('side').value self.serial = self.get_parameter('serial').value + self.ref_lh_serial = self.get_parameter('reference_lighthouse_serial').value self.frame_id = self.get_parameter('frame_id').value + self.linear_scale = self.get_parameter('linear_scale').value - # Load Workspace Cache self.ws = { 'x_min': self.get_parameter('workspace.x_min').value, 'x_max': self.get_parameter('workspace.x_max').value, @@ -53,33 +56,50 @@ def __init__(self): self.vr = triad_openvr() self.device_name = self._find_device_by_serial(self.serial) + # Find Lighthouse + self.lh_device_name = None + if self.ref_lh_serial: + self.lh_device_name = self._find_device_by_serial(self.ref_lh_serial) + if self.lh_device_name: + self.get_logger().info(f"Using Lighthouse {self.ref_lh_serial} as origin frame.") + else: + self.get_logger().warn(f"Lighthouse {self.ref_lh_serial} not found. Using default OpenVR world frame.") + + # --- Publishers & Services --- + self.srv_identify = self.create_service(Trigger, 'identify', self.identify_callback) + self.pub_pose = self.create_publisher(PoseStamped, 'pose', 10) + self.pub_joy = self.create_publisher(JointState, 'joint_states', 10) + self.pub_marker = self.create_publisher(Marker, 'workspace_marker', 10) + self.tf_broadcaster = TransformBroadcaster(self) + if not self.device_name: self.get_logger().error(f"Could not find controller {self.serial}") - # We don't crash, allowing potential re-connection logic later else: - self.get_logger().info( - f"Connected to {self.side} controller: {self.serial}") + self.get_logger().info(f"Connected to {self.side} controller: {self.serial}") - # --- Publishers --- - self.pub_pose = self.create_publisher(PoseStamped, 'pose', 10) - self.pub_joy = self.create_publisher(JointState, 'joint_states', 10) - self.pub_marker = self.create_publisher(Marker, 'workspace_marker', 10) + # --- State Variables --- self.is_calibrating = False - self.create_subscription( - Bool, '/vive/is_calibrating', self.calibration_callback, 10) + self.last_heartbeat_time = 0.0 + self.create_subscription(Bool, '/vive/is_calibrating', self.calibration_callback, 10) self.create_timer(0.05, self.watchdog_check) - # --- Filters --- - self.filters = self._init_filters() - # --- Loop --- - self.create_timer(0.02, self.update) # 50Hz + self.filters = self._init_filters() + self.tracking_active = False + self.haptic_frames_remaining = 0 - # Publish marker once at startup (and periodically in loop) - self.publish_workspace_marker() + self.create_timer(0.02, self.update) + self.publish_workspace_marker(self.frame_id) - # Tracking staate - self.tracking_active = False # Flag to track if we currently have valid tracking - self.haptic_frames_remaining = 0 # Counter for vibration duration + def identify_callback(self, request, response): + if not self.device_name: + response.success = False + response.message = "Controller not connected." + return response + self.get_logger().info(f"Identifying controller: {self.serial}") + self.haptic_frames_remaining = 150 + response.success = True + response.message = f"Vibrating {self.side} controller ({self.serial})" + return response def _find_device_by_serial(self, target_serial): for key, dev in self.vr.devices.items(): @@ -90,15 +110,11 @@ def _find_device_by_serial(self, target_serial): def calibration_callback(self, msg): if msg.data: self.is_calibrating = True - # Reset the watchdog timer self.last_heartbeat_time = self.get_clock().now().nanoseconds / 1e9 def watchdog_check(self): - # If we haven't heard a heartbeat in 0.5 seconds, Force Safety ON now = self.get_clock().now().nanoseconds / 1e9 - timeout = 0.5 - - if self.is_calibrating and (now - self.last_heartbeat_time > timeout): + if self.is_calibrating and (now - self.last_heartbeat_time > 0.5): self.get_logger().warn("Calibration heartbeat lost! Re-enabling safety limits.") self.is_calibrating = False @@ -112,80 +128,165 @@ def _init_filters(self): return {axis: OneEuroFilter(**params) for axis in ['x', 'y', 'z']} def check_workspace_and_vibrate(self, pos, device): - """ - Checks if position is within bounds. - If UNSAFE: Vibrates controller and returns False. - If SAFE: Returns True. - """ x, y, z = pos pad = self.ws['pad'] - in_bounds = ( self.ws['x_min'] + pad < x < self.ws['x_max'] - pad and self.ws['y_min'] + pad < y < self.ws['y_max'] - pad and self.ws['z_min'] + pad < z < self.ws['z_max'] - pad ) - if not in_bounds: - # Vibrate the controller (Haptic Feedback) - 2ms pulse device.trigger_haptic_pulse(2000) - return False # Unsafe + return False + return True + + def _compute_relative_pose(self, target_pose, reference_pose): + """Transforms target_pose into the coordinate frame of reference_pose.""" + T_ref = np.eye(4) + T_ref[:3, 3] = reference_pose[:3] + T_ref[:3, :3] = R.from_quat(reference_pose[3:]).as_matrix() + + T_target = np.eye(4) + T_target[:3, 3] = target_pose[:3] + T_target[:3, :3] = R.from_quat(target_pose[3:]).as_matrix() + + T_rel = np.linalg.inv(T_ref) @ T_target + + pos = T_rel[:3, 3] + quat = R.from_matrix(T_rel[:3, :3]).as_quat() + + return [pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]] - return True # Safe + def _make_tf(self, pose, parent_frame, child_frame): + t = TransformStamped() + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = parent_frame + t.child_frame_id = child_frame + + t.transform.translation.x = pose[0] + t.transform.translation.y = pose[1] + t.transform.translation.z = pose[2] + t.transform.rotation.x = pose[3] + t.transform.rotation.y = pose[4] + t.transform.rotation.z = pose[5] + t.transform.rotation.w = pose[6] + return t + + def _apply_rotation_offset(self, pose, axis='z', angle_degrees=180): + """Rotates a pose around a local axis to correct hardware alignments.""" + T = np.eye(4) + T[:3, 3] = pose[:3] + T[:3, :3] = R.from_quat(pose[3:]).as_matrix() + + T_rot = np.eye(4) + T_rot[:3, :3] = R.from_euler(axis, angle_degrees, degrees=True).as_matrix() + + T_new = T @ T_rot + + pos = T_new[:3, 3] + quat = R.from_matrix(T_new[:3, :3]).as_quat() + return [pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]] def update(self): + # 1. Poll OpenVR to catch any new devices + self.vr.poll_vr_events() + + # 2. Check controller if not self.device_name: - return + self.device_name = self._find_device_by_serial(self.serial) + if self.device_name: + self.get_logger().info(f"Connected to {self.side} controller: {self.serial}") + else: + return + + # 3. Check lighthouse + if self.ref_lh_serial and not self.lh_device_name: + self.lh_device_name = self._find_device_by_serial(self.ref_lh_serial) + if self.lh_device_name: + self.get_logger().info(f"Using Lighthouse {self.ref_lh_serial} as origin frame.") controller = self.vr.devices[self.device_name] + + # --- HAPTICS --- + if self.haptic_frames_remaining > 0: + controller.trigger_haptic_pulse(3900) + time.sleep(0.005) + controller.trigger_haptic_pulse(3900) + self.haptic_frames_remaining -= 1 + pose = controller.get_pose_quaternion() inputs = controller.get_controller_inputs() + # Tracking Check if pose is None: if self.tracking_active: self.get_logger().warn(f"Tracking LOST for {self.side}") self.tracking_active = False - return # Exit early + return - # If we just regained tracking (pose is valid, but flag was False) if not self.tracking_active: self.get_logger().info(f"Tracking ACQUIRED for {self.side}! Buzzing...") self.tracking_active = True - self.haptic_frames_remaining = 50 # Vibrate for 50 frames (~1 second) - - # --- 2. NON-BLOCKING VIBRATION --- - # If the counter is > 0, vibrate this frame and decrease counter - if self.haptic_frames_remaining > 0: - controller.trigger_haptic_pulse(3000) # Strong pulse - self.haptic_frames_remaining -= 1 - - # --- 3. SAFETY CHECK --- - - # --- 2. Publish Data (Only if Safe) --- + self.haptic_frames_remaining = 50 + + # --- Frame Resolution & TF Broadcasting --- + world_frame = self.frame_id + lh_frame = "lighthouse_origin" + controller_frame = f"vive_{self.side}_link" + + final_pose = pose + current_ref_frame = world_frame + + if self.lh_device_name: + lh = self.vr.devices.get(self.lh_device_name) + if lh: + lh_pose = lh.get_pose_quaternion() + if lh_pose: + # --- NEW: Flip the lighthouse frame 180 degrees on the Z (Yaw) axis --- + # Note: If it's flipping upside down instead of spinning around, + # change axis='z' to axis='x' or axis='y' + lh_pose = self._apply_rotation_offset(lh_pose, axis='z', angle_degrees=180) + + current_ref_frame = lh_frame + # Broadcast World -> Lighthouse + self.tf_broadcaster.sendTransform(self._make_tf(lh_pose, world_frame, lh_frame)) + # Compute Relative Pose + final_pose = self._compute_relative_pose(pose, lh_pose) + # Broadcast Lighthouse -> Controller + self.tf_broadcaster.sendTransform(self._make_tf(final_pose, lh_frame, controller_frame)) + + # Fallback if no lighthouse tracked + if current_ref_frame == world_frame: + self.tf_broadcaster.sendTransform(self._make_tf(final_pose, world_frame, controller_frame)) + + # --- Safety Check --- + raw_pos = final_pose[0:3] + if not self.is_calibrating: + if not self.check_workspace_and_vibrate(raw_pos, controller): + return + + # --- Publish Data --- timestamp = self.get_clock().now().to_msg() t = self.get_clock().now().nanoseconds / 1e9 - # A. Pose msg_pose = PoseStamped() msg_pose.header.stamp = timestamp - msg_pose.header.frame_id = self.frame_id + msg_pose.header.frame_id = current_ref_frame - msg_pose.pose.position.x = self.filters['x'](pose[0], t) - msg_pose.pose.position.y = self.filters['y'](pose[1], t) - msg_pose.pose.position.z = self.filters['z'](pose[2], t) - msg_pose.pose.orientation.x = pose[3] - msg_pose.pose.orientation.y = pose[4] - msg_pose.pose.orientation.z = pose[5] - msg_pose.pose.orientation.w = pose[6] + msg_pose.pose.position.x = self.filters['x'](final_pose[0], t) * self.linear_scale + msg_pose.pose.position.y = self.filters['y'](final_pose[1], t) * self.linear_scale + msg_pose.pose.position.z = self.filters['z'](final_pose[2], t) * self.linear_scale + msg_pose.pose.orientation.x = final_pose[3] + msg_pose.pose.orientation.y = final_pose[4] + msg_pose.pose.orientation.z = final_pose[5] + msg_pose.pose.orientation.w = final_pose[6] self.pub_pose.publish(msg_pose) - # B. Buttons msg_joy = JointState() msg_joy.header.stamp = timestamp - msg_joy.header.frame_id = self.frame_id - msg_joy.name = ['trigger', 'trackpad_x', 'trackpad_y', - 'grip', 'menu', 'trackpad_touched', 'trackpad_pressed'] + msg_joy.header.frame_id = current_ref_frame + msg_joy.name = ['trigger', 'trackpad_x', 'trackpad_y', 'grip', 'menu', 'trackpad_touched', 'trackpad_pressed'] msg_joy.position = [ float(inputs.get('trigger', 0.0)), float(inputs.get('trackpad_x', 0.0)), @@ -197,15 +298,12 @@ def update(self): ] self.pub_joy.publish(msg_joy) - # C. Visualization - # We publish this occasionally so RViz always shows the red box - # Only update marker when interacting to save bandwidth if inputs.get('trackpad_touched', False): - self.publish_workspace_marker() + self.publish_workspace_marker(current_ref_frame) - def publish_workspace_marker(self): + def publish_workspace_marker(self, frame_id): m = Marker() - m.header.frame_id = self.frame_id + m.header.frame_id = frame_id m.header.stamp = self.get_clock().now().to_msg() m.ns = "workspace_limit" m.id = 0 @@ -218,10 +316,9 @@ def publish_workspace_marker(self): m.pose.position.y = (self.ws['y_max'] + self.ws['y_min']) / 2.0 m.pose.position.z = (self.ws['z_max'] + self.ws['z_min']) / 2.0 m.pose.orientation.w = 1.0 - m.color.r, m.color.g, m.color.b, m.color.a = 1.0, 0.0, 0.0, 0.15 # Red Transparent + m.color.r, m.color.g, m.color.b, m.color.a = 1.0, 0.0, 0.0, 0.15 self.pub_marker.publish(m) - def main(): rclpy.init() node = ViveDriverNode() @@ -232,6 +329,5 @@ def main(): node.destroy_node() rclpy.shutdown() - if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/scripts/build_docker.py b/scripts/build_docker.py deleted file mode 100644 index 9ac1c6b..0000000 --- a/scripts/build_docker.py +++ /dev/null @@ -1,109 +0,0 @@ -#!/usr/bin/env python3 -import argparse -import subprocess -import os -import sys - -# Default Configuration -DEFAULT_REGISTRY = "registry.gitlab.inria.fr/eurobin-horizon/code/vive_controller_tiago" -IMAGE_NAME = "vive-controller" - -def get_project_version(): - """Reads the version from the VERSION file in the repo root.""" - script_dir = os.path.dirname(os.path.abspath(__file__)) - repo_root = os.path.dirname(script_dir) - version_file = os.path.join(repo_root, "VERSION") - - try: - with open(version_file, "r") as f: - return f.read().strip() - except FileNotFoundError: - print("WARNING: VERSION file not found. Defaulting to 'latest'.") - return "latest" - -def build_docker(args): - # Determine Tag - tag = args.tag if args.tag else get_project_version() - - # Construct full image name - full_image_name = f"{args.registry}/{IMAGE_NAME}:{tag}" - - # Calculate Paths - script_dir = os.path.dirname(os.path.abspath(__file__)) - repo_root = os.path.dirname(script_dir) - # Correct path to Dockerfile inside .ci/ - dockerfile_path = os.path.join(repo_root, ".ci", "Dockerfile") - - print(f"\n[BUILD] Registry: {args.registry}") - print(f"[BUILD] Target Stage: {args.target}") - print(f"[BUILD] Image Tag: {full_image_name}") - print(f"[BUILD] Dockerfile: {dockerfile_path}") - print(f"[BUILD] Context: {repo_root}\n") - - # Build Command - cmd = [ - "docker", "build", - "-t", full_image_name, - "-f", dockerfile_path, - "--target", args.target, - ] - - # --- NEW: Add No-Cache Flag --- - if args.no_cache: - print("[INFO] Building with --no-cache (Forcing rebuild)") - cmd.append("--no-cache") - # ------------------------------ - - # Add Steam Credentials if building stages that descend from 'dep' - # 'dep', 'dev', and 'app' all require these for the SteamVR installation step - if args.target in ["dep", "dev", "app"]: - steam_user = os.getenv("STEAM_USER") - steam_pass = os.getenv("STEAM_PASSWORD") - - if not steam_user or not steam_pass: - print("āŒ Error: STEAM_USER and STEAM_PASSWORD environment variables must be set.") - print(" Run: export STEAM_USER='your_user' STEAM_PASSWORD='your_password'") - sys.exit(1) - - cmd.extend([ - "--build-arg", f"STEAM_USER={steam_user}", - "--build-arg", f"STEAM_PASSWORD={steam_pass}" - ]) - - # Important: The context must be repo_root so Docker can see ros2_vive_controller/ and .ci/ - cmd.append(repo_root) - - try: - subprocess.check_call(cmd) - print(f"\nāœ… Build successful: {full_image_name}") - except subprocess.CalledProcessError: - print("\nāŒ Build failed.") - sys.exit(1) - - # Optional Push - if args.push: - print(f"\n[PUSH] Pushing {full_image_name} to registry...") - try: - subprocess.check_call(["docker", "push", full_image_name]) - print(f"āœ… Push successful.") - except subprocess.CalledProcessError: - print(f"\nāŒ Push failed. Are you logged in to {args.registry}?") - sys.exit(1) - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Build Multi-Stage Docker images for Vive Controller") - - parser.add_argument("--registry", default=DEFAULT_REGISTRY, help="Docker registry URL") - - # Matches your Dockerfile: base -> dep -> dev -> app - parser.add_argument("--target", default="dev", choices=["base", "dep", "dev", "app"], - help="Build target stage (base, dep, dev, app)") - - parser.add_argument("--tag", help="Docker tag (default: reads from VERSION file)") - parser.add_argument("--push", action="store_true", help="Push image to registry after build") - - # This was already in your args, but now it is used in the logic above - parser.add_argument("--no-cache", action="store_true", help="Force rebuild of all layers (ignoring cache)") - - args = parser.parse_args() - build_docker(args) \ No newline at end of file diff --git a/scripts/run_docker.py b/scripts/run_docker.py index 298cbd1..31bdf32 100644 --- a/scripts/run_docker.py +++ b/scripts/run_docker.py @@ -141,4 +141,4 @@ def run_docker(args): parser.add_argument("--prod", action="store_true", help="Run in prod mode") args = parser.parse_args() - run_docker(args) \ No newline at end of file + run_docker(args) diff --git a/setup.py b/setup.py index 92074bf..c1cb68b 100644 --- a/setup.py +++ b/setup.py @@ -19,6 +19,9 @@ # 2. Install Config Files (YAML) (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + + # 3. Install RViz Files (os.path.join('share', package_name, 'rviz'), glob('rviz/*.rviz')), ],