Use head and hand schema in XRIO (#18)

jiwenc-nv · web-flow · commit e9113ae78719 · 2025-12-18T11:09:15.000-08:00
* Use FlatBuffer for head pose in xrio

* Use FlatBuffer for hand pose in xrio
diff --git a/examples/oxr/cpp/oxr_session_sharing.cpp b/examples/oxr/cpp/oxr_session_sharing.cpp
@@ -112,7 +112,13 @@ int main()
         {
             std::cout << "Frame " << i << ": "
                       << "Hands=" << (left.is_active ? "ACTIVE" : "INACTIVE") << " | "
-                      << "Head=" << (head.is_valid ? "VALID" : "INVALID") << std::endl;
+                      << "Head=" << (head.is_valid ? "VALID" : "INVALID");
+            if (head.is_valid && head.pose)
+            {
+                const auto& pos = head.pose->position();
+                std::cout << " [" << pos.x() << ", " << pos.y() << ", " << pos.z() << "]";
+            }
+            std::cout << std::endl;
         }
 
         std::this_thread::sleep_for(std::chrono::milliseconds(16));
diff --git a/examples/oxr/cpp/oxr_simple_api_demo.cpp b/examples/oxr/cpp/oxr_simple_api_demo.cpp
@@ -121,9 +121,10 @@ int main()
         std::cout << "  Right hand: " << (right.is_active ? "ACTIVE" : "INACTIVE") << std::endl;
         std::cout << "  Head pose:  " << (head.is_valid ? "VALID" : "INVALID") << std::endl;
 
-        if (head.is_valid)
+        if (head.is_valid && head.pose)
         {
-            std::cout << "    Position: [" << head.position[0] << ", " << head.position[1] << ", " << head.position[2]
+            const auto& pos = head.pose->position();
+            std::cout << "    Position: [" << pos.x() << ", " << pos.y() << ", " << pos.z()
                       << "]" << std::endl;
         }
         std::cout << std::endl;
diff --git a/examples/oxr/python/modular_example.py b/examples/oxr/python/modular_example.py
@@ -87,24 +87,24 @@ def main():
                         # Get hand data
                         left = hand_tracker.get_left_hand()
                         right = hand_tracker.get_right_hand()
-                        
+
                         print(f"  Hands: Left={'ACTIVE' if left.is_active else 'INACTIVE':8s} | "
                               f"Right={'ACTIVE' if right.is_active else 'INACTIVE':8s}")
-                        
-                        if left.is_active:
-                            wrist = left.get_joint(xrio.JOINT_WRIST)
+
+                        if left.is_active and left.joints:
+                            wrist = left.joints[xrio.JOINT_WRIST]
                             if wrist.is_valid:
-                                pos = wrist.position
-                                print(f"    Left wrist: [{pos[0]:6.3f}, {pos[1]:6.3f}, {pos[2]:6.3f}]")
-                        
-                        # Get head data
+                                pos = wrist.pose.position
+                                print(f"    Left wrist: [{pos.x:6.3f}, {pos.y:6.3f}, {pos.z:6.3f}]")
+
+                        # Get head data (returns HeadPoseT from schema)
                         head = head_tracker.get_head()
                         print(f"  Head: {'VALID' if head.is_valid else 'INVALID':8s}")
-                        
-                        if head.is_valid:
-                            pos = head.position
-                            print(f"    Head position: [{pos[0]:6.3f}, {pos[1]:6.3f}, {pos[2]:6.3f}]")
-                        
+
+                        if head.is_valid and head.pose:
+                            pos = head.pose.position
+                            print(f"    Head position: [{pos.x:6.3f}, {pos.y:6.3f}, {pos.z:6.3f}]")
+
                         print()
                     
                     frame_count += 1
diff --git a/examples/oxr/python/test_extensions.py b/examples/oxr/python/test_extensions.py
@@ -12,6 +12,7 @@
 import sys
 import teleopcore.xrio as xrio
 import teleopcore.oxr as oxr
+from teleopcore.schema import HeadPoseT
 
 print("=" * 80)
 print("OpenXR Required Extensions Test")
@@ -107,11 +108,15 @@
                 # Quick update test
                 if session.update():
                     left = hand.get_left_hand()
-                    head_pose = head.get_head()
+                    head_pose: HeadPoseT = head.get_head()
                     print(f"  ✅ Update successful")
                     print(f"    Hands: {'ACTIVE' if left.is_active else 'INACTIVE'}")
                     print(f"    Head:  {'VALID' if head_pose.is_valid else 'INVALID'}")
-                
+
+                    if head_pose.is_valid and head_pose.pose:
+                        pos = head_pose.pose.position
+                        print(f"    Head position: [{pos.x:.3f}, {pos.y:.3f}, {pos.z:.3f}]")
+
                 # Session will be cleaned up when exiting 'with' block (RAII)
         else:
             print("  ❌ Failed to initialize")
diff --git a/examples/oxr/python/test_modular.py b/examples/oxr/python/test_modular.py
@@ -13,6 +13,7 @@
 try:
     import teleopcore.xrio as xrio
     import teleopcore.oxr as oxr
+    import teleopcore.schema as schema
 except ImportError as e:
     print(f"Error: {e}")
     print("Make sure the module is built")
@@ -85,24 +86,24 @@
         right = hand_tracker.get_right_hand()
         print(f"  Left hand: {'ACTIVE' if left.is_active else 'INACTIVE'}")
         print(f"  Right hand: {'ACTIVE' if right.is_active else 'INACTIVE'}")
-        
-        if left.is_active:
-            wrist = left.get_joint(xrio.JOINT_WRIST)
+
+        if left.is_active and left.joints:
+            wrist = left.joints[xrio.JOINT_WRIST]
             if wrist.is_valid:
-                pos = wrist.position
-                print(f"  Left wrist position: [{pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}]")
+                pos = wrist.pose.position
+                print(f"  Left wrist position: [{pos.x:.3f}, {pos.y:.3f}, {pos.z:.3f}]")
         print()
-        
-        # Test 7: Check head data
+
+        # Test 7: Check head data (returns HeadPoseT from schema)
         print("[Test 7] Checking head tracking data...")
-        head = head_tracker.get_head()
+        head: schema.HeadPoseT = head_tracker.get_head()
         print(f"  Head: {'VALID' if head.is_valid else 'INVALID'}")
-        
-        if head.is_valid:
-            pos = head.position
-            ori = head.orientation
-            print(f"  Head position: [{pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}]")
-            print(f"  Head orientation: [{ori[0]:.3f}, {ori[1]:.3f}, {ori[2]:.3f}, {ori[3]:.3f}]")
+
+        if head.is_valid and head.pose:
+            pos = head.pose.position
+            ori = head.pose.orientation
+            print(f"  Head position: [{pos.x:.3f}, {pos.y:.3f}, {pos.z:.3f}]")
+            print(f"  Head orientation: [{ori.x:.3f}, {ori.y:.3f}, {ori.z:.3f}, {ori.w:.3f}]")
         print()
         
         # Test 8: Run tracking loop
diff --git a/examples/oxr/python/test_session_sharing.py b/examples/oxr/python/test_session_sharing.py
@@ -126,17 +126,17 @@
             print(f"[{elapsed:4.1f}s] Frame {frame_count:3d}:")
             print(f"  Hands: {'ACTIVE' if left.is_active else 'INACTIVE':8s}")
             print(f"  Head:  {'VALID' if head.is_valid else 'INVALID':8s}")
-            
-            if left.is_active:
-                wrist = left.get_joint(xrio.JOINT_WRIST)
+
+            if left.is_active and left.joints:
+                wrist = left.joints[xrio.JOINT_WRIST]
                 if wrist.is_valid:
-                    pos = wrist.position
-                    print(f"    Left wrist: [{pos[0]:6.3f}, {pos[1]:6.3f}, {pos[2]:6.3f}]")
-            
-            if head.is_valid:
-                pos = head.position
-                print(f"    Head pos:   [{pos[0]:6.3f}, {pos[1]:6.3f}, {pos[2]:6.3f}]")
-            
+                    pos = wrist.pose.position
+                    print(f"    Left wrist: [{pos.x:6.3f}, {pos.y:6.3f}, {pos.z:6.3f}]")
+
+            if head.is_valid and head.pose:
+                pos = head.pose.position
+                print(f"    Head pos:   [{pos.x:6.3f}, {pos.y:6.3f}, {pos.z:6.3f}]")
+
             print()
         
         frame_count += 1
diff --git a/examples/oxr/python/test_synthetic_hands.py b/examples/oxr/python/test_synthetic_hands.py
@@ -117,11 +117,12 @@ def run_test():
                     print(f"Frame {frame_count}:")
                     print(f"  Left Hand: {'ACTIVE' if left.is_active else 'INACTIVE'}")
                     print(f"  Right Hand: {'ACTIVE' if right.is_active else 'INACTIVE'}")
-                    
-                    if left.is_active:
-                        wrist = left.get_joint(xrio.JOINT_WRIST)
+
+                    if left.is_active and left.joints:
+                        wrist = left.joints[xrio.JOINT_WRIST]
                         if wrist.is_valid:
-                            print(f"    Left Wrist: {wrist.position}")
+                            pos = wrist.pose.position
+                            print(f"    Left Wrist: [{pos.x:.3f}, {pos.y:.3f}, {pos.z:.3f}]")
                     print()
                     
                 frame_count += 1
diff --git a/src/core/xrio/cpp/CMakeLists.txt b/src/core/xrio/cpp/CMakeLists.txt
@@ -40,6 +40,9 @@ target_link_libraries(xrio_core
         # oxr_utils is header-only and provides OpenXRSessionHandles struct
         # It also brings in OpenXR headers for base types without linking to OpenXR loader
         oxr::oxr_utils
+
+        # Schema library for FlatBuffer types
+        teleopcore_schema
 )
 
 # Platform-specific definitions
diff --git a/src/core/xrio/cpp/handtracker.cpp b/src/core/xrio/cpp/handtracker.cpp
@@ -117,17 +117,17 @@ bool HandTracker::Impl::update(XrTime time)
     return left_ok || right_ok;
 }
 
-const HandData& HandTracker::Impl::get_left_hand() const
+const HandPoseT& HandTracker::Impl::get_left_hand() const
 {
     return left_hand_;
 }
 
-const HandData& HandTracker::Impl::get_right_hand() const
+const HandPoseT& HandTracker::Impl::get_right_hand() const
 {
     return right_hand_;
 }
 
-bool HandTracker::Impl::update_hand(XrHandTrackerEXT tracker, XrTime time, HandData& out_data)
+bool HandTracker::Impl::update_hand(XrHandTrackerEXT tracker, XrTime time, HandPoseT& out_data)
 {
     XrHandJointsLocateInfoEXT locate_info{ XR_TYPE_HAND_JOINTS_LOCATE_INFO_EXT };
     locate_info.baseSpace = base_space_;
@@ -150,23 +150,37 @@ bool HandTracker::Impl::update_hand(XrHandTrackerEXT tracker, XrTime time, HandD
     out_data.is_active = locations.isActive;
     out_data.timestamp = time;
 
-    for (uint32_t i = 0; i < 26; ++i)
+    // Ensure joints struct is allocated
+    if (!out_data.joints)
     {
-        const auto& joint_loc = joint_locations[i];
-        auto& joint_data = out_data.joints[i];
+        out_data.joints = std::make_unique<HandJoints>();
+    }
 
-        joint_data.position[0] = joint_loc.pose.position.x;
-        joint_data.position[1] = joint_loc.pose.position.y;
-        joint_data.position[2] = joint_loc.pose.position.z;
+    // Get mutable access to the joints array
+    auto* mutable_array = const_cast<flatbuffers::Array<HandJointPose, 26>*>(out_data.joints->poses());
 
-        joint_data.orientation[0] = joint_loc.pose.orientation.x;
-        joint_data.orientation[1] = joint_loc.pose.orientation.y;
-        joint_data.orientation[2] = joint_loc.pose.orientation.z;
-        joint_data.orientation[3] = joint_loc.pose.orientation.w;
+    for (uint32_t i = 0; i < 26; ++i)
+    {
+        const auto& joint_loc = joint_locations[i];
 
-        joint_data.radius = joint_loc.radius;
-        joint_data.is_valid = (joint_loc.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) &&
-                              (joint_loc.locationFlags & XR_SPACE_LOCATION_ORIENTATION_VALID_BIT);
+        // Create Pose from position and orientation using FlatBuffers structs
+        Point position(
+            joint_loc.pose.position.x,
+            joint_loc.pose.position.y,
+            joint_loc.pose.position.z);
+        Quaternion orientation(
+            joint_loc.pose.orientation.x,
+            joint_loc.pose.orientation.y,
+            joint_loc.pose.orientation.z,
+            joint_loc.pose.orientation.w);
+        Pose pose(position, orientation);
+
+        bool is_valid = (joint_loc.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) &&
+                        (joint_loc.locationFlags & XR_SPACE_LOCATION_ORIENTATION_VALID_BIT);
+
+        // Create HandJointPose and set it in the array
+        HandJointPose joint_pose(pose, is_valid, joint_loc.radius);
+        mutable_array->Mutate(i, joint_pose);
     }
 
     return true;
@@ -190,18 +204,18 @@ std::vector<std::string> HandTracker::get_required_extensions() const
     return { XR_EXT_HAND_TRACKING_EXTENSION_NAME };
 }
 
-const HandData& HandTracker::get_left_hand() const
+const HandPoseT& HandTracker::get_left_hand() const
 {
-    static const HandData empty_data{};
+    static const HandPoseT empty_data{};
     auto impl = cached_impl_.lock();
     if (!impl)
         return empty_data;
     return impl->get_left_hand();
 }
 
-const HandData& HandTracker::get_right_hand() const
+const HandPoseT& HandTracker::get_right_hand() const
 {
-    static const HandData empty_data{};
+    static const HandPoseT empty_data{};
     auto impl = cached_impl_.lock();
     if (!impl)
         return empty_data;
diff --git a/src/core/xrio/cpp/headtracker.cpp b/src/core/xrio/cpp/headtracker.cpp
@@ -59,28 +59,28 @@ bool HeadTracker::Impl::update(XrTime time)
 
     if (head_.is_valid)
     {
-        // Copy position
-        head_.position[0] = location.pose.position.x;
-        head_.position[1] = location.pose.position.y;
-        head_.position[2] = location.pose.position.z;
-
-        // Copy orientation
-        head_.orientation[0] = location.pose.orientation.x;
-        head_.orientation[1] = location.pose.orientation.y;
-        head_.orientation[2] = location.pose.orientation.z;
-        head_.orientation[3] = location.pose.orientation.w;
+        // Create pose from position and orientation using FlatBuffers structs
+        Point position(
+            location.pose.position.x,
+            location.pose.position.y,
+            location.pose.position.z);
+        Quaternion orientation(
+            location.pose.orientation.x,
+            location.pose.orientation.y,
+            location.pose.orientation.z,
+            location.pose.orientation.w);
+        head_.pose = std::make_shared<Pose>(position, orientation);
     }
     else
     {
-        // Invalid - zero out
-        memset(head_.position, 0, sizeof(head_.position));
-        memset(head_.orientation, 0, sizeof(head_.orientation));
+        // Invalid - reset pose
+        head_.pose.reset();
     }
 
     return true;
 }
 
-const HeadPose& HeadTracker::Impl::get_head() const
+const HeadPoseT& HeadTracker::Impl::get_head() const
 {
     return head_;
 }
@@ -104,9 +104,9 @@ std::vector<std::string> HeadTracker::get_required_extensions() const
     return {};
 }
 
-const HeadPose& HeadTracker::get_head() const
+const HeadPoseT& HeadTracker::get_head() const
 {
-    static const HeadPose empty_pose{};
+    static const HeadPoseT empty_pose{};
     auto impl = cached_impl_.lock();
     if (!impl)
         return empty_pose;
diff --git a/src/core/xrio/cpp/inc/xrio/handtracker.hpp b/src/core/xrio/cpp/inc/xrio/handtracker.hpp
diff --git a/src/core/xrio/cpp/inc/xrio/headtracker.hpp b/src/core/xrio/cpp/inc/xrio/headtracker.hpp
diff --git a/src/core/xrio/python/xrio_bindings.cpp b/src/core/xrio/python/xrio_bindings.cpp
diff --git a/src/core/xrio/python/xrio_init.py b/src/core/xrio/python/xrio_init.py

Original file line number	Diff line number	Diff line change
`@@ -112,7 +112,13 @@ int main()`
`112`	`112`	`{`
`113`	`113`	`std::cout << "Frame " << i << ": "`
`114`	`114`	`<< "Hands=" << (left.is_active ? "ACTIVE" : "INACTIVE") << " \| "`
`115`		`- << "Head=" << (head.is_valid ? "VALID" : "INVALID") << std::endl;`
	`115`	`+ << "Head=" << (head.is_valid ? "VALID" : "INVALID");`
	`116`	`+ if (head.is_valid && head.pose)`
	`117`	`+ {`
	`118`	`+ const auto& pos = head.pose->position();`
	`119`	`+ std::cout << " [" << pos.x() << ", " << pos.y() << ", " << pos.z() << "]";`
	`120`	`+ }`
	`121`	`+ std::cout << std::endl;`
`116`	`122`	`}`
`117`	`123`
`118`	`124`	`std::this_thread::sleep_for(std::chrono::milliseconds(16));`
Original file line number	Diff line number	Diff line change
`@@ -121,9 +121,10 @@ int main()`
`121`	`121`	`std::cout << " Right hand: " << (right.is_active ? "ACTIVE" : "INACTIVE") << std::endl;`
`122`	`122`	`std::cout << " Head pose: " << (head.is_valid ? "VALID" : "INVALID") << std::endl;`
`123`	`123`
`124`		`- if (head.is_valid)`
	`124`	`+ if (head.is_valid && head.pose)`
`125`	`125`	`{`
`126`		`- std::cout << " Position: [" << head.position[0] << ", " << head.position[1] << ", " << head.position[2]`
	`126`	`+ const auto& pos = head.pose->position();`
	`127`	`+ std::cout << " Position: [" << pos.x() << ", " << pos.y() << ", " << pos.z()`
`127`	`128`	`<< "]" << std::endl;`
`128`	`129`	`}`
`129`	`130`	`std::cout << std::endl;`
Original file line number	Diff line number	Diff line change
`@@ -40,6 +40,9 @@ target_link_libraries(xrio_core`
`40`	`40`	`# oxr_utils is header-only and provides OpenXRSessionHandles struct`
`41`	`41`	`# It also brings in OpenXR headers for base types without linking to OpenXR loader`
`42`	`42`	`oxr::oxr_utils`
	`43`	`+`
	`44`	`+ # Schema library for FlatBuffer types`
	`45`	`+ teleopcore_schema`
`43`	`46`	`)`
`44`	`47`
`45`	`48`	`# Platform-specific definitions`