Merge pull request #4 from XR-Robotics/dev/motion_tracker

added individual motion tracker

Author: zhigenzhao

bindings/py_bindings.cpp

@@ -30,6 +30,14 @@ std::array<int64_t, 24> BodyJointsTimestamp;  // IMU timestamp for each joint
 int64_t BodyTimeStampNs = 0;  // Body data timestamp
 bool BodyDataAvailable = false;  // Flag to indicate if body data is available
 
+std::array<std::array<double, 7>, 3> MotionTrackerPose;  // Position and rotation for each joint
+std::array<std::array<double, 6>, 3> MotionTrackerVelocity;  // Velocity and angular velocity for each joint
+std::array<std::array<double, 6>, 3> MotionTrackerAcceleration;  // Acceleration and angular acceleration for each joint
+std::array<std::string, 3> MotionTrackerSerialNumbers;  // Serial numbers of the motion trackers
+int64_t MotionTimeStampNs = 0;  // Motion data timestamp
+int NumMotionDataAvailable = 0;  // number of motion trackers
+
+
 bool LeftMenuButton;
 double LeftTrigger;
 double LeftGrip;
@@ -55,6 +63,7 @@ std::mutex timestampMutex;
 std::mutex leftHandMutex;
 std::mutex rightHandMutex;
 std::mutex bodyMutex;  // Mutex for body tracking data
+std::mutex motionMutex;
 
 std::array<double, 7> stringToPoseArray(const std::string& poseStr) {
     std::array<double, 7> result{0};
@@ -209,6 +218,44 @@ void OnPXREAClientCallback(void* context, PXREAClientCallbackType type, int stat
                         }
                     }
                 }
+                //parse individual tracker data
+                if (value.contains("Motion")) {
+                    auto& motion = value["Motion"];
+                    {
+                        std::lock_guard<std::mutex> lock(motionMutex);
+                        if (motion.contains("timeStampNs")) {
+                            MotionTimeStampNs = motion["timeStampNs"].get<int64_t>();
+                        }
+                        if (motion.contains("joints") && motion["joints"].is_array()) {
+                            auto joints = motion["joints"];
+                            NumMotionDataAvailable = std::min(static_cast<int>(joints.size()), 3);
+
+                            for (int i = 0; i < NumMotionDataAvailable; i++) {
+                                auto& joint = joints[i];
+
+                                // Parse pose (position and rotation)
+                                if (joint.contains("p")) {
+                                    MotionTrackerPose[i] = stringToPoseArray(joint["p"].get<std::string>());
+                                }
+
+                                // Parse velocity and angular velocity
+                                if (joint.contains("va")) {
+                                    MotionTrackerVelocity[i] = stringToVelocityArray(joint["va"].get<std::string>());
+                                }
+
+                                // Parse acceleration and angular acceleration
+                                if (joint.contains("wva")) {
+                                    MotionTrackerAcceleration[i] = stringToVelocityArray(joint["wva"].get<std::string>());
+                                }
+
+                                if (joint.contains("sn")) {
+                                    MotionTrackerSerialNumbers[i] = joint["sn"].get<std::string>();
+                                }
+                            }
+
+                        }
+                    }
+                }
             }
         } catch (const json::exception& e) {
             std::cerr << "JSON parsing error: " << e.what() << std::endl;
@@ -379,6 +426,53 @@ int64_t getBodyTimeStampNs() {
     return BodyTimeStampNs;
 }
 
+int numMotionDataAvailable() {
+    std::lock_guard<std::mutex> lock(motionMutex);
+    return NumMotionDataAvailable;
+}
+
+std::vector<std::array<double, 7>> getMotionTrackerPose() {
+    std::lock_guard<std::mutex> lock(motionMutex);
+    std::vector<std::array<double, 7>> result;
+    for (int i = 0; i < NumMotionDataAvailable; i++) {
+        result.push_back(MotionTrackerPose[i]);
+    }
+    return result;
+}
+
+std::vector<std::array<double, 6>> getMotionTrackerVelocity() {
+    std::lock_guard<std::mutex> lock(motionMutex);
+    std::vector<std::array<double, 6>> result;
+    for (int i = 0; i < NumMotionDataAvailable; i++) {
+        result.push_back(MotionTrackerVelocity[i]);
+    }
+    return result;
+}
+
+std::vector<std::array<double, 6>> getMotionTrackerAcceleration() {
+    std::lock_guard<std::mutex> lock(motionMutex);
+    std::vector<std::array<double, 6>> result;
+    for (int i = 0; i < NumMotionDataAvailable; i++) {
+        result.push_back(MotionTrackerAcceleration[i]);
+    }
+    return result;
+}
+
+std::vector<std::string> getMotionTrackerSerialNumbers() {
+    std::lock_guard<std::mutex> lock(motionMutex);
+    std::vector<std::string> result;
+    for (int i = 0; i < NumMotionDataAvailable; i++) {
+        result.push_back(MotionTrackerSerialNumbers[i]);
+    }
+    return result;
+}
+
+int64_t getMotionTimeStampNs() {
+    std::lock_guard<std::mutex> lock(motionMutex);
+    return MotionTimeStampNs;
+}
+
+
 PYBIND11_MODULE(xrobotoolkit_sdk, m) {
     m.def("init", &init, "Initialize the PXREARobot SDK.");
     m.def("close", &deinit, "Deinitialize the PXREARobot SDK.");
@@ -412,6 +506,14 @@ PYBIND11_MODULE(xrobotoolkit_sdk, m) {
     m.def("get_body_joints_acceleration", &getBodyJointsAcceleration, "Get the body joints acceleration data (24 joints, 6 values each: ax,ay,az,wax,way,waz).");
     m.def("get_body_joints_timestamp", &getBodyJointsTimestamp, "Get the body joints IMU timestamp data (24 joints).");
     m.def("get_body_timestamp_ns", &getBodyTimeStampNs, "Get the body data timestamp in nanoseconds.");
+
+    // Motion tracker functions
+    m.def("num_motion_data_available", &numMotionDataAvailable, "Check if motion tracker data is available.");
+    m.def("get_motion_tracker_pose", &getMotionTrackerPose, "Get the motion tracker pose data (3 trackers, 7 values each: x,y,z,qx,qy,qz,qw).");
+    m.def("get_motion_tracker_velocity", &getMotionTrackerVelocity, "Get the motion tracker velocity data (3 trackers, 6 values each: vx,vy,vz,wx,wy,wz).");
+    m.def("get_motion_tracker_acceleration", &getMotionTrackerAcceleration, "Get the motion tracker acceleration data (3 trackers, 6 values each: ax,ay,az,wax,way,waz).");
+    m.def("get_motion_tracker_serial_numbers", &getMotionTrackerSerialNumbers, "Get the serial numbers of the motion trackers.");
+    m.def("get_motion_timestamp_ns", &getMotionTimeStampNs, "Get the motion data timestamp in nanoseconds.");
 
     m.doc() = "Python bindings for PXREARobot SDK using pybind11.";
 }

examples/example_motion_tracker.py

@@ -0,0 +1,17 @@
+import xrobotoolkit_sdk as xrt
+
+xrt.init()
+num_motion_data = xrt.num_motion_data_available()
+print(f"Number of Motion Trackers: {num_motion_data}")
+if num_motion_data > 0:
+    motion_tracker_pose = xrt.get_motion_tracker_pose()
+    motion_tracker_velocity = xrt.get_motion_tracker_velocity()
+    motion_tracker_acceleration = xrt.get_motion_tracker_acceleration()
+    motion_tracker_serial_numbers = xrt.get_motion_tracker_serial_numbers()
+    motion_timestamp_ns = xrt.get_motion_timestamp_ns()
+
+    print(f"Motion Tracker Pose: {motion_tracker_pose}")
+    print(f"Motion Tracker Velocity: {motion_tracker_velocity}")
+    print(f"Motion Tracker Acceleration: {motion_tracker_acceleration}")
+    print(f"Motion Tracker Serial Numbers: {motion_tracker_serial_numbers}")
+    print(f"Motion Timestamp (ns): {motion_timestamp_ns}")

examples/run_binding_continuous.py

@@ -77,6 +77,21 @@ def run_tests():
             timestamp = xrt.get_time_stamp_ns()
             print(f"Timestamp (ns): {timestamp}")
 
+            num_motion_data = xrt.num_motion_data_available()
+            print(f"Number of Motion Trackers: {num_motion_data}")
+            if num_motion_data > 0:
+                motion_tracker_pose = xrt.get_motion_tracker_pose()
+                motion_tracker_velocity = xrt.get_motion_tracker_velocity()
+                motion_tracker_acceleration = xrt.get_motion_tracker_acceleration()
+                motion_tracker_serial_numbers = xrt.get_motion_tracker_serial_numbers()
+                motion_timestamp_ns = xrt.get_motion_timestamp_ns()
+
+                print(f"Motion Tracker Pose: {motion_tracker_pose}")
+                print(f"Motion Tracker Velocity: {motion_tracker_velocity}")
+                print(f"Motion Tracker Acceleration: {motion_tracker_acceleration}")
+                print(f"Motion Tracker Serial Numbers: {motion_tracker_serial_numbers}")
+                print(f"Motion Timestamp (ns): {motion_timestamp_ns}")
+
             time.sleep(0.5)  # Wait for 0.5 seconds before the next iteration
 
         print("\nAll iterations complete.")

setup_ubuntu.sh

@@ -1,3 +1,7 @@
+if [[ "$CONDA_DEFAULT_ENV" != "" ]]; then
+    conda install -c conda-forge libstdcxx-ng -y
+fi
+
 mkdir -p tmp
 cd tmp
 git clone https://github.com/XR-Robotics/XRoboToolkit-PC-Service.git