Polaris p4 support (#281)

* ADD: support polaris P4 * ENH: remove tracker IDs * ADD: PolarisP4 roms files * FIX: disconnect - debug approach tracker

Polaris p4 support (#281)
* ADD: support polaris P4 * ENH: remove tracker IDs * ADD: PolarisP4 roms files * FIX: disconnect - debug approach tracker
Renan · GitHub
1 parent 5af2a8ad
Showing 7 changed files with 90 additions and 9 deletions Show diff stats
invesalius/constants.py
invesalius/data/coordinates.py
invesalius/data/trackers.py
invesalius/gui/dialogs.py
navigation/ndi_files/Markers/coilP4.rom
navigation/ndi_files/Markers/probe_tipP4.rom
navigation/ndi_files/Markers/refP4.rom
@@ -666,10 +666,11 @@ ISOTRAKII = 3
 PATRIOT = 4
 CAMERA = 5
 POLARIS = 6
-OPTITRACK = 7
-DEBUGTRACKRANDOM = 8
-DEBUGTRACKAPPROACH = 9
-DISCTRACK = 10
+POLARISP4 = 7
+OPTITRACK = 8
+DEBUGTRACKRANDOM = 9
+DEBUGTRACKAPPROACH = 10
+DISCTRACK = 11
 DEFAULT_TRACKER = SELECT
  
 NDICOMPORT = b'COM1'
@@ -677,7 +678,8 @@ NDICOMPORT = b&#39;COM1&#39;
 TRACKER = [_("Select tracker:"), _("Claron MicronTracker"),
            _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
            _("Polhemus PATRIOT"), _("Camera tracker"),
-           _("NDI Polaris"), _("Optitrack"),_("Debug tracker (random)"),
+           _("NDI Polaris"), _("NDI Polaris P4"),
+           _("Optitrack"), _("Debug tracker (random)"),
            _("Debug tracker (approach)"), _("Disconnect tracker")]
  
 STATIC_REF = 0
@@ -48,6 +48,7 @@ def GetCoordinates(trck_init, trck_id, ref_mode):
                     const.PATRIOT: PolhemusCoord,
                     const.CAMERA: CameraCoord,
                     const.POLARIS: PolarisCoord,
+                    const.POLARISP4: PolarisP4Coord,
                     const.OPTITRACK: OptitrackCoord,
                     const.DEBUGTRACKRANDOM: DebugCoordRandom,
                     const.DEBUGTRACKAPPROACH: DebugCoordRandom}
@@ -121,6 +122,53 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
  
     return coord
  
+def PolarisP4Coord(trck_init, trck_id, ref_mode):
+    trck = trck_init[0]
+    trck.Run()
+
+    probe = trck.probe.decode(const.FS_ENCODE)
+    ref = trck.ref.decode(const.FS_ENCODE)
+    obj = trck.obj.decode(const.FS_ENCODE)
+
+    probe = probe[2:]
+    ref = ref[2:]
+    obj = obj[2:]
+
+    if probe[:7] == "MISSING":
+        if not "coord1" in locals():
+            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():
+            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():
+            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))
+
+    Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID, obj_id=trck.objID)
+    coord = np.vstack([coord1, coord2, coord3])
+
+    return coord
+
 def CameraCoord(trck_init, trck_id, ref_mode):
     trck = trck_init[0]
     coord, probeID, refID = trck.Run()
@@ -40,6 +40,7 @@ def TrackerConnection(tracker_id, trck_init, action):
                     const.PATRIOT: PolhemusTracker,    # PATRIOT
                     const.CAMERA: CameraTracker,      # CAMERA
                     const.POLARIS: PolarisTracker,      # POLARIS
+                    const.POLARISP4: PolarisP4Tracker,  # POLARISP4
                     const.OPTITRACK: OptitrackTracker,   #Optitrack
                     const.DEBUGTRACKRANDOM: DebugTrackerRandom,
                     const.DEBUGTRACKAPPROACH: DebugTrackerApproach}
@@ -119,6 +120,35 @@ def PolarisTracker(tracker_id):
     return trck_init, lib_mode
  
  
+def PolarisP4Tracker(tracker_id):
+    from wx import ID_OK
+    trck_init = None
+    dlg_port = dlg.SetNDIconfigs()
+    if dlg_port.ShowModal() == ID_OK:
+        com_port, PROBE_DIR, REF_DIR, OBJ_DIR = dlg_port.GetValue()
+        try:
+            import pypolarisP4
+            lib_mode = 'wrapper'
+            trck_init = pypolarisP4.pypolarisP4()
+
+            if trck_init.Initialize(com_port, PROBE_DIR, REF_DIR, OBJ_DIR) != 0:
+                trck_init = None
+                lib_mode = None
+                print('Could not connect to Polaris P4 tracker.')
+            else:
+                print('Connect to Polaris P4 tracking device.')
+
+        except:
+            lib_mode = 'error'
+            trck_init = None
+            print('Could not connect to Polaris P4 tracker.')
+    else:
+        lib_mode = None
+        print('Could not connect to Polaris P4 tracker.')
+    # return tracker initialization variable and type of connection
+    return trck_init, lib_mode
+
+
 def CameraTracker(tracker_id):
     trck_init = None
     try:
@@ -305,7 +335,7 @@ def DisconnectTracker(tracker_id, trck_init):
     :param trck_init: Initialization variable of tracking device.
     """
  
-    if tracker_id == const.DEBUGTRACKRANDOM:
+    if tracker_id == const.DEBUGTRACKRANDOM or tracker_id == const.DEBUGTRACKAPPROACH:
         trck_init = False
         lib_mode = 'debug'
         print('Debug tracker disconnected.')
@@ -869,6 +869,7 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode):
             const.PATRIOT: 'Polhemus PATRIOT',
             const.CAMERA: 'CAMERA',
             const.POLARIS: 'NDI Polaris',
+            const.POLARISP4: 'NDI Polaris P4',
             const.OPTITRACK: 'Optitrack',
             const.DEBUGTRACKRANDOM: 'Debug tracker device (random)',
             const.DEBUGTRACKAPPROACH: 'Debug tracker device (approach)'}
@@ -4146,20 +4147,20 @@ class SetNDIconfigs(wx.Dialog):
                                            wildcard="Rom files (*.rom)|*.rom", message="Select probe's rom file")
         row_probe = wx.BoxSizer(wx.VERTICAL)
         row_probe.Add(wx.StaticText(self, wx.ID_ANY, "Set probe's rom file"), 0, wx.TOP|wx.RIGHT, 5)
-        row_probe.Add(self.dir_probe, 0, wx.ALIGN_CENTER)
+        row_probe.Add(self.dir_probe, 0, wx.ALL | wx.CENTER | wx.EXPAND)
  
         self.dir_ref = wx.FilePickerCtrl(self, path=last_ndi_ref_marker, style=wx.FLP_USE_TEXTCTRL|wx.FLP_SMALL,
                                          wildcard="Rom files (*.rom)|*.rom", message="Select reference's rom file")
         row_ref = wx.BoxSizer(wx.VERTICAL)
         row_ref.Add(wx.StaticText(self, wx.ID_ANY, "Set reference's rom file"), 0, wx.TOP | wx.RIGHT, 5)
-        row_ref.Add(self.dir_ref, 0, wx.ALIGN_CENTER)
+        row_ref.Add(self.dir_ref, 0, wx.ALL | wx.CENTER | wx.EXPAND)
  
         self.dir_obj = wx.FilePickerCtrl(self, path=last_ndi_obj_marker, style=wx.FLP_USE_TEXTCTRL|wx.FLP_SMALL,
                                          wildcard="Rom files (*.rom)|*.rom", message="Select object's rom file")
         #self.dir_probe.Bind(wx.EVT_FILEPICKER_CHANGED, self.Selected)
         row_obj = wx.BoxSizer(wx.VERTICAL)
         row_obj.Add(wx.StaticText(self, wx.ID_ANY, "Set object's rom file"), 0, wx.TOP|wx.RIGHT, 5)
-        row_obj.Add(self.dir_obj, 0, wx.ALIGN_CENTER)
+        row_obj.Add(self.dir_obj, 0, wx.ALL | wx.CENTER | wx.EXPAND)
  
         btn_ok = wx.Button(self, wx.ID_OK)
         btn_ok.SetHelpText("")
...	...	@@ -666,10 +666,11 @@ ISOTRAKII = 3
666	666	PATRIOT = 4
667	667	CAMERA = 5
668	668	POLARIS = 6
669		-OPTITRACK = 7
670		-DEBUGTRACKRANDOM = 8
671		-DEBUGTRACKAPPROACH = 9
672		-DISCTRACK = 10
	669	+POLARISP4 = 7
	670	+OPTITRACK = 8
	671	+DEBUGTRACKRANDOM = 9
	672	+DEBUGTRACKAPPROACH = 10
	673	+DISCTRACK = 11
673	674	DEFAULT_TRACKER = SELECT
674	675
675	676	NDICOMPORT = b'COM1'
...	...	@@ -677,7 +678,8 @@ NDICOMPORT = b'COM1'
677	678	TRACKER = [_("Select tracker:"), _("Claron MicronTracker"),
678	679	_("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
679	680	_("Polhemus PATRIOT"), _("Camera tracker"),
680		- _("NDI Polaris"), _("Optitrack"),_("Debug tracker (random)"),
	681	+ _("NDI Polaris"), _("NDI Polaris P4"),
	682	+ _("Optitrack"), _("Debug tracker (random)"),
681	683	_("Debug tracker (approach)"), _("Disconnect tracker")]
682	684
683	685	STATIC_REF = 0
...	...
...	...	@@ -48,6 +48,7 @@ def GetCoordinates(trck_init, trck_id, ref_mode):
48	48	const.PATRIOT: PolhemusCoord,
49	49	const.CAMERA: CameraCoord,
50	50	const.POLARIS: PolarisCoord,
	51	+ const.POLARISP4: PolarisP4Coord,
51	52	const.OPTITRACK: OptitrackCoord,
52	53	const.DEBUGTRACKRANDOM: DebugCoordRandom,
53	54	const.DEBUGTRACKAPPROACH: DebugCoordRandom}
...	...	@@ -121,6 +122,53 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
121	122
122	123	return coord
123	124
	125	+def PolarisP4Coord(trck_init, trck_id, ref_mode):
	126	+ trck = trck_init[0]
	127	+ trck.Run()
	128	+
	129	+ probe = trck.probe.decode(const.FS_ENCODE)
	130	+ ref = trck.ref.decode(const.FS_ENCODE)
	131	+ obj = trck.obj.decode(const.FS_ENCODE)
	132	+
	133	+ probe = probe[2:]
	134	+ ref = ref[2:]
	135	+ obj = obj[2:]
	136	+
	137	+ if probe[:7] == "MISSING":
	138	+ if not "coord1" in locals():
	139	+ coord1 = np.hstack(([0, 0, 0], [0, 0, 0]))
	140	+ else:
	141	+ q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
	142	+ t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
	143	+ angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
	144	+ trans_probe = np.array(t).astype(float)
	145	+ coord1 = np.hstack((trans_probe, angles_probe))
	146	+
	147	+ if ref[:7] == "MISSING":
	148	+ if not "coord2" in locals():
	149	+ coord2 = np.hstack(([0, 0, 0], [0, 0, 0]))
	150	+ else:
	151	+ q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
	152	+ t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
	153	+ angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
	154	+ trans_ref = np.array(t).astype(float)
	155	+ coord2 = np.hstack((trans_ref, angles_ref))
	156	+
	157	+ if obj[:7] == "MISSING":
	158	+ if not "coord3" in locals():
	159	+ coord3 = np.hstack(([0, 0, 0], [0, 0, 0]))
	160	+ else:
	161	+ q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
	162	+ t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
	163	+ angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
	164	+ trans_obj = np.array(t).astype(float)
	165	+ coord3 = np.hstack((trans_obj, angles_obj))
	166	+
	167	+ Publisher.sendMessage('Sensors ID', probe_id=trck.probeID, ref_id=trck.refID, obj_id=trck.objID)
	168	+ coord = np.vstack([coord1, coord2, coord3])
	169	+
	170	+ return coord
	171	+
124	172	def CameraCoord(trck_init, trck_id, ref_mode):
125	173	trck = trck_init[0]
126	174	coord, probeID, refID = trck.Run()
...	...
...	...	@@ -40,6 +40,7 @@ def TrackerConnection(tracker_id, trck_init, action):
40	40	const.PATRIOT: PolhemusTracker, # PATRIOT
41	41	const.CAMERA: CameraTracker, # CAMERA
42	42	const.POLARIS: PolarisTracker, # POLARIS
	43	+ const.POLARISP4: PolarisP4Tracker, # POLARISP4
43	44	const.OPTITRACK: OptitrackTracker, #Optitrack
44	45	const.DEBUGTRACKRANDOM: DebugTrackerRandom,
45	46	const.DEBUGTRACKAPPROACH: DebugTrackerApproach}
...	...	@@ -119,6 +120,35 @@ def PolarisTracker(tracker_id):
119	120	return trck_init, lib_mode
120	121
121	122
	123	+def PolarisP4Tracker(tracker_id):
	124	+ from wx import ID_OK
	125	+ trck_init = None
	126	+ dlg_port = dlg.SetNDIconfigs()
	127	+ if dlg_port.ShowModal() == ID_OK:
	128	+ com_port, PROBE_DIR, REF_DIR, OBJ_DIR = dlg_port.GetValue()
	129	+ try:
	130	+ import pypolarisP4
	131	+ lib_mode = 'wrapper'
	132	+ trck_init = pypolarisP4.pypolarisP4()
	133	+
	134	+ if trck_init.Initialize(com_port, PROBE_DIR, REF_DIR, OBJ_DIR) != 0:
	135	+ trck_init = None
	136	+ lib_mode = None
	137	+ print('Could not connect to Polaris P4 tracker.')
	138	+ else:
	139	+ print('Connect to Polaris P4 tracking device.')
	140	+
	141	+ except:
	142	+ lib_mode = 'error'
	143	+ trck_init = None
	144	+ print('Could not connect to Polaris P4 tracker.')
	145	+ else:
	146	+ lib_mode = None
	147	+ print('Could not connect to Polaris P4 tracker.')
	148	+ # return tracker initialization variable and type of connection
	149	+ return trck_init, lib_mode
	150	+
	151	+
122	152	def CameraTracker(tracker_id):
123	153	trck_init = None
124	154	try:
...	...	@@ -305,7 +335,7 @@ def DisconnectTracker(tracker_id, trck_init):
305	335	:param trck_init: Initialization variable of tracking device.
306	336	"""
307	337
308		- if tracker_id == const.DEBUGTRACKRANDOM:
	338	+ if tracker_id == const.DEBUGTRACKRANDOM or tracker_id == const.DEBUGTRACKAPPROACH:
309	339	trck_init = False
310	340	lib_mode = 'debug'
311	341	print('Debug tracker disconnected.')
...	...
...	...	@@ -869,6 +869,7 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode):
869	869	const.PATRIOT: 'Polhemus PATRIOT',
870	870	const.CAMERA: 'CAMERA',
871	871	const.POLARIS: 'NDI Polaris',
	872	+ const.POLARISP4: 'NDI Polaris P4',
872	873	const.OPTITRACK: 'Optitrack',
873	874	const.DEBUGTRACKRANDOM: 'Debug tracker device (random)',
874	875	const.DEBUGTRACKAPPROACH: 'Debug tracker device (approach)'}
...	...	@@ -4146,20 +4147,20 @@ class SetNDIconfigs(wx.Dialog):
4146	4147	wildcard="Rom files (.rom)\|.rom", message="Select probe's rom file")
4147	4148	row_probe = wx.BoxSizer(wx.VERTICAL)
4148	4149	row_probe.Add(wx.StaticText(self, wx.ID_ANY, "Set probe's rom file"), 0, wx.TOP\|wx.RIGHT, 5)
4149		- row_probe.Add(self.dir_probe, 0, wx.ALIGN_CENTER)
	4150	+ row_probe.Add(self.dir_probe, 0, wx.ALL \| wx.CENTER \| wx.EXPAND)
4150	4151
4151	4152	self.dir_ref = wx.FilePickerCtrl(self, path=last_ndi_ref_marker, style=wx.FLP_USE_TEXTCTRL\|wx.FLP_SMALL,
4152	4153	wildcard="Rom files (.rom)\|.rom", message="Select reference's rom file")
4153	4154	row_ref = wx.BoxSizer(wx.VERTICAL)
4154	4155	row_ref.Add(wx.StaticText(self, wx.ID_ANY, "Set reference's rom file"), 0, wx.TOP \| wx.RIGHT, 5)
4155		- row_ref.Add(self.dir_ref, 0, wx.ALIGN_CENTER)
	4156	+ row_ref.Add(self.dir_ref, 0, wx.ALL \| wx.CENTER \| wx.EXPAND)
4156	4157
4157	4158	self.dir_obj = wx.FilePickerCtrl(self, path=last_ndi_obj_marker, style=wx.FLP_USE_TEXTCTRL\|wx.FLP_SMALL,
4158	4159	wildcard="Rom files (.rom)\|.rom", message="Select object's rom file")
4159	4160	#self.dir_probe.Bind(wx.EVT_FILEPICKER_CHANGED, self.Selected)
4160	4161	row_obj = wx.BoxSizer(wx.VERTICAL)
4161	4162	row_obj.Add(wx.StaticText(self, wx.ID_ANY, "Set object's rom file"), 0, wx.TOP\|wx.RIGHT, 5)
4162		- row_obj.Add(self.dir_obj, 0, wx.ALIGN_CENTER)
	4163	+ row_obj.Add(self.dir_obj, 0, wx.ALL \| wx.CENTER \| wx.EXPAND)
4163	4164
4164	4165	btn_ok = wx.Button(self, wx.ID_OK)
4165	4166	btn_ok.SetHelpText("")
...	...