Merge pull request #1 from XR-Robotics/dev/hand_tracking

Dev/hand tracking

Author: zhigenzhao

README.md

@@ -68,5 +68,21 @@ print(f"Left Axis: {left_axis}, Right Axis Clicked: {right_axis_click}")
 timestamp = xrt.get_time_stamp_ns()
 print(f"Current Timestamp (ns): {timestamp}")
 
+xrt.close()
+```
+
+**3. Get hand tracking state**
+```python
+import xrobotoolkit_sdk as xrt
+
+xrt.init()
+
+# Left Hand State
+left_hand_tracking_state = xrt.get_left_hand_tracking_state()
+print(f"Left Hand State: {left_hand_state}")
+# Right Hand State
+right_hand_tracking_state = xrt.get_right_hand_tracking_state()
+print(f"Right Hand State: {right_hand_tracking_state}")
+
 xrt.close()
 ```

bindings/py_bindings.cpp

@@ -15,6 +15,11 @@ std::array<double, 7> LeftControllerPose;
 std::array<double, 7> RightControllerPose;
 std::array<double, 7> HeadsetPose;
 
+std::array<std::array<double, 7>, 26> LeftHandTrackingState;
+double LeftHandScale = 1.0;
+std::array<std::array<double, 7>, 26> RightHandTrackingState;
+double RightHandScale = 1.0;
+
 bool LeftMenuButton;
 double LeftTrigger;
 double LeftGrip;
@@ -37,6 +42,8 @@ std::mutex leftMutex;
 std::mutex rightMutex;
 std::mutex headsetPoseMutex;
 std::mutex timestampMutex;
+std::mutex leftHandMutex;
+std::mutex rightHandMutex;
 
 std::array<double, 7> stringToPoseArray(const std::string& poseStr) {
     std::array<double, 7> result{0};
@@ -60,13 +67,13 @@ void OnPXREAClientCallback(void* context, PXREAClientCallbackType type, int stat
         std::cout << "server disconnect\n" << std::endl;
         break;
     case PXREADeviceFind:
-        std::cout << "device find " << (const char*)userData << std::endl;
+        std::cout << "device found\n" << (const char*)userData << std::endl;
         break;
     case PXREADeviceMissing:
-        std::cout << "device missing " << (const char*)userData << std::endl;
+        std::cout << "device missing\n" << (const char*)userData << std::endl;
         break;
     case PXREADeviceConnect:
-        std::cout << "device connect " << (const char*)userData << status << std::endl;
+        std::cout << "device connect\n" << (const char*)userData << status << std::endl;
         break;
     case PXREADeviceStateJson:
         auto& dsj = *((PXREADevStateJson*)userData);
@@ -115,6 +122,27 @@ void OnPXREAClientCallback(void* context, PXREAClientCallbackType type, int stat
                     std::lock_guard<std::mutex> lock(timestampMutex);
                     TimeStampNs = value["timeStampNs"].get<int64_t>();
                 }
+                if (value["Hand"].contains("leftHand")) {
+                    auto& leftHand = value["Hand"]["leftHand"];
+                    {
+                        std::lock_guard<std::mutex> lock(leftHandMutex);
+                        
+                        LeftHandScale = leftHand["scale"].get<double>();
+                        for (int i = 0; i < 26; i++) {
+                            LeftHandTrackingState[i] = stringToPoseArray(leftHand["HandJointLocations"][i]["p"].get<std::string>());
+                        }
+                    }
+                }
+                if (value["Hand"].contains("rightHand")) {
+                    auto& rightHand = value["Hand"]["rightHand"];
+                    {
+                        std::lock_guard<std::mutex> lock(rightHandMutex);
+                        RightHandScale = rightHand["scale"].get<double>();
+                        for (int i = 0; i < 26; i++) {
+                            RightHandTrackingState[i] = stringToPoseArray(rightHand["HandJointLocations"][i]["p"].get<std::string>());
+                        }
+                    }
+                }
             }
         } catch (const json::exception& e) {
             std::cerr << "JSON parsing error: " << e.what() << std::endl;
@@ -224,6 +252,26 @@ int64_t getTimeStampNs() {
     return TimeStampNs;
 }
 
+std::array<std::array<double, 7>, 26> getLeftHandTrackingState() {
+    std::lock_guard<std::mutex> lock(leftHandMutex);
+    return LeftHandTrackingState;
+}
+
+int getLeftHandScale() {
+    std::lock_guard<std::mutex> lock(leftHandMutex);
+    return LeftHandScale;
+}
+
+std::array<std::array<double, 7>, 26> getRightHandTrackingState() {
+    std::lock_guard<std::mutex> lock(rightHandMutex);
+    return RightHandTrackingState;
+}
+
+int getRightHandScale() {
+    std::lock_guard<std::mutex> lock(rightHandMutex);
+    return RightHandScale;
+}
+
 PYBIND11_MODULE(xrobotoolkit_sdk, m) {
     m.def("init", &init, "Initialize the PXREARobot SDK.");
     m.def("close", &deinit, "Deinitialize the PXREARobot SDK.");
@@ -245,5 +293,7 @@ PYBIND11_MODULE(xrobotoolkit_sdk, m) {
     m.def("get_Y_button", &getLeftSecondaryButton, "Get the left secondary button state.");
     m.def("get_B_button", &getRightSecondaryButton, "Get the right secondary button state.");
     m.def("get_time_stamp_ns", &getTimeStampNs, "Get the timestamp in nanoseconds.");
+    m.def("get_left_hand_tracking_state", &getLeftHandTrackingState, "Get the left hand state.");
+    m.def("get_right_hand_tracking_state", &getRightHandTrackingState, "Get the right hand state.");
     m.doc() = "Python bindings for PXREARobot SDK using pybind11.";
 }

python/test_binding.py

@@ -66,6 +66,13 @@ def run_tests():
             print(f"Y Button (Left Secondary): {y_button}")
             print(f"B Button (Right Secondary): {b_button}")
 
+            # Left Hand State
+            left_hand_state = xrt.get_left_hand_tracking_state()
+            print(f"Left Hand State: {left_hand_state}")
+            # Right Hand State
+            right_hand_state = xrt.get_right_hand_tracking_state()
+            print(f"Right Hand State: {right_hand_state}")
+
             # Timestamp
             timestamp = xrt.get_time_stamp_ns()
             print(f"Timestamp (ns): {timestamp}")
@@ -79,9 +86,9 @@ def run_tests():
     except Exception as e:
         print(f"An unexpected error occurred: {e}", file=sys.stderr)
     finally:
-        print("\nDeinitializing SDK...")
+        print("\nClosing SDK...")
         xrt.close()
-        print("SDK Deinitialized.")
+        print("SDK closed.")
         print("Test finished.")
 
 

setup.py

@@ -131,9 +131,9 @@ def run(self):
 
 setup(
     name="xrobotoolkit_sdk",
-    version="1.0.0",
-    author="Zhigen Zhao",  # Replace with your name
-    author_email="[email protected]",  # Replace with your email
+    version="1.0.1",
+    author="Zhigen Zhao",
+    author_email="[email protected]",
     description="A Python binding for XRobotoolkit PC Service SDK using pybind11 and CMake",
     long_description="",  # Optionally, load from a README.md file
     ext_modules=[CMakeExtension("xrobotoolkit_sdk")],

setup_ubuntu.sh

@@ -1,5 +1,3 @@
-# export CC=/usr/bin/gcc
-# export CXX=/usr/bin/g++
 mkdir -p tmp
 cd tmp
 git clone https://github.com/XR-Robotics/XRoboToolkit-PC-Service.git
@@ -16,4 +14,6 @@ rm -rf tmp
 
 # Build the project
 pip install pybind11
+
+pip uninstall -y xrobotoolkit_sdk
 python setup.py install