From 0f775c4e8b37f4de8d9b9acc4e62e3af298d20f7 Mon Sep 17 00:00:00 2001
From: okahilak <olli-pekka.kahilakoski@aalto.fi>
Date: Wed, 25 Aug 2021 16:59:52 +0300
Subject: [PATCH] ADD: Send pulse via serial port when pedal triggered and coil at target (#326)

---
 invesalius/data/serial_port_connection.py | 48 +++++++++++++++++++++++++++++++++++++-----------
 invesalius/gui/task_navigator.py          | 23 +++++++++++++++++++++++
 2 files changed, 60 insertions(+), 11 deletions(-)

diff --git a/invesalius/data/serial_port_connection.py b/invesalius/data/serial_port_connection.py
index 356c8e2..e957040 100644
--- a/invesalius/data/serial_port_connection.py
+++ b/invesalius/data/serial_port_connection.py
@@ -17,6 +17,7 @@
 #    detalhes.
 #--------------------------------------------------------------------------
 
+import queue
 import threading
 import time
 
@@ -25,6 +26,7 @@ from invesalius.pubsub import pub as Publisher
 
 
 class SerialPortConnection(threading.Thread):
+    BINARY_PULSE = b'\x01'
 
     def __init__(self, port, serial_port_queue, event, sleep_nav):
         """
@@ -54,25 +56,49 @@ class SerialPortConnection(threading.Thread):
         except:
             print("Serial port init error: Connecting to port {} failed.".format(self.port))
 
+    def SendPulse(self):
+        success = False
+        try:
+            self.connection.write(self.BINARY_PULSE)
+            success = True
+        except:
+            print("Error: Serial port could not be written into.")
+
+        return success
+
     def run(self):
         while not self.event.is_set():
             trigger_on = False
             try:
-                self.connection.write(b'0')
-                time.sleep(0.3)
-
                 lines = self.connection.readlines()
-                if lines:
-                    trigger_on = True
+            except:
+                print("Error: Serial port could not be read.")
 
-                if self.stylusplh:
-                    trigger_on = True
-                    self.stylusplh = False
+            if lines:
+                trigger_on = True
 
+            if self.stylusplh:
+                trigger_on = True
+                self.stylusplh = False
+
+            try:
                 self.serial_port_queue.put_nowait(trigger_on)
-                time.sleep(self.sleep_nav)
-            except:
-                print("Trigger not read, error")
+            except queue.Full:
+                print("Error: Serial port queue full.")
+
+            time.sleep(self.sleep_nav)
+
+            # XXX: This is needed here because the serial port queue has to be read
+            #      at least as fast as it is written into, otherwise it will eventually
+            #      become full. Reading is done in another thread, which has the same
+            #      sleeping parameter sleep_nav between consecutive runs as this thread.
+            #      However, a single run of that thread takes longer to finish than a
+            #      single run of this thread, causing that thread to lag behind. Hence,
+            #      the additional sleeping here to ensure that this thread lags behind the
+            #      other thread and not the other way around. However, it would be nice to
+            #      handle the timing dependencies between the threads in a more robust way.
+            #
+            time.sleep(0.3)
         else:
             if self.connection:
                 self.connection.close()
diff --git a/invesalius/gui/task_navigator.py b/invesalius/gui/task_navigator.py
index 83b1116..8cf5066 100644
--- a/invesalius/gui/task_navigator.py
+++ b/invesalius/gui/task_navigator.py
@@ -310,6 +310,8 @@ class InnerFoldPanel(wx.Panel):
 
 class Navigation():
     def __init__(self):
+        self.pedal_connection = PedalConnection()
+
         self.image_fiducials = np.full([3, 3], np.nan)
         self.correg = None
         self.current_coord = 0, 0, 0
@@ -344,6 +346,17 @@ class Navigation():
         self.serial_port = None
         self.serial_port_connection = None
 
+        # During navigation
+        self.coil_at_target = False
+
+        self.__bind_events()
+
+    def __bind_events(self):
+        Publisher.subscribe(self.CoilAtTarget, 'Coil at target')
+
+    def CoilAtTarget(self, state):
+        self.coil_at_target = state
+
     def UpdateSleep(self, sleep):
         self.sleep_nav = sleep
         self.serial_port_connection.sleep_nav = sleep
@@ -371,6 +384,12 @@ class Navigation():
         fre = icp.icp_fre if icp.use_icp else self.fre
         return fre, fre <= const.FIDUCIAL_REGISTRATION_ERROR_THRESHOLD
 
+    def PedalStateChanged(self, state):
+        if state is True and self.coil_at_target and self.SerialPortEnabled():
+            success = self.serial_port_connection.SendPulse()
+            if success:
+                Publisher.sendMessage('Pulse triggered', state=True)
+
     def StartNavigation(self, tracker):
         tracker_fiducials, tracker_fiducials_raw = tracker.GetTrackerFiducials()
         ref_mode_id = tracker.GetReferenceMode()
@@ -466,9 +485,13 @@ class Navigation():
                 jobs.start()
                 # del jobs
 
+            self.pedal_connection.add_callback('navigation', self.PedalStateChanged)
+
     def StopNavigation(self):
         self.event.set()
 
+        self.pedal_connection.remove_callback('navigation')
+
         self.coord_queue.clear()
         self.coord_queue.join()
 
--
libgit2 0.21.2