From 4aea8c2373ded30b7af08541df77f65cb90bbd73 Mon Sep 17 00:00:00 2001 From: Renan Date: Mon, 19 Jul 2021 15:11:03 -0300 Subject: [PATCH] Polaris p4 support (#281) --- invesalius/constants.py | 12 +++++++----- invesalius/data/coordinates.py | 48 ++++++++++++++++++++++++++++++++++++++++++++++++ invesalius/data/trackers.py | 32 +++++++++++++++++++++++++++++++- invesalius/gui/dialogs.py | 7 ++++--- navigation/ndi_files/Markers/coilP4.rom | Bin 0 -> 752 bytes navigation/ndi_files/Markers/probe_tipP4.rom | Bin 0 -> 752 bytes navigation/ndi_files/Markers/refP4.rom | Bin 0 -> 752 bytes 7 files changed, 90 insertions(+), 9 deletions(-) create mode 100644 navigation/ndi_files/Markers/coilP4.rom create mode 100644 navigation/ndi_files/Markers/probe_tipP4.rom create mode 100644 navigation/ndi_files/Markers/refP4.rom diff --git a/invesalius/constants.py b/invesalius/constants.py index aaf23aa..2ebe085 100644 --- a/invesalius/constants.py +++ b/invesalius/constants.py @@ -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'COM1' 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 diff --git a/invesalius/data/coordinates.py b/invesalius/data/coordinates.py index d73ab48..10399b1 100644 --- a/invesalius/data/coordinates.py +++ b/invesalius/data/coordinates.py @@ -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() diff --git a/invesalius/data/trackers.py b/invesalius/data/trackers.py index 0fa9631..fb1f862 100644 --- a/invesalius/data/trackers.py +++ b/invesalius/data/trackers.py @@ -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.') diff --git a/invesalius/gui/dialogs.py b/invesalius/gui/dialogs.py index c9fe563..139ffbb 100644 --- a/invesalius/gui/dialogs.py +++ b/invesalius/gui/dialogs.py @@ -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("") diff --git a/navigation/ndi_files/Markers/coilP4.rom b/navigation/ndi_files/Markers/coilP4.rom new file mode 100644 index 0000000..03687e2 Binary files /dev/null and b/navigation/ndi_files/Markers/coilP4.rom differ diff --git a/navigation/ndi_files/Markers/probe_tipP4.rom b/navigation/ndi_files/Markers/probe_tipP4.rom new file mode 100644 index 0000000..b72e4d3 Binary files /dev/null and b/navigation/ndi_files/Markers/probe_tipP4.rom differ diff --git a/navigation/ndi_files/Markers/refP4.rom b/navigation/ndi_files/Markers/refP4.rom new file mode 100644 index 0000000..e7e43e1 Binary files /dev/null and b/navigation/ndi_files/Markers/refP4.rom differ -- libgit2 0.21.2