Skip to content

Commit 5e96983

Browse files
nvddrnvddr
committed
Add controller tracker
1 parent 7ba8c93 commit 5e96983

File tree

13 files changed

+1075
-238
lines changed

13 files changed

+1075
-238
lines changed

‎examples/oxr/python/test_controller_tracker.py‎

Lines changed: 203 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,203 @@
1+
#!/usr/bin/env python3
2+
# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
3+
# SPDX-License-Identifier: Apache-2.0
4+
5+
"""
6+
Test script for ControllerTracker with simplified API.
7+
8+
Demonstrates:
9+
- Snapshot mode for continuous state queries (includes poses and inputs)
10+
"""
11+
12+
import sys
13+
import time
14+
15+
try:
16+
import teleopcore.xrio as xrio
17+
import teleopcore.oxr as oxr
18+
except ImportError as e:
19+
print(f"Error: {e}")
20+
print("Make sure the module is built")
21+
sys.exit(1)
22+
23+
print("=" * 80)
24+
print("Controller Tracker Test")
25+
print("=" * 80)
26+
print()
27+
28+
# Test 1: Create controller tracker (handles both controllers)
29+
print("[Test 1] Creating controller tracker...")
30+
controller_tracker = xrio.ControllerTracker()
31+
print(f"✓ {controller_tracker.get_name()} created")
32+
print()
33+
34+
# Test 2: Create builder and add tracker
35+
print("[Test 2] Creating builder...")
36+
builder = xrio.XrioSessionBuilder()
37+
builder.add_tracker(controller_tracker)
38+
print("✓ Tracker added to builder")
39+
print()
40+
41+
# Test 3: Initialize
42+
print("[Test 3] Creating OpenXR session and initializing...")
43+
44+
# Get required extensions
45+
required_extensions = builder.get_required_extensions()
46+
print(f"Required extensions: {required_extensions if required_extensions else 'None (uses core OpenXR)'}")
47+
48+
# Use context managers for proper RAII cleanup
49+
with oxr.OpenXRSession.create("ControllerTrackerTest", required_extensions) as oxr_session:
50+
handles = oxr_session.get_handles()
51+
52+
# Create teleop session
53+
session = builder.build(handles)
54+
if session is None:
55+
print("❌ Failed to initialize")
56+
sys.exit(1)
57+
58+
with session:
59+
print("✅ OpenXR session initialized")
60+
print()
61+
62+
# Test 4: Initial update
63+
print("[Test 4] Testing initial data retrieval...")
64+
if not session.update():
65+
print("❌ Update failed")
66+
sys.exit(1)
67+
68+
print("✓ Update successful")
69+
print()
70+
71+
# Test 5: Check initial controller state (snapshot mode)
72+
print("[Test 5] Checking controller state (snapshot mode)...")
73+
left_snap = controller_tracker.get_snapshot(xrio.Hand.Left)
74+
right_snap = controller_tracker.get_snapshot(xrio.Hand.Right)
75+
print(f" Left controller: {'ACTIVE' if left_snap.is_active else 'INACTIVE'}")
76+
print(f" Right controller: {'ACTIVE' if right_snap.is_active else 'INACTIVE'}")
77+
78+
if left_snap.is_active and left_snap.grip_pose.is_valid:
79+
pos = left_snap.grip_pose.position
80+
print(f" Left grip position: [{pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}]")
81+
82+
if right_snap.is_active and right_snap.grip_pose.is_valid:
83+
pos = right_snap.grip_pose.position
84+
print(f" Right grip position: [{pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}]")
85+
print()
86+
87+
# Test 6: Available inputs
88+
print("[Test 6] Available controller inputs:")
89+
print(" Buttons: primary_click, secondary_click, thumbstick_click")
90+
print(" Axes: thumbstick_x, thumbstick_y, squeeze_value, trigger_value")
91+
print()
92+
93+
# Test 7: Run tracking loop
94+
print("[Test 7] Running controller tracking loop (10 seconds)...")
95+
print("Press buttons or move controls to see state!")
96+
print()
97+
98+
frame_count = 0
99+
start_time = time.time()
100+
last_status_print = start_time
101+
102+
try:
103+
while time.time() - start_time < 10.0:
104+
if not session.update():
105+
print("Update failed")
106+
break
107+
108+
# SNAPSHOT MODE: Get current state (includes poses and inputs)
109+
current_time = time.time()
110+
if current_time - last_status_print >= 0.5: # Print every 0.5 seconds
111+
elapsed = current_time - start_time
112+
left_snap = controller_tracker.get_snapshot(xrio.Hand.Left)
113+
right_snap = controller_tracker.get_snapshot(xrio.Hand.Right)
114+
115+
# Show current state using field access
116+
left_trigger = left_snap.inputs.trigger_value
117+
left_squeeze = left_snap.inputs.squeeze_value
118+
left_stick_x = left_snap.inputs.thumbstick_x
119+
left_stick_y = left_snap.inputs.thumbstick_y
120+
left_primary = left_snap.inputs.primary_click
121+
left_secondary = left_snap.inputs.secondary_click
122+
123+
right_trigger = right_snap.inputs.trigger_value
124+
right_squeeze = right_snap.inputs.squeeze_value
125+
right_stick_x = right_snap.inputs.thumbstick_x
126+
right_stick_y = right_snap.inputs.thumbstick_y
127+
right_primary = right_snap.inputs.primary_click
128+
right_secondary = right_snap.inputs.secondary_click
129+
130+
# Build status strings
131+
left_status = f"Trig={left_trigger:.2f} Sq={left_squeeze:.2f}"
132+
if abs(left_stick_x) > 0.1 or abs(left_stick_y) > 0.1:
133+
left_status += f" Stick=({left_stick_x:+.2f},{left_stick_y:+.2f})"
134+
if left_primary:
135+
left_status += " [X]"
136+
if left_secondary:
137+
left_status += " [Y]"
138+
139+
right_status = f"Trig={right_trigger:.2f} Sq={right_squeeze:.2f}"
140+
if abs(right_stick_x) > 0.1 or abs(right_stick_y) > 0.1:
141+
right_status += f" Stick=({right_stick_x:+.2f},{right_stick_y:+.2f})"
142+
if right_primary:
143+
right_status += " [A]"
144+
if right_secondary:
145+
right_status += " [B]"
146+
147+
print(f" [{elapsed:5.2f}s] Frame {frame_count:4d} | L: {left_status} | R: {right_status}")
148+
last_status_print = current_time
149+
150+
frame_count += 1
151+
time.sleep(0.016) # ~60 FPS
152+
153+
except KeyboardInterrupt:
154+
print("\n⚠️ Interrupted by user")
155+
156+
print()
157+
print(f"✓ Processed {frame_count} frames")
158+
print()
159+
160+
# Test 8: Show final statistics
161+
print("[Test 8] Final controller state...")
162+
163+
def print_controller_summary(hand_name, hand):
164+
snapshot = controller_tracker.get_snapshot(hand)
165+
print(f" {hand_name} Controller:")
166+
if snapshot.is_active:
167+
print(f" Status: ACTIVE")
168+
169+
# Poses from snapshot
170+
grip = snapshot.grip_pose
171+
aim = snapshot.aim_pose
172+
173+
if grip.is_valid:
174+
pos = grip.position
175+
print(f" Grip position: [{pos[0]:+.3f}, {pos[1]:+.3f}, {pos[2]:+.3f}]")
176+
177+
if aim.is_valid:
178+
pos = aim.position
179+
print(f" Aim position: [{pos[0]:+.3f}, {pos[1]:+.3f}, {pos[2]:+.3f}]")
180+
181+
# Input values using field access
182+
inputs = snapshot.inputs
183+
print(f" Trigger: {inputs.trigger_value:.2f}")
184+
print(f" Squeeze: {inputs.squeeze_value:.2f}")
185+
print(f" Thumbstick: ({inputs.thumbstick_x:+.2f}, {inputs.thumbstick_y:+.2f})")
186+
print(f" Primary: {'PRESSED' if inputs.primary_click else 'released'}")
187+
print(f" Secondary: {'PRESSED' if inputs.secondary_click else 'released'}")
188+
else:
189+
print(f" Status: INACTIVE")
190+
191+
print_controller_summary("Left", xrio.Hand.Left)
192+
print()
193+
print_controller_summary("Right", xrio.Hand.Right)
194+
print()
195+
196+
# Cleanup
197+
print("[Test 9] Cleanup...")
198+
print("✓ Resources will be cleaned up when exiting 'with' blocks (RAII)")
199+
print()
200+
201+
print("=" * 80)
202+
print("✅ All tests passed")
203+
print("=" * 80)

‎src/core/oxr/cpp/inc/oxr/oxr_session.hpp‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -31,10 +31,11 @@ class OpenXRSession
3131
OpenXRSession();
3232

3333
// Initialization methods
34-
bool create_instance(const std::string& app_name, const std::vector<std::string>& extensions);
35-
bool create_system();
36-
bool create_session();
37-
bool create_reference_space();
34+
void create_instance(const std::string& app_name, const std::vector<std::string>& extensions);
35+
void create_system();
36+
void create_session();
37+
void create_reference_space();
38+
void begin();
3839

3940
XrInstance instance_;
4041
XrSystemId system_id_;

‎src/core/oxr/cpp/oxr_session.cpp‎

Lines changed: 55 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55

66
#include <cstring>
77
#include <iostream>
8+
#include <stdexcept>
89

910
namespace core
1011
{
@@ -39,28 +40,14 @@ OpenXRSession::~OpenXRSession()
3940
std::shared_ptr<OpenXRSession> OpenXRSession::Create(const std::string& app_name,
4041
const std::vector<std::string>& extensions)
4142
{
42-
4343
auto session = std::shared_ptr<OpenXRSession>(new OpenXRSession());
4444

45-
if (!session->create_instance(app_name, extensions))
46-
{
47-
return nullptr;
48-
}
49-
50-
if (!session->create_system())
51-
{
52-
return nullptr;
53-
}
54-
55-
if (!session->create_session())
56-
{
57-
return nullptr;
58-
}
59-
60-
if (!session->create_reference_space())
61-
{
62-
return nullptr;
63-
}
45+
// These methods throw on failure, no need to check return values
46+
session->create_instance(app_name, extensions);
47+
session->create_system();
48+
session->create_session();
49+
session->create_reference_space();
50+
session->begin();
6451

6552
return session;
6653
}
@@ -71,7 +58,8 @@ OpenXRSessionHandles OpenXRSession::get_handles() const
7158
return OpenXRSessionHandles(instance_, session_, space_, ::xrGetInstanceProcAddr);
7259
}
7360

74-
bool OpenXRSession::create_instance(const std::string& app_name, const std::vector<std::string>& extensions)
61+
62+
void OpenXRSession::create_instance(const std::string& app_name, const std::vector<std::string>& extensions)
7563
{
7664
XrInstanceCreateInfo create_info{ XR_TYPE_INSTANCE_CREATE_INFO };
7765
create_info.applicationInfo.apiVersion = XR_CURRENT_API_VERSION;
@@ -98,31 +86,27 @@ bool OpenXRSession::create_instance(const std::string& app_name, const std::vect
9886
XrResult result = xrCreateInstance(&create_info, &instance_);
9987
if (XR_FAILED(result))
10088
{
101-
std::cerr << "Failed to create OpenXR instance: " << result << std::endl;
102-
return false;
89+
throw std::runtime_error("Failed to create OpenXR instance: " + std::to_string(result));
10390
}
10491

10592
std::cout << "Created OpenXR instance" << std::endl;
106-
return true;
10793
}
10894

109-
bool OpenXRSession::create_system()
95+
void OpenXRSession::create_system()
11096
{
11197
XrSystemGetInfo system_info{ XR_TYPE_SYSTEM_GET_INFO };
11298
system_info.formFactor = XR_FORM_FACTOR_HEAD_MOUNTED_DISPLAY;
11399

114100
XrResult result = xrGetSystem(instance_, &system_info, &system_id_);
115101
if (XR_FAILED(result))
116102
{
117-
std::cerr << "Failed to get OpenXR system: " << result << std::endl;
118-
return false;
103+
throw std::runtime_error("Failed to get OpenXR system: " + std::to_string(result));
119104
}
120105

121106
std::cout << "Created OpenXR system" << std::endl;
122-
return true;
123107
}
124108

125-
bool OpenXRSession::create_session()
109+
void OpenXRSession::create_session()
126110
{
127111
// XrSessionCreateInfoOverlayEXTX structure for overlay/headless mode
128112
struct XrSessionCreateInfoOverlayEXTX
@@ -146,16 +130,14 @@ bool OpenXRSession::create_session()
146130
XrResult result = xrCreateSession(instance_, &create_info, &session_);
147131
if (XR_FAILED(result))
148132
{
149-
std::cerr << "Failed to create OpenXR session: " << result << std::endl;
150-
return false;
133+
throw std::runtime_error("Failed to create OpenXR session: " + std::to_string(result));
151134
}
152135

153136
std::cout << "Created OpenXR session (headless mode)" << std::endl;
154137
std::cout << " Session handle: " << session_ << std::endl;
155-
return true;
156138
}
157139

158-
bool OpenXRSession::create_reference_space()
140+
void OpenXRSession::create_reference_space()
159141
{
160142
XrReferenceSpaceCreateInfo create_info{ XR_TYPE_REFERENCE_SPACE_CREATE_INFO };
161143
create_info.referenceSpaceType = XR_REFERENCE_SPACE_TYPE_STAGE;
@@ -164,13 +146,50 @@ bool OpenXRSession::create_reference_space()
164146
XrResult result = xrCreateReferenceSpace(session_, &create_info, &space_);
165147
if (XR_FAILED(result))
166148
{
167-
std::cerr << "Failed to create reference space: " << result << std::endl;
168-
return false;
149+
throw std::runtime_error("Failed to create reference space: " + std::to_string(result));
169150
}
170151

171152
std::cout << "Created reference space" << std::endl;
172153
std::cout << " Space handle: " << space_ << std::endl;
173-
return true;
154+
}
155+
156+
void OpenXRSession::begin()
157+
{
158+
// Enumerate view configurations to find a valid one
159+
uint32_t view_config_count = 0;
160+
XrResult result = xrEnumerateViewConfigurations(instance_, system_id_, 0, &view_config_count, nullptr);
161+
if (XR_FAILED(result) || view_config_count == 0)
162+
{
163+
throw std::runtime_error("Failed to enumerate view configurations: " + std::to_string(result));
164+
}
165+
166+
std::vector<XrViewConfigurationType> view_configs(view_config_count);
167+
result =
168+
xrEnumerateViewConfigurations(instance_, system_id_, view_config_count, &view_config_count, view_configs.data());
169+
if (XR_FAILED(result))
170+
{
171+
throw std::runtime_error("Failed to get view configurations: " + std::to_string(result));
172+
}
173+
174+
// Find the primary stereo view configuration (preferred), or use the first available
175+
XrViewConfigurationType selected_view_config = view_configs[0];
176+
for (const auto& config : view_configs)
177+
{
178+
if (config == XR_VIEW_CONFIGURATION_TYPE_PRIMARY_STEREO)
179+
{
180+
selected_view_config = config;
181+
break;
182+
}
183+
}
184+
185+
XrSessionBeginInfo begin_info{ XR_TYPE_SESSION_BEGIN_INFO };
186+
begin_info.primaryViewConfigurationType = selected_view_config;
187+
188+
result = xrBeginSession(session_, &begin_info);
189+
if (XR_FAILED(result))
190+
{
191+
throw std::runtime_error("Failed to begin OpenXR session: " + std::to_string(result));
192+
}
174193
}
175194

176195
} // namespace core

0 commit comments

Comments
 (0)