Add interpolation to visualiser

demo/main.tscn

@@ -476,6 +476,7 @@ data_img = SubResource("Image_gt7mg")
 [node name="Hands" type="Node3D" parent="."]
 script = ExtResource("8_uah46")
 tracking = NodePath("../UltraleapHandTracking")
+use_interpolated_frames = true
 
 [node name="LeftHand" type="Node3D" parent="Hands"]
 

demo/scripts/DebugHands.gd

@@ -3,6 +3,7 @@ extends Node3D
 
 @export_node_path("UltraleapHandTracking", "UltraleapDeviceNode") var tracking
 @export var hide_non_visible_hands : bool = true
+@export var use_interpolated_frames : bool = false
 
 var autorotate : bool = false
 var inc : int = 1
@@ -104,7 +105,13 @@ func get_frame_from_service():
 		get_node("RightHand").hide()
 		return
 		
-	return tracking.devices[device_index].get_last_frame()
+	if not use_interpolated_frames:
+		return tracking.devices[device_index].get_last_frame()
+		
+	var f : UltraleapFrame = tracking.devices[device_index].get_interpolated_frame(Time.get_ticks_usec())
+	if f == null:
+		return tracking.devices[device_index].get_last_frame()
+	return f
 
 func set_hand_transform(hand : UltraleapHand, chirality : UltraleapTypes.Chirality):
 	var chirality_str = chirality_to_str(chirality)

src/device.cpp

@@ -5,6 +5,7 @@
 #include <godot_cpp/classes/global_constants.hpp>
 #include <godot_cpp/classes/ref.hpp>
 #include <godot_cpp/classes/node3d.hpp>
+#include <godot_cpp/classes/time.hpp>
 
 #include <godot_cpp/variant/utility_functions.hpp>
 
@@ -24,6 +25,7 @@ void UltraleapDevice::_bind_methods() {
 
     ClassDB::bind_method(D_METHOD("set_last_frame", "last_frame"), &UltraleapDevice::set_last_frame);
     ClassDB::bind_method(D_METHOD("get_last_frame"), &UltraleapDevice::get_last_frame);
+    ClassDB::bind_method(D_METHOD("get_interpolated_frame", "time"), &UltraleapDevice::get_interpolated_frame);
 
     ClassDB::bind_method(D_METHOD("set_tracking_mode", "tracking_mode"), &UltraleapDevice::set_tracking_mode, DEFVAL(0));
     ClassDB::bind_method(D_METHOD("get_tracking_mode"), &UltraleapDevice::get_tracking_mode);
@@ -145,6 +147,7 @@ void UltraleapDevice::unsubscribe() {
 
 void UltraleapDevice::open() {
     LeapOpenDevice(device_ref, &device);
+    LeapCreateClockRebaser(&rebaser);
 }
 
 void UltraleapDevice::close() {
@@ -158,4 +161,28 @@ void UltraleapDevice::on_frame_received(LEAP_TRACKING_EVENT* frame) {
 
 void UltraleapDevice::on_image_received(LEAP_IMAGE* image) {
     UltraleapImage::fill_images_data(left_image_ref, right_image_ref, image);
+}
+
+Variant UltraleapDevice::get_interpolated_frame(int64_t time) {
+    int64_t cpu_time = Time::get_singleton()->get_ticks_usec();
+    LeapUpdateRebase(rebaser, cpu_time, LeapGetNow());
+    LeapRebaseClock(rebaser, cpu_time, &time);
+
+    uint64_t targetFrameSize = 0;
+
+    //Get the buffer size needed to hold the tracking data
+    eLeapRS result = LeapGetFrameSizeEx(connection, device, time, &targetFrameSize);
+    if (result == eLeapRS_Success) {
+        //Allocate enough memory
+        LEAP_TRACKING_EVENT* interpolatedFrame = (LEAP_TRACKING_EVENT*)malloc((size_t)targetFrameSize);
+        //Get the frame
+        result = LeapInterpolateFrameEx(connection, device, time, interpolatedFrame, targetFrameSize);
+        if (result == eLeapRS_Success) {
+            UltraleapFrame* new_frame = memnew(UltraleapFrame);
+            UltraleapFrame::fill_frame_data(new_frame, interpolatedFrame);
+            return Ref<UltraleapFrame>(new_frame);
+        }
+    }
+
+    return Variant();
 }

src/device.h

@@ -39,6 +39,8 @@ public:
     Variant get_last_frame() { return last_frame_ref; }
     void set_last_frame(Variant value) { return; }
 
+    Variant get_interpolated_frame(int64_t time);
+
     UltraleapTypes::TrackingMode get_tracking_mode() { return tracking_mode; }
     void set_tracking_mode(UltraleapTypes::TrackingMode value);
 
@@ -63,6 +65,7 @@ public:
     LEAP_DEVICE_REF device_ref;
     LEAP_DEVICE device;
     LEAP_CONNECTION connection;
+    LEAP_CLOCK_REBASER rebaser;
 
     void tracking_mode_changed(UltraleapTypes::TrackingMode value);
 

src/device_node.cpp

@@ -92,6 +92,7 @@ void UltraleapDeviceNode::_bind_methods() {
     ClassDB::bind_method(D_METHOD("get_tracker"), &UltraleapDeviceNode::get_tracker);
 
     ClassDB::bind_method(D_METHOD("get_last_frame"), &UltraleapDeviceNode::get_last_frame);
+    ClassDB::bind_method(D_METHOD("get_interpolated_frame", "time"), &UltraleapDeviceNode::get_interpolated_frame);
 
     ClassDB::bind_method(D_METHOD("device_added", "device"), &UltraleapDeviceNode::device_added);
     ClassDB::bind_method(D_METHOD("device_removed", "device"), &UltraleapDeviceNode::device_removed);
@@ -248,4 +249,11 @@ void UltraleapDeviceNode::on_tracking_mode_changed(UltraleapTypes::TrackingMode
 void UltraleapDeviceNode::on_tracking_event_received(Ref<UltraleapFrame> frame) {
     last_frame = frame;
     emit_signal("frame_received", last_frame);
+}
+
+Variant UltraleapDeviceNode::get_interpolated_frame(int64_t time) {
+    if (current_device == NULL) {
+        return Variant::NIL;
+    }
+    return current_device->get_interpolated_frame(time);
 }

src/device_node.h

@@ -46,6 +46,7 @@ public:
     bool _get(const StringName &p_name, Variant &r_ret) const;
 
     Variant get_last_frame() { return last_frame; }
+    Variant get_interpolated_frame(int64_t time);
 
 protected:
     static void _bind_methods();

src/types.cpp

@@ -32,4 +32,76 @@ Quaternion UltraleapTypes::ultraleap_quaternion_to_godot_quaternion(LEAP_QUATERN
         quaternion->z,
         quaternion->w
     );
+}
+
+String UltraleapTypes::ultraleap_result_to_string(eLeapRS result) {
+    switch (result)
+    {
+    case eLeapRS_Success:
+        return "Success";
+        break;
+    case eLeapRS_TimestampTooEarly:
+        return "Timestap too early";
+        break;
+    case eLeapRS_RoutineIsNotSeer:
+        return "Routine is not Seer";
+        break;
+    case eLeapRS_NotConnected:
+        return "Not connected";
+        break;
+    case eLeapRS_UnknownError:
+        return "Unknown error";
+        break;
+    case eLeapRS_InvalidArgument:
+        return "Invalid argument";
+        break;
+    case eLeapRS_InsufficientResources:
+        return "Insufficient resources";
+        break;
+    case eLeapRS_InsufficientBuffer:
+        return "Insufficient buffers";
+        break;
+    case eLeapRS_Timeout:
+        return "Timeout";
+        break;
+    case eLeapRS_HandshakeIncomplete:
+        return "Handshake incomplete";
+        break;
+    case eLeapRS_BufferSizeOverflow:
+        return "Buffer size overflow";
+        break;
+    case eLeapRS_ProtocolError:
+        return "Protocol error";
+        break;
+    case eLeapRS_InvalidClientID:
+        return "Invalid client ID";
+        break;
+    case eLeapRS_UnexpectedClosed:
+        return "Unexpected close";
+        break;
+    case eLeapRS_UnknownImageFrameRequest:
+        return "Unknown image frame request";
+        break;
+    case eLeapRS_UnknownTrackingFrameID:
+        return "Unknown tracking frame ID";
+        break;
+    case eLeapRS_ConcurrentPoll:
+        return "Concurrent poll";
+        break;
+    case eLeapRS_NotAvailable:
+        return "Not available";
+        break;
+    case eLeapRS_NotStreaming:
+        return "Not streaming";
+        break;
+    case eLeapRS_CannotOpenDevice:
+        return "Cannot open device";
+        break;
+    case eLeapRS_Unsupported:
+        return "Unsupported";
+        break;
+    default:
+        return "Result message conversion to string not implemented";
+        break;
+    }
 }

src/types.h

@@ -30,6 +30,7 @@ public:
 
     static Vector3 ultraleap_vector3_to_godot_vector3(LEAP_VECTOR* vector);
     static Quaternion ultraleap_quaternion_to_godot_quaternion(LEAP_QUATERNION* quaternion);
+    static String ultraleap_result_to_string(eLeapRS result);
 
 protected:
     static void _bind_methods();

src/ultraleap.cpp

@@ -3,13 +3,11 @@
 #include <godot_cpp/core/class_db.hpp>
 
 #include <godot_cpp/classes/global_constants.hpp>
-#include <godot_cpp/classes/time.hpp>
 #include <godot_cpp/variant/utility_functions.hpp>
 #include <godot_cpp/variant/vector3.hpp>
 #include <godot_cpp/classes/node3d.hpp>
 
 #include <LeapC.h>
-#include <time.h>
 
 #include <thread>
 #include <functional>
@@ -35,10 +33,6 @@ void UltraleapHandTracking::_bind_methods() {
         "Desktop, HMD, Screentop"
     );
 
-    ClassDB::bind_method(D_METHOD("set_interpolate", "interpolate"), &UltraleapHandTracking::set_interpolate, DEFVAL(true));
-    ClassDB::bind_method(D_METHOD("get_interpolate"), &UltraleapHandTracking::get_interpolate);
-    ClassDB::add_property("UltraleapHandTracking", PropertyInfo(Variant::BOOL, "interpolate"), "set_interpolate", "get_interpolate");
-
     ClassDB::bind_method(D_METHOD("start"), &UltraleapHandTracking::start);
 
     ClassDB::bind_method(D_METHOD("set_last_frame", "last_frame"), &UltraleapHandTracking::set_last_frame);
@@ -145,28 +139,22 @@ void UltraleapHandTracking::start() {
     }
 }
 
-bool UltraleapHandTracking::get_interpolate() {
-    return interpolate;
-}
-
-void UltraleapHandTracking::set_interpolate(bool value) {
-    interpolate = value;
-}
-
 void UltraleapHandTracking::handleConnectionEvent(const LEAP_CONNECTION_EVENT *evt) {
     IsConnected = true;
     UtilityFunctions::print("Connected");
-
-    //LeapCreateClockRebaser(&clockSynchronizer);
 }
 
 void UltraleapHandTracking::serviceMessageLoop() {
     LEAP_CONNECTION_MESSAGE msg;
     eLeapRS result;
-    int timeout = 100;
 
     while (_isRunning) {
-        result = LeapPollConnection(connectionHandle, timeout, &msg);
+        result = LeapPollConnection(connectionHandle, 100, &msg);
+
+        if (result != eLeapRS_Success) {
+            UtilityFunctions::print(UltraleapTypes::ultraleap_result_to_string(result));
+            continue;
+        }
 
         // Polling may have taken some time, re-check exit condition
         if (!_isRunning) {