From 0fad2cc33f4f369dded091a492c743d892bfbb65 Mon Sep 17 00:00:00 2001 From: TOTSILA Dionis Date: Thu, 12 Mar 2026 23:41:26 +0100 Subject: [PATCH 1/4] franka cyclonedds --- config/cyclonedds.xml | 28 +++------------------------- 1 file changed, 3 insertions(+), 25 deletions(-) diff --git a/config/cyclonedds.xml b/config/cyclonedds.xml index 0020c7e..643daa0 100644 --- a/config/cyclonedds.xml +++ b/config/cyclonedds.xml @@ -1,30 +1,8 @@ - false - 1400B - - - - + enx607d09178f25 + true - - auto - - - - - - 500 - - - - 2000kB - - - - config - ${HOME}/.ros/log/cdds.log - - \ No newline at end of file + From 86bf2bfbe165cde45bd86d063335868f115d277b Mon Sep 17 00:00:00 2001 From: TOTSILA Dionis Date: Tue, 17 Mar 2026 10:12:57 +0100 Subject: [PATCH 2/4] add linear scale, use only right option, expose controller ids --- launch/franka_single.launch.py | 58 +++++++++++++++++++------ launch/g1_dual.launch.py | 78 +++++++++++++++++----------------- launch/tiago_dual.launch.py | 68 +++++++++++++++-------------- launch/vive_teleop.launch.py | 58 +++++++++---------------- 4 files changed, 139 insertions(+), 123 deletions(-) diff --git a/launch/franka_single.launch.py b/launch/franka_single.launch.py index c6c8354..be82014 100644 --- a/launch/franka_single.launch.py +++ b/launch/franka_single.launch.py @@ -7,17 +7,35 @@ from launch_ros.actions import Node def generate_launch_description(): - # 1. Locate the vive_teleop.launch.py file + # 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' + ) + + 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', @@ -30,14 +48,25 @@ def generate_launch_description(): 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' ) - # 5. Teleop bridge node - RIGHT + # --- 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'), + 'only_right': LaunchConfiguration('only_right'), + 'linear_scale': LaunchConfiguration('linear_scale') # Passed to vive_node + }.items() + ) + + # --- Teleop Bridge Node --- teleop_bridge_franka_tcp = Node( package='ros2_vive_controller', executable='teleop_bridge_node', @@ -50,21 +79,24 @@ 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) --- + # Button Mapping '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([ + serial_right_arg, + serial_left_arg, + only_right_arg, + linear_scale_arg, include_vive_teleop, publish_frequency_arg, reference_frame_arg, diff --git a/launch/g1_dual.launch.py b/launch/g1_dual.launch.py index af97774..03b57e2 100644 --- a/launch/g1_dual.launch.py +++ b/launch/g1_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' - ) - - target_frame_left_arg = DeclareLaunchArgument( - 'target_frame_left', - default_value='ci/gripper_left_grasping_frame', - description='Target frame for left controller' + 'reference_frame', default_value='ci/base_link' ) - 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', + 'linear_scale': LaunchConfiguration('linear_scale') + }.items() ) - # 4. Teleop bridge node - LEFT + # --- Teleop bridge node - LEFT --- teleop_bridge_left = Node( package='ros2_vive_controller', executable='teleop_bridge_node', @@ -53,23 +54,23 @@ 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='ci/gripper_left_grasping_frame'), 'reference_frame': LaunchConfiguration('reference_frame'), + 'rotation_offset': [0.0, 0.0, 0.0], - # --- MANUAL BUTTON TOPIC MAPPING (LEFT) --- + # --- G1 Specific Mappings --- 'trigger_topic': '/vive/left/trigger', - # 'trackpad_x_topic': '/vive/left/trackpad_x', 'trackpad_x_topic': '/g1pilot/left_hand/dx3/action', 'trackpad_y_topic': '/vive/left/trackpad_y', 'grip_topic': '/vive/left/grip', 'menu_topic': '/vive/left/menu', 'trackpad_touched_topic': '/vive/left/trackpad_touched', 'trackpad_pressed_topic': '/vive/left/trackpad_pressed', - 'trackpad_pressed_required' : 'true' # Only publish when trackpad is pressed + 'trackpad_pressed_required' : 'true' }] ) - # 5. Teleop bridge node - RIGHT + # --- Teleop bridge node - RIGHT --- teleop_bridge_right = Node( package='ros2_vive_controller', executable='teleop_bridge_node', @@ -80,28 +81,29 @@ 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='ci/gripper_right_grasping_frame'), 'reference_frame': LaunchConfiguration('reference_frame'), + 'rotation_offset': [0.0, 0.0, 0.0], - # --- MANUAL BUTTON TOPIC MAPPING (RIGHT) --- + # --- G1 Specific Mappings --- 'trigger_topic': '/vive/right/trigger', - # 'trackpad_x_topic': '/vive/right/trackpad_x', - 'trackpad_y_topic': '/vive/right/trackpad_y', 'trackpad_x_topic': '/g1pilot/right_hand/dx3/action', + 'trackpad_y_topic': '/vive/right/trackpad_y', 'grip_topic': '/vive/right/grip', 'menu_topic': '/vive/right/menu', 'trackpad_touched_topic': '/vive/right/trackpad_touched', 'trackpad_pressed_topic': '/vive/right/trackpad_pressed', - 'trackpad_pressed_required' : 'true' # + 'trackpad_pressed_required' : 'true' }] ) 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_dual.launch.py b/launch/tiago_dual.launch.py index 234a4e3..b33528e 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='ci/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='ci/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='ci/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/vive_teleop.launch.py b/launch/vive_teleop.launch.py index c946d41..1193c89 100644 --- a/launch/vive_teleop.launch.py +++ b/launch/vive_teleop.launch.py @@ -1,76 +1,55 @@ 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 # <--- Added 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('tracking_reference', default_value='LHB-DFA5BD2C'), + # --- NEW: Added these to match the Franka launch expectations --- DeclareLaunchArgument( - 'serial_left', - default_value='LHR-97752221', - description='Serial number for the left controller' + 'only_right', + default_value='false', + description='If true, the left controller node is skipped.' ), - - DeclareLaunchArgument( - 'serial_right', - default_value='LHR-9ABF6D66', - description='Serial number for the right controller' - ), - 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 --- + # --- LEFT HAND DRIVER (Modified) --- Node( package='ros2_vive_controller', executable='vive_node', name='driver_left', namespace='vive/left', output='screen', + # --- Added Condition --- + 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')} ] ), - # --- RIGHT HAND DRIVER --- + # --- RIGHT HAND DRIVER (Modified) --- TimerAction(period=3.0, actions=[ Node( package='ros2_vive_controller', @@ -81,6 +60,7 @@ def generate_launch_description(): parameters=[ vive_params_file, {'side': 'right', + 'linear_scale': LaunchConfiguration('linear_scale'), # <--- Added Parameter 'serial': LaunchConfiguration('serial_right'), 'htc_vive.tracking_reference': LaunchConfiguration('tracking_reference') } From f2cb946193eaeb6bfe3d1f354bc5af3ec8ff2fe9 Mon Sep 17 00:00:00 2001 From: TOTSILA Dionis Date: Tue, 17 Mar 2026 12:32:44 +0100 Subject: [PATCH 3/4] docker compose istead of scripts --- .ci/entrypoint.sh | 14 ++++ Makefile | 79 +++++++++++++++++++ config/cyclonedds.xml | 23 +++++- config/franka_cyclonedds.xml | 23 ++++++ config/tiago_cyclonedds.xml | 23 ++++++ docker-compose.yml | 115 ++++++++++++++++++++++++---- ros2_vive_controller/vive_node.py | 121 ++++++++++++++++-------------- setup.py | 3 + 8 files changed, 324 insertions(+), 77 deletions(-) create mode 100755 .ci/entrypoint.sh create mode 100644 Makefile create mode 100644 config/franka_cyclonedds.xml create mode 100644 config/tiago_cyclonedds.xml diff --git a/.ci/entrypoint.sh b/.ci/entrypoint.sh new file mode 100755 index 0000000..abe1f47 --- /dev/null +++ b/.ci/entrypoint.sh @@ -0,0 +1,14 @@ +#!/bin/bash +# Exit immediately if a command exits with a non-zero status. +set -e + +source "/opt/ros/humble/setup.bash" + + +if [ -f "/ros2_ws/install/setup.bash" ]; then + source "/ros2_ws/install/setup.bash" + # Optional: Log that the workspace was found + # echo "✅ Workspace sourced." +fi + +exec "$@" \ No newline at end of file diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..5700196 --- /dev/null +++ b/Makefile @@ -0,0 +1,79 @@ +# --- Configuration --- +# Load variables from .env if it exists +ifneq ("$(wildcard .env)","") + include .env + export $(shell sed 's/=.*//' .env) +endif + +# Default shell +SHELL := /bin/bash + +# --- Help --- +.PHONY: help +help: + @echo "Vive Controller Docker Management" + @echo "Usage: make " + @echo "" + @echo "Robot Missions:" + @echo " make franka - Start Franka (Single Right Controller)" + @echo " make tiago - Start Tiago Dual Arm" + @echo " make g1 - Start G1 Dual Arm" + @echo "" + @echo "Utility:" + @echo " make build - Build the 'app' image (cached)" + @echo " make build-force - Build from scratch (no cache)" + @echo " make calibrate - Run workspace calibration" + @echo " make identify - Vibrate controllers to check IDs" + @echo " make stop - Stop all running vive containers" + @echo " make clean - Remove all vive containers and networks" + +# --- GUI Permissions --- +.PHONY: gui-perms +gui-perms: + @echo "Setting X11 permissions for Docker..." + @xhost + > /dev/null + +# --- Build Targets --- +.PHONY: build +build: + docker compose build + +.PHONY: build-force +build-force: + docker compose build --no-cache + +# --- Robot Missions --- +.PHONY: franka +franka: gui-perms + docker compose --profile franka up + +.PHONY: tiago +tiago: gui-perms + docker compose --profile tiago up + +.PHONY: g1 +g1: gui-perms + docker compose --profile g1 up + +# --- Utilities --- +.PHONY: calibrate +calibrate: gui-perms + # Runs calibration inside the franka container context + docker compose --profile franka run --rm franka ros2 launch ros2_vive_controller calibration.launch.py + +.PHONY: identify +identify: + @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_g1 /entrypoint.sh ros2 service call /vive/right/identify std_srvs/srv/Trigger + @echo "Vibrating LEFT controller..." + -docker exec -it ros2_vive_tiago /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 +.PHONY: stop +stop: + docker compose down + +.PHONY: clean +clean: + docker compose down --remove-orphans --volumes \ No newline at end of file diff --git a/config/cyclonedds.xml b/config/cyclonedds.xml index 643daa0..409cee8 100644 --- a/config/cyclonedds.xml +++ b/config/cyclonedds.xml @@ -1,8 +1,23 @@ - - + + + - enx607d09178f25 + + + true + + + + + + + + auto + 200 + - + \ 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..409cee8 --- /dev/null +++ b/config/tiago_cyclonedds.xml @@ -0,0 +1,23 @@ + + + + + + + + true + + + + + + + + + auto + 200 + + + \ No newline at end of file diff --git a/docker-compose.yml b/docker-compose.yml index 479c40a..734219a 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,85 @@ 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 + 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} + only_right:=true + linear_scale:=${LINEAR_SCALE:-2.0} + + # ── TIAGO (Dual Arm) ───────────────────────────────────────────────────── + tiago: + <<: *base_robot + profiles: [tiago] + container_name: ros2_vive_tiago + 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} + + # ── 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/ros2_vive_controller/vive_node.py b/ros2_vive_controller/vive_node.py index 72d4e58..221f8ec 100644 --- a/ros2_vive_controller/vive_node.py +++ b/ros2_vive_controller/vive_node.py @@ -6,11 +6,10 @@ from std_msgs.msg import Bool from sensor_msgs.msg import JointState from visualization_msgs.msg import Marker +from std_srvs.srv import Trigger # Import standard Trigger service 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') @@ -22,12 +21,11 @@ def __init__(self): ('side', 'right'), ('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) @@ -37,6 +35,7 @@ def __init__(self): self.side = self.get_parameter('side').value self.serial = self.get_parameter('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 = { @@ -53,33 +52,53 @@ def __init__(self): self.vr = triad_openvr() self.device_name = self._find_device_by_serial(self.serial) + # Usage: ros2 service call /vive/right/identify std_srvs/srv/Trigger + self.srv_identify = self.create_service( + Trigger, + 'identify', + self.identify_callback + ) + 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 --- + # --- Publishers & State --- 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.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() + self.tracking_active = False + self.haptic_frames_remaining = 0 # --- Loop --- self.create_timer(0.02, self.update) # 50Hz - - # Publish marker once at startup (and periodically in loop) self.publish_workspace_marker() - # Tracking staate - self.tracking_active = False # Flag to track if we currently have valid tracking - self.haptic_frames_remaining = 0 # Counter for vibration duration + # --- NEW: Service Callback --- + def identify_callback(self, request, response): + """Vibrates the controller on demand to identify which serial is which.""" + if not self.device_name: + response.success = False + response.message = "Controller not connected." + return response + + self.get_logger().info(f"Identifying controller: {self.serial}") + + # Trigger a strong 1-second buzz (150 frames at 3900us) + # We set the counter so the update loop handles it safely without blocking + 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,14 +109,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): self.get_logger().warn("Calibration heartbeat lost! Re-enabling safety limits.") self.is_calibrating = False @@ -112,80 +128,76 @@ 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 True # Safe + return False + return True def update(self): if not self.device_name: return controller = self.vr.devices[self.device_name] + + # --- HAPTIC VIBRATION HANDLER (Moved UP) --- + # This now runs even if optical tracking is lost + if self.haptic_frames_remaining > 0: + controller.trigger_haptic_pulse(3900) + time.sleep(0.005) # Optional: Double pulse for stronger feel + 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) + self.haptic_frames_remaining = 50 - # --- 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 --- + # Safety Check + raw_pos = pose[0:3] + if not self.is_calibrating: + if not self.check_workspace_and_vibrate(raw_pos, controller): + return - # --- 2. Publish Data (Only if Safe) --- + # --- 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.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.position.x = self.filters['x'](pose[0], t) * self.linear_scale + msg_pose.pose.position.y = self.filters['y'](pose[1], t) * self.linear_scale + msg_pose.pose.position.z = self.filters['z'](pose[2], t) * self.linear_scale + 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] - self.pub_pose.publish(msg_pose) - # B. Buttons + # 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.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,9 +209,6 @@ 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() @@ -218,10 +227,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 +240,5 @@ def main(): node.destroy_node() rclpy.shutdown() - if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/setup.py b/setup.py index c6aa319..ee2122c 100644 --- a/setup.py +++ b/setup.py @@ -16,6 +16,9 @@ # 1. 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')), ], From 8e397a957f5d9ed14456400a3530b32389da4f30 Mon Sep 17 00:00:00 2001 From: TOTSILA Dionis Date: Tue, 17 Mar 2026 14:34:52 +0100 Subject: [PATCH 4/4] docker compose changes, add master lighthouse selection for vive node --- docker-compose.yml | 4 + launch/franka_single.launch.py | 41 ++-- launch/vive_teleop.launch.py | 19 +- ros2_vive_controller/list_devices.py | 25 +++ .../openvr_class/openvr_class.py | 6 +- ros2_vive_controller/vive_node.py | 179 +++++++++++++----- scripts/build_docker.py | 109 ----------- scripts/run_docker.py | 132 ------------- 8 files changed, 190 insertions(+), 325 deletions(-) create mode 100644 ros2_vive_controller/list_devices.py delete mode 100644 scripts/build_docker.py delete mode 100644 scripts/run_docker.py diff --git a/docker-compose.yml b/docker-compose.yml index 734219a..cc02aec 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -156,12 +156,15 @@ services: <<: *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} @@ -170,6 +173,7 @@ services: <<: *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 diff --git a/launch/franka_single.launch.py b/launch/franka_single.launch.py index be82014..6bd8bea 100644 --- a/launch/franka_single.launch.py +++ b/launch/franka_single.launch.py @@ -7,51 +7,40 @@ from launch_ros.actions import Node def generate_launch_description(): - # 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') # --- General Launch Arguments --- serial_right_arg = DeclareLaunchArgument( - 'serial_right', - default_value='LHR-9ABF6D66', - description='Serial number for the right controller' + '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' + '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." + '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.' + 'linear_scale', default_value='1.0', description='Scaling factor for controller translation.' ) 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 --- @@ -61,8 +50,9 @@ def generate_launch_description(): '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') # Passed to vive_node + 'linear_scale': LaunchConfiguration('linear_scale') }.items() ) @@ -80,8 +70,6 @@ def generate_launch_description(): 'target_frame': LaunchConfiguration('target_frame_franka_tcp'), 'reference_frame': LaunchConfiguration('reference_frame'), 'rotation_offset': [180.0, 0.0, 0.0], - - # Button Mapping 'trigger_topic': '/vive/right/trigger', 'trackpad_x_topic': '/vive/right/trackpad_x', 'trackpad_y_topic': '/vive/right/trackpad_y', @@ -95,11 +83,12 @@ def generate_launch_description(): return LaunchDescription([ serial_right_arg, serial_left_arg, + reference_lighthouse_serial_arg, only_right_arg, linear_scale_arg, - include_vive_teleop, 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/vive_teleop.launch.py b/launch/vive_teleop.launch.py index 1193c89..6dff50e 100644 --- a/launch/vive_teleop.launch.py +++ b/launch/vive_teleop.launch.py @@ -2,7 +2,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, TimerAction -from launch.conditions import IfCondition, UnlessCondition # <--- Added UnlessCondition +from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -17,9 +17,10 @@ def generate_launch_description(): DeclareLaunchArgument('rviz', default_value='true'), DeclareLaunchArgument('serial_left', default_value='LHR-97752221'), DeclareLaunchArgument('serial_right', default_value='LHR-9ABF6D66'), - DeclareLaunchArgument('tracking_reference', default_value='LHB-DFA5BD2C'), - # --- NEW: Added these to match the Franka launch expectations --- + # --- CHANGED: Name matches parent launch and node parameter --- + DeclareLaunchArgument('reference_lighthouse_serial', default_value=''), + DeclareLaunchArgument( 'only_right', default_value='false', @@ -31,25 +32,24 @@ def generate_launch_description(): description='Translation scaling factor.' ), - # --- LEFT HAND DRIVER (Modified) --- + # --- LEFT HAND DRIVER --- Node( package='ros2_vive_controller', executable='vive_node', name='driver_left', namespace='vive/left', output='screen', - # --- Added Condition --- 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 ] ), - # --- RIGHT HAND DRIVER (Modified) --- + # --- RIGHT HAND DRIVER --- TimerAction(period=3.0, actions=[ Node( package='ros2_vive_controller', @@ -60,10 +60,9 @@ def generate_launch_description(): parameters=[ vive_params_file, {'side': 'right', - 'linear_scale': LaunchConfiguration('linear_scale'), # <--- Added Parameter + '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 221f8ec..6036f4c 100644 --- a/ros2_vive_controller/vive_node.py +++ b/ros2_vive_controller/vive_node.py @@ -1,12 +1,15 @@ #!/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 # Import standard Trigger service +from std_srvs.srv import Trigger +from tf2_ros import TransformBroadcaster from OneEuroFilter import OneEuroFilter from ros2_vive_controller.openvr_class.openvr_class import triad_openvr @@ -20,6 +23,7 @@ def __init__(self): parameters=[ ('side', 'right'), ('serial', ''), + ('reference_lighthouse_serial', ''), ('frame_id', 'vive_world'), ('linear_scale', 1.0), ('workspace.x_min', -1.0), ('workspace.x_max', 1.0), @@ -34,10 +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, @@ -52,23 +56,28 @@ def __init__(self): self.vr = triad_openvr() self.device_name = self._find_device_by_serial(self.serial) - # Usage: ros2 service call /vive/right/identify std_srvs/srv/Trigger - self.srv_identify = self.create_service( - Trigger, - 'identify', - self.identify_callback - ) + # 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}") else: self.get_logger().info(f"Connected to {self.side} controller: {self.serial}") - # --- Publishers & State --- - 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.last_heartbeat_time = 0.0 self.create_subscription(Bool, '/vive/is_calibrating', self.calibration_callback, 10) @@ -78,24 +87,16 @@ def __init__(self): self.tracking_active = False self.haptic_frames_remaining = 0 - # --- Loop --- - self.create_timer(0.02, self.update) # 50Hz - self.publish_workspace_marker() + self.create_timer(0.02, self.update) + self.publish_workspace_marker(self.frame_id) - # --- NEW: Service Callback --- def identify_callback(self, request, response): - """Vibrates the controller on demand to identify which serial is which.""" if not self.device_name: response.success = False response.message = "Controller not connected." return response - self.get_logger().info(f"Identifying controller: {self.serial}") - - # Trigger a strong 1-second buzz (150 frames at 3900us) - # We set the counter so the update loop handles it safely without blocking self.haptic_frames_remaining = 150 - response.success = True response.message = f"Vibrating {self.side} controller ({self.serial})" return response @@ -113,8 +114,7 @@ def calibration_callback(self, msg): def watchdog_check(self): 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 @@ -140,17 +140,77 @@ def check_workspace_and_vibrate(self, pos, device): 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]] + + 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] - # --- HAPTIC VIBRATION HANDLER (Moved UP) --- - # This now runs even if optical tracking is lost + # --- HAPTICS --- if self.haptic_frames_remaining > 0: controller.trigger_haptic_pulse(3900) - time.sleep(0.005) # Optional: Double pulse for stronger feel + time.sleep(0.005) controller.trigger_haptic_pulse(3900) self.haptic_frames_remaining -= 1 @@ -169,8 +229,38 @@ def update(self): self.tracking_active = True self.haptic_frames_remaining = 50 - # Safety Check - raw_pos = pose[0:3] + # --- 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 @@ -181,22 +271,21 @@ def update(self): 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) * self.linear_scale - msg_pose.pose.position.y = self.filters['y'](pose[1], t) * self.linear_scale - msg_pose.pose.position.z = self.filters['z'](pose[2], t) * self.linear_scale + 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 = 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.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) - # Buttons msg_joy = JointState() msg_joy.header.stamp = timestamp - msg_joy.header.frame_id = self.frame_id + 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)), @@ -210,11 +299,11 @@ def update(self): self.pub_joy.publish(msg_joy) 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 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 deleted file mode 100644 index 978a28a..0000000 --- a/scripts/run_docker.py +++ /dev/null @@ -1,132 +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" -CONTAINER_NAME = "ros2_vive_controller" - -def get_project_version(): - 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: - return "latest" - -def run_docker(args): - tag = args.tag if args.tag else get_project_version() - full_image_name = f"{args.registry}/{IMAGE_NAME}:{tag}" - - script_dir = os.path.dirname(os.path.abspath(__file__)) - repo_root = os.path.dirname(script_dir) - - # Mission Aliases - missions = { - "calibrate": "ros2 launch ros2_vive_controller calibration.launch.py", - "teleop": "ros2 launch ros2_vive_controller vive_teleop.launch.py", - "g1": "ros2 launch ros2_vive_controller g1_dual.launch.py", - "tiago": "ros2 launch ros2_vive_controller tiago_dual.launch.py", - "franka": "ros2 launch ros2_vive_controller franka_single.launch.py" - } - - # Resolve command - user_cmd_str = missions.get(args.command, args.command) if args.command else "/bin/bash" - - # 1. Check if container is already running - is_running = subprocess.getoutput(f"docker ps -q -f name={CONTAINER_NAME}") - if is_running: - print(f"--- Container {CONTAINER_NAME} is already running. Joining... ---") - exec_cmd = ["docker", "exec", "-it", CONTAINER_NAME] - exec_cmd.append("/entrypoint.sh") - exec_cmd.extend(user_cmd_str.split()) - subprocess.call(exec_cmd) - return - - # 2. GUI Setup - subprocess.call(["xhost", "+local:docker"], stdout=subprocess.DEVNULL) - - # Path logic - config_dir = os.path.join(repo_root, "config") - steam_cfg = os.path.join(config_dir, "steamvr_config") - - # 3. Construct Docker Run Command - cmd = [ - "docker", "run", "-it", "--rm", - "--name", CONTAINER_NAME, - "--gpus", "all", - "--net", "host", - "--ipc", "host", - "--pid", "host", - "--privileged", - # Environment - "-e", f"DISPLAY={os.getenv('DISPLAY')}", - "-e", "NVIDIA_DRIVER_CAPABILITIES=all", - "-e", f"XDG_RUNTIME_DIR={os.getenv('XDG_RUNTIME_DIR')}", - "-e", "QT_X11_NO_MITSHM=1", - "-e", f"ROS_DOMAIN_ID={args.domain}", - "-e", "RMW_IMPLEMENTATION=rmw_cyclonedds_cpp", - "-e", "CYCLONEDDS_URI=/ros2_ws/src/ros2_vive_controller/config/cyclonedds.xml", - # Hardware - "-v", "/tmp/.X11-unix:/tmp/.X11-unix", - "-v", "/dev:/dev", - "-v", "/run/udev:/run/udev", - # SteamVR - "-v", f"{steam_cfg}/openvrpaths.vrpath:/root/.config/openvr/openvrpaths.vrpath", - "-v", f"{steam_cfg}/default_driver_null:/home/steam/Steam/steamapps/common/SteamVR/drivers/null/resources/settings/default.vrsettings", - "-v", f"{steam_cfg}/default_resources:/home/steam/Steam/steamapps/common/SteamVR/resources/settings/default.vrsettings", - "-w", "/ros2_ws", - ] - - # 4. Granular Mounting Strategy (Dev Mode) - # Instead of mounting the whole root (which overwrites marker files), we mount only what we edit. - if not args.prod: - print(f"[INFO] Running in DEV mode (Granular Hot-Reload)") - - # Mount the Python Source Code (Changes here apply instantly) - cmd.extend(["-v", f"{repo_root}/ros2_vive_controller:/ros2_ws/src/ros2_vive_controller/ros2_vive_controller"]) - - # Mount Launch Files - cmd.extend(["-v", f"{repo_root}/launch:/ros2_ws/src/ros2_vive_controller/launch"]) - - # Mount Config Files - cmd.extend(["-v", f"{repo_root}/config:/ros2_ws/src/ros2_vive_controller/config"]) - - # Mount RViz Configs - cmd.extend(["-v", f"{repo_root}/rviz:/ros2_ws/src/ros2_vive_controller/rviz"]) - - # Mount Assets (Meshes/Images) - cmd.extend(["-v", f"{repo_root}/assets:/ros2_ws/src/ros2_vive_controller/assets"]) - - else: - print(f"[INFO] Running in PROD mode (Using baked-in image code)") - - # 5. Finalize - cmd.append(full_image_name) - cmd.append("/entrypoint.sh") - cmd.extend(user_cmd_str.split()) - - print(f"\n🚀 Launching {CONTAINER_NAME}...") - print(f" Domain ID: {args.domain}") - print(f" Command: {user_cmd_str}\n") - - try: - subprocess.check_call(cmd) - except subprocess.CalledProcessError: - pass - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Run Vive Controller Container") - parser.add_argument("command", nargs="?", help="Mission alias or raw command") - parser.add_argument("--domain", type=int, default=1, help="ROS_DOMAIN_ID") - parser.add_argument("--registry", default=DEFAULT_REGISTRY) - parser.add_argument("--tag", help="Docker tag") - 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