Download as
Browse Code »

Commit 4aea8c2373ded30b7af08541df77f65cb90bbd73

Authored by Renan
Committed by GitHub
1 parent 5af2a8ad
Exists in master

Polaris p4 support (#281) 

* ADD: support polaris P4

* ENH: remove tracker IDs

* ADD: PolarisP4 roms files

* FIX: disconnect - debug approach tracker
Showing 7 changed files with 90 additions and 9 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @4aea8c2
@@ -666,10 +666,11 @@ ISOTRAKII = 3 @@ -666,10 +666,11 @@ ISOTRAKII = 3
666 PATRIOT = 4 666 PATRIOT = 4
667 CAMERA = 5 667 CAMERA = 5
668 POLARIS = 6 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 DEFAULT_TRACKER = SELECT 674 DEFAULT_TRACKER = SELECT
674 675
675 NDICOMPORT = b'COM1' 676 NDICOMPORT = b'COM1'
@@ -677,7 +678,8 @@ NDICOMPORT = b'COM1' @@ -677,7 +678,8 @@ NDICOMPORT = b'COM1'
677 TRACKER = [_("Select tracker:"), _("Claron MicronTracker"), 678 TRACKER = [_("Select tracker:"), _("Claron MicronTracker"),
678 _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"), 679 _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
679 _("Polhemus PATRIOT"), _("Camera tracker"), 680 _("Polhemus PATRIOT"), _("Camera tracker"),
680 - _("NDI Polaris"), _("Optitrack"),_("Debug tracker (random)"), 681 + _("NDI Polaris"), _("NDI Polaris P4"),
  682 + _("Optitrack"), _("Debug tracker (random)"),
681 _("Debug tracker (approach)"), _("Disconnect tracker")] 683 _("Debug tracker (approach)"), _("Disconnect tracker")]
682 684
683 STATIC_REF = 0 685 STATIC_REF = 0
invesalius/data/coordinates.py
  Diff comments   View file @4aea8c2
@@ -48,6 +48,7 @@ def GetCoordinates(trck_init, trck_id, ref_mode): @@ -48,6 +48,7 @@ def GetCoordinates(trck_init, trck_id, ref_mode):
48 const.PATRIOT: PolhemusCoord, 48 const.PATRIOT: PolhemusCoord,
49 const.CAMERA: CameraCoord, 49 const.CAMERA: CameraCoord,
50 const.POLARIS: PolarisCoord, 50 const.POLARIS: PolarisCoord,
  51 + const.POLARISP4: PolarisP4Coord,
51 const.OPTITRACK: OptitrackCoord, 52 const.OPTITRACK: OptitrackCoord,
52 const.DEBUGTRACKRANDOM: DebugCoordRandom, 53 const.DEBUGTRACKRANDOM: DebugCoordRandom,
53 const.DEBUGTRACKAPPROACH: DebugCoordRandom} 54 const.DEBUGTRACKAPPROACH: DebugCoordRandom}
@@ -121,6 +122,53 @@ def PolarisCoord(trck_init, trck_id, ref_mode): @@ -121,6 +122,53 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
121 122
122 return coord 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 def CameraCoord(trck_init, trck_id, ref_mode): 172 def CameraCoord(trck_init, trck_id, ref_mode):
125 trck = trck_init[0] 173 trck = trck_init[0]
126 coord, probeID, refID = trck.Run() 174 coord, probeID, refID = trck.Run()
invesalius/data/trackers.py
  Diff comments   View file @4aea8c2
@@ -40,6 +40,7 @@ def TrackerConnection(tracker_id, trck_init, action): @@ -40,6 +40,7 @@ def TrackerConnection(tracker_id, trck_init, action):
40 const.PATRIOT: PolhemusTracker, # PATRIOT 40 const.PATRIOT: PolhemusTracker, # PATRIOT
41 const.CAMERA: CameraTracker, # CAMERA 41 const.CAMERA: CameraTracker, # CAMERA
42 const.POLARIS: PolarisTracker, # POLARIS 42 const.POLARIS: PolarisTracker, # POLARIS
  43 + const.POLARISP4: PolarisP4Tracker, # POLARISP4
43 const.OPTITRACK: OptitrackTracker, #Optitrack 44 const.OPTITRACK: OptitrackTracker, #Optitrack
44 const.DEBUGTRACKRANDOM: DebugTrackerRandom, 45 const.DEBUGTRACKRANDOM: DebugTrackerRandom,
45 const.DEBUGTRACKAPPROACH: DebugTrackerApproach} 46 const.DEBUGTRACKAPPROACH: DebugTrackerApproach}
@@ -119,6 +120,35 @@ def PolarisTracker(tracker_id): @@ -119,6 +120,35 @@ def PolarisTracker(tracker_id):
119 return trck_init, lib_mode 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 def CameraTracker(tracker_id): 152 def CameraTracker(tracker_id):
123 trck_init = None 153 trck_init = None
124 try: 154 try:
@@ -305,7 +335,7 @@ def DisconnectTracker(tracker_id, trck_init): @@ -305,7 +335,7 @@ def DisconnectTracker(tracker_id, trck_init):
305 :param trck_init: Initialization variable of tracking device. 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 trck_init = False 339 trck_init = False
310 lib_mode = 'debug' 340 lib_mode = 'debug'
311 print('Debug tracker disconnected.') 341 print('Debug tracker disconnected.')
invesalius/gui/dialogs.py
  Diff comments   View file @4aea8c2
@@ -869,6 +869,7 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode): @@ -869,6 +869,7 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode):
869 const.PATRIOT: 'Polhemus PATRIOT', 869 const.PATRIOT: 'Polhemus PATRIOT',
870 const.CAMERA: 'CAMERA', 870 const.CAMERA: 'CAMERA',
871 const.POLARIS: 'NDI Polaris', 871 const.POLARIS: 'NDI Polaris',
  872 + const.POLARISP4: 'NDI Polaris P4',
872 const.OPTITRACK: 'Optitrack', 873 const.OPTITRACK: 'Optitrack',
873 const.DEBUGTRACKRANDOM: 'Debug tracker device (random)', 874 const.DEBUGTRACKRANDOM: 'Debug tracker device (random)',
874 const.DEBUGTRACKAPPROACH: 'Debug tracker device (approach)'} 875 const.DEBUGTRACKAPPROACH: 'Debug tracker device (approach)'}
@@ -4146,20 +4147,20 @@ class SetNDIconfigs(wx.Dialog): @@ -4146,20 +4147,20 @@ class SetNDIconfigs(wx.Dialog):
4146 wildcard="Rom files (*.rom)|*.rom", message="Select probe's rom file") 4147 wildcard="Rom files (*.rom)|*.rom", message="Select probe's rom file")
4147 row_probe = wx.BoxSizer(wx.VERTICAL) 4148 row_probe = wx.BoxSizer(wx.VERTICAL)
4148 row_probe.Add(wx.StaticText(self, wx.ID_ANY, "Set probe's rom file"), 0, wx.TOP|wx.RIGHT, 5) 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 self.dir_ref = wx.FilePickerCtrl(self, path=last_ndi_ref_marker, style=wx.FLP_USE_TEXTCTRL|wx.FLP_SMALL, 4152 self.dir_ref = wx.FilePickerCtrl(self, path=last_ndi_ref_marker, style=wx.FLP_USE_TEXTCTRL|wx.FLP_SMALL,
4152 wildcard="Rom files (*.rom)|*.rom", message="Select reference's rom file") 4153 wildcard="Rom files (*.rom)|*.rom", message="Select reference's rom file")
4153 row_ref = wx.BoxSizer(wx.VERTICAL) 4154 row_ref = wx.BoxSizer(wx.VERTICAL)
4154 row_ref.Add(wx.StaticText(self, wx.ID_ANY, "Set reference's rom file"), 0, wx.TOP | wx.RIGHT, 5) 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 self.dir_obj = wx.FilePickerCtrl(self, path=last_ndi_obj_marker, style=wx.FLP_USE_TEXTCTRL|wx.FLP_SMALL, 4158 self.dir_obj = wx.FilePickerCtrl(self, path=last_ndi_obj_marker, style=wx.FLP_USE_TEXTCTRL|wx.FLP_SMALL,
4158 wildcard="Rom files (*.rom)|*.rom", message="Select object's rom file") 4159 wildcard="Rom files (*.rom)|*.rom", message="Select object's rom file")
4159 #self.dir_probe.Bind(wx.EVT_FILEPICKER_CHANGED, self.Selected) 4160 #self.dir_probe.Bind(wx.EVT_FILEPICKER_CHANGED, self.Selected)
4160 row_obj = wx.BoxSizer(wx.VERTICAL) 4161 row_obj = wx.BoxSizer(wx.VERTICAL)
4161 row_obj.Add(wx.StaticText(self, wx.ID_ANY, "Set object's rom file"), 0, wx.TOP|wx.RIGHT, 5) 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 btn_ok = wx.Button(self, wx.ID_OK) 4165 btn_ok = wx.Button(self, wx.ID_OK)
4165 btn_ok.SetHelpText("") 4166 btn_ok.SetHelpText("")
navigation/ndi_files/Markers/coilP4.rom 0 → 100644
View file @4aea8c2
No preview for this file type
navigation/ndi_files/Markers/probe_tipP4.rom 0 → 100644
View file @4aea8c2
No preview for this file type
navigation/ndi_files/Markers/refP4.rom 0 → 100644
View file @4aea8c2
No preview for this file type