Merge pull request #369 from rmatsuda/Thread-for-coordinates

Creation of a new thread for coordinates acquisition

Merge pull request #369 from rmatsuda/Thread-for-coordinates
Creation of a new thread for coordinates acquisition
Victor Hugo Souza · GitHub
2 parents dd8b84f9 0b8581bf
Showing 8 changed files with 92 additions and 51 deletions Show diff stats
invesalius/constants.py
invesalius/data/coordinates.py
invesalius/data/coregistration.py
invesalius/data/viewer_volume.py
invesalius/gui/dialogs.py
invesalius/navigation/icp.py
invesalius/navigation/navigation.py
invesalius/navigation/tracker.py
@@ -818,6 +818,7 @@ SEED_RADIUS = 1.5
 # Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation.
 SLEEP_NAVIGATION = 0.15
+SLEEP_COORDINATES = 0.05
 BRAIN_OPACITY = 0.5
 N_CPU = psutil.cpu_count()
@@ -19,6 +19,9 @@
 from math import sin, cos
 import numpy as np
+import queue
+import threading
+import wx
 import invesalius.data.bases as db
 import invesalius.data.transformations as tr
@@ -28,9 +31,21 @@ from time import sleep
 from random import uniform
 from invesalius.pubsub import pub as Publisher
+class TrackerCoordinates():
+    def __init__(self):
+        self.coord = None
+        self.markers_flag = [False, False, False]
-def GetCoordinates(trck_init, trck_id, ref_mode):
+    def SetCoordinates(self, coord, markers_flag):
+        self.coord = coord
+        self.markers_flag = markers_flag
+        wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
+    def GetCoordinates(self):
+        return self.coord, self.markers_flag
+
+
+def GetCoordinatesForThread(trck_init, trck_id, ref_mode):
     """
     Read coordinates from spatial tracking devices using
@@ -52,11 +67,13 @@ def GetCoordinates(trck_init, trck_id, ref_mode):
                     const.OPTITRACK: OptitrackCoord,
                     const.DEBUGTRACKRANDOM: DebugCoordRandom,
                     const.DEBUGTRACKAPPROACH: DebugCoordRandom}
-        coord = getcoord[trck_id](trck_init, trck_id, ref_mode)
+
+        coord, markers_flag = getcoord[trck_id](trck_init, trck_id, ref_mode)
     else:
         print("Select Tracker")
-    return coord
+    return coord, markers_flag
+
 def OptitrackCoord(trck_init, trck_id, ref_mode):
     """
@@ -93,8 +110,10 @@ def OptitrackCoord(trck_init, trck_id, ref_mode):
     coord3 = np.array([float(trck.PositionCoilZ1) * scale[0], float(trck.PositionCoilX1) * scale[1],
                        float(trck.PositionCoilY1) * scale[2]])
     coord3 = np.hstack((coord3, angles_coil))
+
     coord = np.vstack([coord1, coord2, coord3])
-    return coord
+
+    return coord, [trck.probeID, trck.HeadID, trck.coilID]
 def PolarisCoord(trck_init, trck_id, ref_mode):
@@ -117,9 +136,9 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
     coord3 = np.hstack((trans_obj, angles_obj))
     coord = np.vstack([coord1, coord2, coord3])
-    # Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID, obj_id=trck.objID)
-    return coord
+    return coord, [trck.probeID, trck.refID, trck.objID]
+
 def PolarisP4Coord(trck_init, trck_id, ref_mode):
     trck = trck_init[0]
@@ -134,45 +153,41 @@ def PolarisP4Coord(trck_init, trck_id, ref_mode):
     obj = obj[2:]
     if probe[:7] == "MISSING":
-        if not "coord1" in locals():
-            coord1 = np.hstack(([0, 0, 0], [0, 0, 0]))
+        coord1 = np.hstack(([0, 0, 0], [0, 0, 0]))
     else:
         q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
         t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
         angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
         trans_probe = np.array(t).astype(float)
         coord1 = np.hstack((trans_probe, angles_probe))
-
-    if ref[:7] == "MISSING":
-        if not "coord2" in locals():
+        if ref[:7] == "MISSING":
             coord2 = np.hstack(([0, 0, 0], [0, 0, 0]))
-    else:
-        q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
-        t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
-        angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
-        trans_ref = np.array(t).astype(float)
-        coord2 = np.hstack((trans_ref, angles_ref))
-
-    if obj[:7] == "MISSING":
-        if not "coord3" in locals():
+        else:
+            q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
+            t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
+            angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
+            trans_ref = np.array(t).astype(float)
+            coord2 = np.hstack((trans_ref, angles_ref))
+
+        if obj[:7] == "MISSING":
             coord3 = np.hstack(([0, 0, 0], [0, 0, 0]))
-    else:
-        q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
-        t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
-        angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
-        trans_obj = np.array(t).astype(float)
-        coord3 = np.hstack((trans_obj, angles_obj))
+        else:
+            q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
+            t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
+            angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
+            trans_obj = np.array(t).astype(float)
+            coord3 = np.hstack((trans_obj, angles_obj))
-    Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID, obj_id=trck.objID)
-    coord = np.vstack([coord1, coord2, coord3])
+        coord = np.vstack([coord1, coord2, coord3])
+
+        return coord, [trck.probeID, trck.refID, trck.objID]
-    return coord
 def CameraCoord(trck_init, trck_id, ref_mode):
     trck = trck_init[0]
-    coord, probeID, refID = trck.Run()
-    Publisher.sendMessage('Sensors ID', probe_id=probeID, ref_id=refID)
-    return coord
+    coord, probeID, refID, coilID = trck.Run()
+
+    return coord, [probeID, refID, coilID]
 def ClaronCoord(trck_init, trck_id, ref_mode):
     trck = trck_init[0]
@@ -193,9 +208,7 @@ def ClaronCoord(trck_init, trck_id, ref_mode):
     coord = np.vstack([coord1, coord2, coord3])
-    Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID)
-
-    return coord
+    return coord, [trck.probeID, trck.refID, trck.coilID]
 def PolhemusCoord(trck, trck_id, ref_mode):
@@ -210,7 +223,7 @@ def PolhemusCoord(trck, trck_id, ref_mode):
     elif trck[1] == 'wrapper':
         coord = PolhemusWrapperCoord(trck[0], trck_id, ref_mode)
-    return coord
+    return coord, None
 def PolhemusWrapperCoord(trck, trck_id, ref_mode):
@@ -362,16 +375,13 @@ def DebugCoordRandom(trk_init, trck_id, ref_mode):
     # coord4 = np.array([uniform(1, 200), uniform(1, 200), uniform(1, 200),
     #                    uniform(-180.0, 180.0), uniform(-180.0, 180.0), uniform(-180.0, 180.0)])
-    Publisher.sendMessage('Sensors ID', probe_id=int(uniform(0, 5)), ref_id=int(uniform(0, 5)), obj_id=int(uniform(0, 5)))
-
-    return np.vstack([coord1, coord2, coord3, coord4])
+    return np.vstack([coord1, coord2, coord3, coord4]), [int(uniform(0, 5)), int(uniform(0, 5)), int(uniform(0, 5))]
 def coordinates_to_transformation_matrix(position, orientation, axes='sxyz'):
     """
     Transform vectors consisting of position and orientation (in Euler angles) in 3d-space into a 4x4
     transformation matrix that combines the rotation and translation.
-
     :param position: A vector of three coordinates.
     :param orientation: A vector of three Euler angles in degrees.
     :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'.
@@ -391,9 +401,7 @@ def transformation_matrix_to_coordinates(matrix, axes=&#39;sxyz&#39;):
     """
     Given a matrix that combines the rotation and translation, return the position and the orientation
     determined by the matrix. The orientation is given as three Euler angles.
-
     The inverse of coordinates_of_transformation_matrix when the parameter 'axes' matches.
-
     :param matrix: A 4x4 transformation matrix.
     :param axes: The order in which the rotations are done for the axes. See transformations.py for details. Defaults to 'sxyz'.
     :return: The position (a vector of length 3) and Euler angles for the orientation in degrees (a vector of length 3).
@@ -479,6 +487,7 @@ def dynamic_reference_m2(probe, reference):
     :param reference: sensor two defined as reference
     :return: rotated and translated coordinates
     """
+
     M = coordinates_to_transformation_matrix(
         position=reference[:3],
         orientation=reference[3:],
@@ -489,6 +498,7 @@ def dynamic_reference_m2(probe, reference):
         orientation=probe[3:],
         axes='rzyx',
     )
+
     M_dyn = np.linalg.inv(M) @ M_p
     al, be, ga = tr.euler_from_matrix(M_dyn, 'rzyx')
@@ -529,3 +539,17 @@ def offset_coordinate(p_old, norm_vec, offset):
     """
     p_offset = p_old - offset * norm_vec
     return p_offset
+
+class ReceiveCoordinates(threading.Thread):
+    def __init__(self, trck_init, trck_id, TrackerCoordinates, event):
+        threading.Thread.__init__(self, name='ReceiveCoordinates')
+        self.trck_init = trck_init
+        self.trck_id = trck_id
+        self.event = event
+        self.TrackerCoordinates = TrackerCoordinates
+
+    def run(self):
+        while not self.event.is_set():
+            coord_raw, markers_flag = GetCoordinatesForThread(self.trck_init, self.trck_id, const.DEFAULT_REF_MODE)
+            self.TrackerCoordinates.SetCoordinates(coord_raw, markers_flag)
+            sleep(const.SLEEP_COORDINATES)
 \ No newline at end of file
@@ -219,7 +219,7 @@ class CoordinateCorregistrate(threading.Thread):
                 else:
                     self.icp, self.m_icp = self.icp_queue.get_nowait()
                 # print(f"Set the coordinate")
-                coord_raw = dco.GetCoordinates(trck_init, trck_id, self.ref_mode_id)
+                coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
                 coord, m_img = corregistrate_object_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp])
                 # XXX: This is not the best place to do the logic related to approaching the target when the
@@ -294,7 +294,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
                     self.icp, self.m_icp = self.icp_queue.get_nowait()
                 # print(f"Set the coordinate")
                 #print(self.icp, self.m_icp)
-                coord_raw = dco.GetCoordinates(trck_init, trck_id, self.ref_mode_id)
+                coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
                 coord, m_img = corregistrate_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp])
                 # print("Coord: ", coord)
                 m_img_flip = m_img.copy()
@@ -368,7 +368,9 @@ class Viewer(wx.Panel):
             self.interactor.Render()
-    def OnSensors(self, probe_id, ref_id, obj_id=0):
+    def OnSensors(self, markers_flag):
+        probe_id, ref_id, obj_id = markers_flag
+
         if not self.probe:
             self.CreateSensorID()
@@ -3648,8 +3648,7 @@ class ICPCorregistrationDialog(wx.Dialog):
         import invesalius.project as prj
         self.m_change = nav_prop[0]
-        self.tracker_id = nav_prop[1]
-        self.trk_init = nav_prop[2]
+        self.tracker = nav_prop[1]
         self.obj_ref_id = 2
         self.obj_name = None
         self.obj_actor = None
@@ -3805,7 +3804,7 @@ class ICPCorregistrationDialog(wx.Dialog):
         self.interactor.Render()
     def GetCurrentCoord(self):
-        coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, const.DYNAMIC_REF)
+        coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
         coord, _ = dcr.corregistrate_dynamic((self.m_change, 0), coord_raw, const.DEFAULT_REF_MODE, [None, None])
         return coord[:3]
@@ -48,7 +48,7 @@ class ICP():
     def OnICP(self, navigation, tracker, m_change):
         ref_mode_id = navigation.GetReferenceMode()
-        dialog = dlg.ICPCorregistrationDialog(nav_prop=(m_change, tracker.tracker_id, tracker.trk_init, ref_mode_id))
+        dialog = dlg.ICPCorregistrationDialog(nav_prop=(m_change, tracker, ref_mode_id))
         if dialog.ShowModal() == wx.ID_OK:
             m_icp, point_coord, transformed_points, prev_error, final_error = dialog.GetValue()
@@ -255,7 +255,7 @@ class Navigation():
                 coreg_data = [m_change, obj_ref_mode]
                 if self.ref_mode_id:
-                    coord_raw = dco.GetCoordinates(tracker.trk_init, tracker.tracker_id, self.ref_mode_id)
+                    coord_raw, markers_flag = tracker.TrackerCoordinates.GetCoordinates()
                 else:
                     coord_raw = np.array([None])
@@ -18,6 +18,7 @@
 #--------------------------------------------------------------------------
 import numpy as np
+import threading
 import invesalius.constants as const
 import invesalius.data.coordinates as dco
@@ -36,6 +37,12 @@ class Tracker():
         self.tracker_connected = False
+        self.thread_coord = None
+
+        self.event_coord = threading.Event()
+
+        self.TrackerCoordinates = dco.TrackerCoordinates()
+
     def SetTracker(self, new_tracker):
         if new_tracker:
             self.DisconnectTracker()
@@ -49,6 +56,9 @@ class Tracker():
             else:
                 self.tracker_id = new_tracker
                 self.tracker_connected = True
+                self.thread_coord = dco.ReceiveCoordinates(self.trk_init, self.tracker_id, self.TrackerCoordinates,
+                                       self.event_coord)
+                self.thread_coord.start()
     def DisconnectTracker(self):
         if self.tracker_connected:
@@ -62,6 +72,11 @@ class Tracker():
                 self.tracker_connected = False
                 self.tracker_id = 0
+                if self.thread_coord:
+                    self.event_coord.set()
+                    self.thread_coord.join()
+                    self.event_coord.clear()
+
                 Publisher.sendMessage('Update status text in GUI',
                                         label=_("Tracker disconnected"))
                 print("Tracker disconnected!")
@@ -81,7 +96,7 @@ class Tracker():
         coord_samples = {}
         for i in range(n_samples):
-            coord_raw = dco.GetCoordinates(self.trk_init, self.tracker_id, ref_mode_id)
+            coord_raw, markers_flag = self.TrackerCoordinates.GetCoordinates()
             if ref_mode_id == const.DYNAMIC_REF:
                 coord = dco.dynamic_reference_m(coord_raw[0, :], coord_raw[1, :])
	@@ -818,6 +818,7 @@ SEED_RADIUS = 1.5		@@ -818,6 +818,7 @@ SEED_RADIUS = 1.5
818		818
819	# Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation.	819	# Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation.
820	SLEEP_NAVIGATION = 0.15	820	SLEEP_NAVIGATION = 0.15
		821	+SLEEP_COORDINATES = 0.05
821		822
822	BRAIN_OPACITY = 0.5	823	BRAIN_OPACITY = 0.5
823	N_CPU = psutil.cpu_count()	824	N_CPU = psutil.cpu_count()
	@@ -219,7 +219,7 @@ class CoordinateCorregistrate(threading.Thread):		@@ -219,7 +219,7 @@ class CoordinateCorregistrate(threading.Thread):
219	else:	219	else:
220	self.icp, self.m_icp = self.icp_queue.get_nowait()	220	self.icp, self.m_icp = self.icp_queue.get_nowait()
221	# print(f"Set the coordinate")	221	# print(f"Set the coordinate")
222	- coord_raw = dco.GetCoordinates(trck_init, trck_id, self.ref_mode_id)	222	+ coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
223	coord, m_img = corregistrate_object_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp])	223	coord, m_img = corregistrate_object_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp])
224		224
225	# XXX: This is not the best place to do the logic related to approaching the target when the	225	# XXX: This is not the best place to do the logic related to approaching the target when the
	@@ -294,7 +294,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):		@@ -294,7 +294,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
294	self.icp, self.m_icp = self.icp_queue.get_nowait()	294	self.icp, self.m_icp = self.icp_queue.get_nowait()
295	# print(f"Set the coordinate")	295	# print(f"Set the coordinate")
296	#print(self.icp, self.m_icp)	296	#print(self.icp, self.m_icp)
297	- coord_raw = dco.GetCoordinates(trck_init, trck_id, self.ref_mode_id)	297	+ coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
298	coord, m_img = corregistrate_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp])	298	coord, m_img = corregistrate_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp])
299	# print("Coord: ", coord)	299	# print("Coord: ", coord)
300	m_img_flip = m_img.copy()	300	m_img_flip = m_img.copy()
	@@ -368,7 +368,9 @@ class Viewer(wx.Panel):		@@ -368,7 +368,9 @@ class Viewer(wx.Panel):
368		368
369	self.interactor.Render()	369	self.interactor.Render()
370		370
371	- def OnSensors(self, probe_id, ref_id, obj_id=0):	371	+ def OnSensors(self, markers_flag):
		372	+ probe_id, ref_id, obj_id = markers_flag
		373	+
372	if not self.probe:	374	if not self.probe:
373	self.CreateSensorID()	375	self.CreateSensorID()
374		376
	@@ -48,7 +48,7 @@ class ICP():		@@ -48,7 +48,7 @@ class ICP():
48	def OnICP(self, navigation, tracker, m_change):	48	def OnICP(self, navigation, tracker, m_change):
49	ref_mode_id = navigation.GetReferenceMode()	49	ref_mode_id = navigation.GetReferenceMode()
50		50
51	- dialog = dlg.ICPCorregistrationDialog(nav_prop=(m_change, tracker.tracker_id, tracker.trk_init, ref_mode_id))	51	+ dialog = dlg.ICPCorregistrationDialog(nav_prop=(m_change, tracker, ref_mode_id))
52		52
53	if dialog.ShowModal() == wx.ID_OK:	53	if dialog.ShowModal() == wx.ID_OK:
54	m_icp, point_coord, transformed_points, prev_error, final_error = dialog.GetValue()	54	m_icp, point_coord, transformed_points, prev_error, final_error = dialog.GetValue()
	@@ -255,7 +255,7 @@ class Navigation():		@@ -255,7 +255,7 @@ class Navigation():
255	coreg_data = [m_change, obj_ref_mode]	255	coreg_data = [m_change, obj_ref_mode]
256		256
257	if self.ref_mode_id:	257	if self.ref_mode_id:
258	- coord_raw = dco.GetCoordinates(tracker.trk_init, tracker.tracker_id, self.ref_mode_id)	258	+ coord_raw, markers_flag = tracker.TrackerCoordinates.GetCoordinates()
259	else:	259	else:
260	coord_raw = np.array([None])	260	coord_raw = np.array([None])
261		261