Improvements in device management

Multiple improvements in device management:
* Working management of lost device
* Use deferred call instead of managing a queue of events for signals
* Clean some old code
* Safer open/subscribe/unsubscribe/close
* get_interpolated_frame now returns null if the device is not
  "interpolation_enabled"

demo/scripts/Visualiser.gd

@@ -7,27 +7,12 @@ extends Node3D
 @export var disconnected : CanvasLayer
 @export var no_device : CanvasLayer
 
-signal view_size_changed(view_size : Vector2i)
-signal current_device_tracking_mode_changed(tracking_mode : int)
 signal presence_of_device_changed(is_present : bool)
 signal current_device_changed(device : UltraleapDevice)
 
-var current_device : UltraleapDevice
-
 var previous_tracking_mode : int
 
 
-# Called every frame. 'delta' is the elapsed time since the previous frame.
-func _process(_delta):
-	if current_device == null:
-		return
-	
-	if current_device.get_tracking_mode() != previous_tracking_mode:
-		emit_signal("current_device_tracking_mode_changed", current_device.get_tracking_mode())
-		
-	previous_tracking_mode = current_device.get_tracking_mode()
-
-
 func _on_device_selection(serial):
 	var dev : UltraleapDevice = _get_device_from_serial(serial)
 	var current_dev : UltraleapDevice = tracking.get_primary_device()
@@ -38,36 +23,23 @@ func _on_device_selection(serial):
 
 
 func _get_device_from_serial(serial : String):
-	for device in tracking.devices:
+	for device in tracking.devices_list:
 		if device.serial == serial:
 			return device
 
 
-func _open_device():
-	current_device.open()
-	current_device.subscribe()
-
-
-func _close_device():
-	current_device.unsubscribe()
-	current_device.close()
-
-
 func _on_hand_tracking_device_added(device):
-	if tracking.devices.size() > 0:
+	if tracking.devices_list.size() > 0:
 		emit_signal("presence_of_device_changed", true)
 		no_device.hide()
 
 
-func _on_hand_tracking_device_removed(_device):
-	if tracking.devices.size() == 0:
+func _on_hand_tracking_device_removed(_device : UltraleapDevice):
+	if tracking.devices_list.size() == 0:
 		emit_signal("presence_of_device_changed", false)
 		no_device.show()
-	if current_device == null:
-		return
-	_close_device()
-	current_device = null
-	current_device_changed.emit(null)
+	if tracking.devices_primary_device == null:
+		current_device_changed.emit(null)
 
 
 func on_undistort_toggled(enabled):

src/device.cpp

@@ -13,6 +13,11 @@
 
 using namespace godot;
 
+UltraleapDevice::~UltraleapDevice() {
+    unsubscribe();
+    close();
+}
+
 void UltraleapDevice::_bind_methods() {
     ClassDB::bind_method(D_METHOD("set_baseline", "baseline"), &UltraleapDevice::set_baseline);
     ClassDB::bind_method(D_METHOD("get_baseline"), &UltraleapDevice::get_baseline);
@@ -136,21 +141,62 @@ void UltraleapDevice::tracking_mode_changed(UltraleapTypes::TrackingMode value)
 }
 
 void UltraleapDevice::subscribe() {
-    LeapSubscribeEvents(connection, device);
+    if (subscribed) {
+        UtilityFunctions::print("Device already subscribed to");
+        return;
+    }
+    eLeapRS result = LeapSubscribeEvents(connection, device);
+    if (result != eLeapRS_Success) {
+        UtilityFunctions::print(UltraleapTypes::ultraleap_result_to_string(result));
+        return;
+    }
+    subscribed = true;
 }
 
 void UltraleapDevice::unsubscribe() {
-    LeapUnsubscribeEvents(connection, device);
+    if (!subscribed) {
+        UtilityFunctions::print("Device not subscribed to");
+        return;
+    }
+    eLeapRS result = LeapUnsubscribeEvents(connection, device);
+    if (result != eLeapRS_Success) {
+        UtilityFunctions::print(UltraleapTypes::ultraleap_result_to_string(result));
+        return;
+    }
+    subscribed = false;
 }
 
 void UltraleapDevice::open() {
-    LeapOpenDevice(device_ref, &device);
-    LeapCreateClockRebaser(&rebaser);
+    if (opened) {
+        UtilityFunctions::print("Device already opened");
+        return;
+    }
+    eLeapRS result = LeapOpenDevice(device_ref, &device);
+    if (result != eLeapRS_Success) {
+        UtilityFunctions::print(UltraleapTypes::ultraleap_result_to_string(result));
+        return;
+    }
+    opened = true;
+    result = LeapCreateClockRebaser(&rebaser);
+    if (result != eLeapRS_Success) {
+        UtilityFunctions::print(UltraleapTypes::ultraleap_result_to_string(result));
+        return;
+    }
+    interpolation_available = true;
 }
 
 void UltraleapDevice::close() {
+    if (!opened) {
+        UtilityFunctions::print("Device already closed");
+        return;
+    }
     LeapCloseDevice(device);
-    LeapDestroyClockRebaser(rebaser);
+    opened = false;
+    if (rebaser != NULL) {
+        LeapDestroyClockRebaser(rebaser);
+        rebaser = NULL;
+        interpolation_available = false;
+    }
 }
 
 void UltraleapDevice::on_frame_received(const LEAP_TRACKING_EVENT* frame) {
@@ -187,6 +233,10 @@ Ref<UltraleapImage> UltraleapDevice::get_right_image() {
 }
 
 Ref<UltraleapFrame> UltraleapDevice::get_interpolated_frame(int64_t time) {
+    if (!interpolation_available) {
+        return NULL;
+    }
+
     int64_t cpu_time = Time::get_singleton()->get_ticks_usec();
     LeapUpdateRebase(rebaser, cpu_time, LeapGetNow());
     LeapRebaseClock(rebaser, cpu_time, &time);
@@ -208,4 +258,9 @@ Ref<UltraleapFrame> UltraleapDevice::get_interpolated_frame(int64_t time) {
     }
 
     return NULL;
+}
+
+void UltraleapDevice::on_lost() {
+    unsubscribe();
+    close();
 }

src/device.h

@@ -23,6 +23,8 @@ class UltraleapDevice : public Resource {
     GDCLASS(UltraleapDevice, Resource);
 
 public:
+    ~UltraleapDevice();
+
     uint32_t baseline;
     String serial;
     uint32_t id;
@@ -66,6 +68,10 @@ public:
 
     void on_frame_received(const LEAP_TRACKING_EVENT* frame);
     void on_image_received(const LEAP_IMAGE* image);
+    void on_lost();
+
+    bool is_opened() { return opened; }
+    bool is_subscribed() { return subscribed; }
 
 protected:
     static void _bind_methods();
@@ -73,6 +79,10 @@ protected:
 private:
     std::mutex frame_mutex;
     std::mutex image_mutex;
+
+    bool opened = false;
+    bool subscribed = false;
+    bool interpolation_available = false;
 };
 
 

src/digit.cpp

@@ -33,6 +33,8 @@ void UltraleapDigit::_bind_methods() {
     ClassDB::bind_method(D_METHOD("get_is_extended"), &UltraleapDigit::get_is_extended);
     ClassDB::bind_method(D_METHOD("set_is_extended", "is_extended"), &UltraleapDigit::set_is_extended);
 
+    ClassDB::bind_method(D_METHOD("get_bones"), &UltraleapDigit::get_bones);
+
     ClassDB::add_property(
         "UltraleapDigit",
         PropertyInfo(
@@ -102,6 +104,19 @@ void UltraleapDigit::_bind_methods() {
         "set_is_extended",
         "get_is_extended"
     );
+
+    ClassDB::add_property(
+        "UltraleapDigit",
+        PropertyInfo(
+            Variant::ARRAY,
+            "bones",
+            PROPERTY_HINT_ARRAY_TYPE,
+            "UltraleapBone",
+            PROPERTY_USAGE_READ_ONLY | PROPERTY_USAGE_EDITOR
+        ),
+        "",
+        "get_bones"
+    );
 }
 
 void UltraleapDigit::fill_digit_data(Ref<UltraleapDigit> ul_digit, LEAP_DIGIT* digit, FingerType type) {

src/digit.h

@@ -2,7 +2,7 @@
 #define ULTRALEAP_DIGIT_H
 
 #include <godot_cpp/classes/global_constants.hpp>
-
+#include <godot_cpp/classes/ref.hpp>
 #include <godot_cpp/core/binder_common.hpp>
 #include <godot_cpp/core/object.hpp>
 
@@ -53,6 +53,19 @@ public:
     bool get_is_extended() { return is_extended; }
     void set_is_extended(bool value) { is_extended = value; }
 
+    Array get_bones() {
+        if (bones == Variant::NIL) {
+            bones = Array();
+        }
+        if (bones.size() == 0) {
+            bones.append(metacarpal_ref);
+            bones.append(proximal_ref);
+            bones.append(intermediate_ref);
+            bones.append(distal_ref);
+        }
+        return bones;
+    }
+
     static void fill_digit_data(Ref<UltraleapDigit> ul_digit, LEAP_DIGIT* digit, FingerType type);
 protected:
     static void _bind_methods();
@@ -62,6 +75,7 @@ private:
     Ref<UltraleapBone> proximal_ref;
     Ref<UltraleapBone> intermediate_ref;
     Ref<UltraleapBone> distal_ref;
+    Array bones;
 };
 
 VARIANT_ENUM_CAST(UltraleapDigit::FingerType);

src/hand.cpp

@@ -53,7 +53,6 @@ void UltraleapHand::_bind_methods() {
     ClassDB::bind_method(D_METHOD("set_palm", "palm"), &UltraleapHand::set_palm);
     ClassDB::bind_method(D_METHOD("get_palm"), &UltraleapHand::get_palm);
 
-    ClassDB::bind_method(D_METHOD("set_digits", "digits"), &UltraleapHand::set_digits);
     ClassDB::bind_method(D_METHOD("get_digits"), &UltraleapHand::get_digits);
 
     ClassDB::add_property(
@@ -216,18 +215,16 @@ void UltraleapHand::_bind_methods() {
         "UltraleapHand",
         PropertyInfo(
             Variant::ARRAY,
-            "digits"
+            "digits",
+            PROPERTY_HINT_ARRAY_TYPE,
+            "UltraleapDigit",
+            PROPERTY_USAGE_READ_ONLY | PROPERTY_USAGE_EDITOR
         ),
-        "set_digits",
+        "",
         "get_digits"
     );
 };
 
-void UltraleapHand::set_digits(Array digits) {
-    for (int i = 0; i < digits.size(); i++) {
-    }
-}
-
 void UltraleapHand::fill_hand_data(Ref<UltraleapHand> ul_hand, LEAP_HAND* hand) {
     // For now we just check if the arm ref is not set
     if (ul_hand->arm_ref == NULL) {

src/hand.h

@@ -74,7 +74,9 @@ public:
     void set_palm(Ref<UltraleapPalm> value) { palm_ref = value; }
 
     Array get_digits() {
-        Array digits_array = Array();
+        if (digits_array == Variant::NIL) {
+            digits_array = Array();
+        }
         if (digits_array.size() == 0) {
             digits_array.append(thumb);
             digits_array.append(index);
@@ -84,7 +86,6 @@ public:
         }
         return digits_array;
     }
-    void set_digits(Array digits);
 
     static void fill_hand_data(Ref<UltraleapHand> ul_hand, LEAP_HAND* hand);
 protected:
@@ -93,7 +94,7 @@ protected:
 private:
     Ref<UltraleapBone> arm_ref;
     Ref<UltraleapPalm> palm_ref;
-    Array digits_array = Array();
+    Array digits_array;
 
     Ref<UltraleapDigit> thumb;
     Ref<UltraleapDigit> index;

src/ultraleap.cpp

@@ -85,16 +85,14 @@ void UltraleapHandTracking::_bind_methods() {
     ClassDB::bind_method(D_METHOD("is_connected"), &UltraleapHandTracking::is_connected);
     ClassDB::bind_method(D_METHOD("is_started"), &UltraleapHandTracking::is_started);
 
-    //ClassDB::bind_method(D_METHOD("set_last_frame", "last_frame"), &UltraleapHandTracking::set_last_frame);
     ClassDB::bind_method(D_METHOD("get_last_frame"), &UltraleapHandTracking::get_last_frame);
 
-    ClassDB::bind_method(D_METHOD("set_tracking_mode", "tracking_mode"), &UltraleapHandTracking::set_tracking_mode, DEFVAL(0));
+    ClassDB::bind_method(D_METHOD("set_tracking_mode", "tracking_mode"), &UltraleapHandTracking::set_tracking_mode);
     ClassDB::bind_method(D_METHOD("get_tracking_mode"), &UltraleapHandTracking::get_tracking_mode);
 
     ClassDB::bind_method(D_METHOD("get_left_image"), &UltraleapHandTracking::get_left_image);
     ClassDB::bind_method(D_METHOD("get_right_image"), &UltraleapHandTracking::get_right_image);
 
-    ClassDB::bind_method(D_METHOD("set_devices", "devices"), &UltraleapHandTracking::set_devices);
     ClassDB::bind_method(D_METHOD("get_devices"), &UltraleapHandTracking::get_devices);
 
     ClassDB::bind_method(D_METHOD("set_service_ip", "service_ip"), &UltraleapHandTracking::set_service_ip);
@@ -137,19 +135,38 @@ void UltraleapHandTracking::_bind_methods() {
         "get_tracking_mode"
     );
 
+    ClassDB::add_property_group(
+        "UltraleapHandTracking",
+        "Devices",
+        "devices"
+    );
+
     ClassDB::add_property(
         "UltraleapHandTracking",
         PropertyInfo(
             Variant::ARRAY,
-            "devices",
+            "devices_list",
             PROPERTY_HINT_ARRAY_TYPE,
             "UltraleapDevice",
             PROPERTY_USAGE_READ_ONLY | PROPERTY_USAGE_EDITOR
         ),
-        "set_devices",
+        "",
         "get_devices"
     );
 
+    ClassDB::add_property(
+        "UltraleapHandTracking",
+        PropertyInfo(
+            Variant::OBJECT,
+            "devices_primary_device",
+            PROPERTY_HINT_RESOURCE_TYPE,
+            "UltraleapDevice",
+            PROPERTY_USAGE_READ_ONLY | PROPERTY_USAGE_EDITOR
+        ),
+        "set_primary_device",
+        "get_primary_device"
+    );
+
     ClassDB::add_property_group(
         "UltraleapHandTracking",
         "Connection",
@@ -369,12 +386,12 @@ void UltraleapHandTracking::stop() {
 }
 
 void UltraleapHandTracking::handle_connection_event(const LEAP_CONNECTION_EVENT *evt) {
-    events.push({ "connection_status_changed", true });
+    call_deferred_thread_group("emit_signal", "connection_status_changed", true);
     connected = true;
 }
 
 void UltraleapHandTracking::handle_connection_lost_event(const LEAP_CONNECTION_LOST_EVENT *event) {
-    events.push({ "connection_status_changed", false });
+    call_deferred_thread_group("emit_signal", "connection_status_changed", true);
     connected = false;
 
     // TODO: write something more pretty
@@ -538,10 +555,7 @@ bool UltraleapHandTracking::get_policy(UltraleapTypes::PolicyFlag flag) {
 
 void UltraleapHandTracking::set_pause(bool value) {
     eLeapRS result = LeapSetPause(connectionHandle, value);
-    if (result == eLeapRS_Success) {
-        // Maybe send a signal here ?
-    }
-    else {
+    if (result != eLeapRS_Success) {
         UtilityFunctions::print(UltraleapTypes::ultraleap_result_to_string(result));
     }
 }
@@ -559,12 +573,6 @@ void UltraleapHandTracking::handle_tracking_event(const LEAP_TRACKING_EVENT* eve
     }
 
     dev->on_frame_received(event);
-
-    // Commenting for now, might remove later. The getter now get the frame
-    // from the primary device instead of storing it.
-    // if (primary_device != NULL && primary_device->id == device_id) {
-    //     last_frame_ref = primary_device->get_last_frame();
-    // }
 }
 
 void UltraleapHandTracking::handle_image_event(const LEAP_IMAGE_EVENT* event, uint32_t device_id) {
@@ -582,13 +590,6 @@ void UltraleapHandTracking::handle_image_event(const LEAP_IMAGE_EVENT* event, ui
     }
 
     dev->on_image_received(event->image);
-
-    // Commenting for now, might remove later. The getters now get the images
-    // from the primary device instead of storing them.
-    // if (primary_device != NULL && primary_device->id == device_id) {
-    //     left_image_ref = primary_device->left_image_ref;
-    //     right_image_ref = primary_device->right_image_ref;
-    // }
 }
 
 void UltraleapHandTracking::handle_policy_change_event(const LEAP_POLICY_EVENT* event) {
@@ -622,19 +623,19 @@ void UltraleapHandTracking::handle_policy_change_event(const LEAP_POLICY_EVENT*
     bool current_allow_pause_resume_policy = UltraleapTypes::read_policy_flag(flags, UltraleapTypes::PolicyFlag::AllowPauseResume);
 
     if (current_images_policy != previous_images_policy) {
-        events.push({ "images_policy_changed", current_images_policy });
+        call_deferred_thread_group("emit_signal", "images_policy_changed", current_images_policy);
     }
 
     if (current_background_frames_policy != previous_background_frames_policy) {
-        events.push({ "background_frames_policy_changed", current_background_frames_policy });
+        call_deferred_thread_group("emit_signal", "background_frames_policy_changed", current_background_frames_policy);
     }
 
     if (current_map_points_policy != previous_map_points_policy) {
-        events.push({ "map_points_policy_changed", current_map_points_policy });
+        call_deferred_thread_group("emit_signal", "map_points_policy_changed", current_map_points_policy);
     }
 
     if (current_allow_pause_resume_policy != previous_allow_pause_resume_policy) {
-        events.push({ "allow_pause_resume_policy_changed", current_allow_pause_resume_policy });
+        call_deferred_thread_group("emit_signal", "allow_pause_resume_policy_changed", current_allow_pause_resume_policy);
     }
 
     policy_flags = flags;
@@ -658,7 +659,7 @@ void UltraleapHandTracking::handle_tracking_mode_event(const LEAP_TRACKING_MODE_
 
     if (primary_device != NULL && device_id == primary_device->id) {
         if (primary_device->tracking_mode != tm) {
-            events.push({ "tracking_mode_changed", tm });
+            call_deferred_thread_group("emit_signal", "tracking_mode_changed", tm);
         }
     }
 
@@ -672,23 +673,26 @@ void UltraleapHandTracking::handle_device_event(const LEAP_DEVICE_EVENT* event)
     Ref<UltraleapDevice> dev = create_device(event->device);
     if (!devices->has_device(dev)) {
         devices->add_device(dev);
-        events.push({ "device_added", dev });
     }
     else {
         UtilityFunctions::print("Device was already in the list, updating");
         devices->remove_device(dev);
         devices->add_device(dev);
-        events.push({ "device_added", dev });
     }
+
+    call_deferred_thread_group("emit_signal", "device_added", dev);
 }
 
 void UltraleapHandTracking::handle_device_lost_event(const LEAP_DEVICE_EVENT* event) {
-    UtilityFunctions::print("Device lost");
+    UtilityFunctions::print("Device lost with ID ", event->device.id);
 
-    Ref<UltraleapDevice> dev = create_device(event->device);
-    if (devices->has_device(dev)) {
+    Ref<UltraleapDevice> dev = devices->find_by_id(event->device.id);
+    if (dev != NULL) {
+        if (dev == primary_device) {
+            primary_device = Variant::NIL;
+        }
         devices->remove_device(dev);
-        events.push({ "device_removed", dev });
+        call_deferred_thread_group("emit_signal", "device_removed", dev);
     }
     else {
         UtilityFunctions::print("Device was not in the list");
@@ -762,19 +766,11 @@ Ref<UltraleapDevice> UltraleapHandTracking::create_device(LEAP_DEVICE_REF ref) {
 void UltraleapHandTracking::_notification(int p_what) {
     //UtilityFunctions::print(p_what);
     if (p_what == Node::NOTIFICATION_ENTER_TREE) {
-        set_process_internal(true);
         // For now we will also start automatically if we're in the editor, so that we can have data for tool scripts
         if (autostart || Engine::get_singleton()->is_editor_hint()) {
             start();
         }
     }
-    else if (p_what == Node::NOTIFICATION_INTERNAL_PROCESS) {
-        while (!events.empty()) {
-            event ev = events.front();
-            emit_signal(ev.signal, ev.arg);
-            events.pop();
-        }
-    }
     else if (p_what == Node::NOTIFICATION_EXIT_TREE) {
         stop();
     }
@@ -790,17 +786,15 @@ String UltraleapHandTracking::generate_connection_payload() {
 }
 
 void UltraleapHandTracking::set_primary_device(Ref<UltraleapDevice> device) {
-    if (primary_device == NULL || primary_device->id != device->id) {
-        if (primary_device != NULL && primary_device->id != device->id) {
-            LeapCloseDevice(primary_device->device);
+    if (primary_device != NULL) {
+        if (!primary_device->is_opened()) {
+            primary_device->close();
         }
-        // Pretty hack-ish, we open the device and re-store the handle in the UltraleapDevice object
-        LeapOpenDevice(device->device_ref, &(device->device));
-
-        primary_device = device;
-
-        LeapSetPrimaryDevice(connectionHandle, device->device, false);
     }
+
+    device->open();
+    primary_device = device;
+    LeapSetPrimaryDevice(connectionHandle, device->device, false);
 }
 
 Ref<UltraleapDevice> UltraleapHandTracking::get_primary_device() {
@@ -808,27 +802,24 @@ Ref<UltraleapDevice> UltraleapHandTracking::get_primary_device() {
 }
 
 Ref<UltraleapFrame> UltraleapHandTracking::get_last_frame() {
-    // Uuuuh... don't like this part
     if (primary_device == NULL) {
-        return last_frame_ref;
+        return NULL;
     }
 
     return primary_device->get_last_frame();
 }
 
 Ref<UltraleapImage> UltraleapHandTracking::get_left_image() {
-    // Uuuuh... don't like this part
     if (primary_device == NULL) {
-        return left_image_ref;
+        return NULL;
     }
 
     return primary_device->get_left_image();
 }
 
 Ref<UltraleapImage> UltraleapHandTracking::get_right_image() {
-    // Uuuuh... don't like this part
     if (primary_device == NULL) {
-        return right_image_ref;
+        return NULL;
     }
 
     return primary_device->get_right_image();

src/ultraleap.h

@@ -42,22 +42,16 @@ public:
 
     Ref<UltraleapDeviceList> devices = Ref<UltraleapDeviceList>(memnew(UltraleapDeviceList));
 
-    Ref<UltraleapFrame> last_frame_ref;
-    Ref<UltraleapImage> left_image_ref;
-    Ref<UltraleapImage> right_image_ref;
-
     String service_ip = String("127.0.0.1");
     uint16_t service_port = 12345;
 
     bool autostart = true;
     bool enable_images = true;
 
-    Ref<UltraleapFrame> get_last_frame();
-    void set_last_frame(Ref<UltraleapFrame> value) { return; }
-
     UltraleapTypes::TrackingMode get_tracking_mode();
     void set_tracking_mode(UltraleapTypes::TrackingMode value);
 
+    Ref<UltraleapFrame> get_last_frame();
     Ref<UltraleapImage> get_left_image();
     Ref<UltraleapImage> get_right_image();
 
@@ -112,12 +106,7 @@ private:
     bool opened = false;            /* Indicate an opened connection */
     bool connected = false;         /* We have confirmed connection */
 
-    Ref<UltraleapDevice> primary_device = NULL;
-    typedef struct {
-        StringName signal;
-        Variant arg;
-    } event;
-    std::queue<event> events;
+    Ref<UltraleapDevice> primary_device;
 
     void serviceMessageLoop();
     void dispose_ultraleap();