Download as
Browse Code »

Commit 3185fc2b24e5c0fad0a6c35c645c34babe08a2a6

Authored by sotodela
Committed by GitHub
2 parents 52a9c203 e9f5883b
Exists in master

Merge branch 'invesalius:master' into 278_add_visualization_for_the_direction_of_the_marker

Showing 23 changed files with 1600 additions and 308 deletions   Show diff stats
app.py
  Diff comments   View file @3185fc2
... ... @@ -87,11 +87,11 @@ session = ses.Session()
87 87 if session.ReadSession():
88 88 lang = session.GetLanguage()
89 89 if lang:
90   - LANG = lang
91 90 try:
92 91 _ = i18n.InstallLanguage(lang)
  92 + LANG = lang
93 93 except FileNotFoundError:
94   - LANG = None
  94 + pass
95 95  
96 96  
97 97 class InVesalius(wx.App):
... ... @@ -134,7 +134,7 @@ class Inv3SplashScreen(SplashScreen):
134 134 Splash screen to be shown in InVesalius initialization.
135 135 """
136 136 def __init__(self):
137   - # Splash screen image will depend on currently language
  137 + # Splash screen image will depend on the current language
138 138 lang = LANG
139 139 self.locale = wx.Locale(wx.LANGUAGE_ENGLISH)
140 140  
... ... @@ -143,22 +143,20 @@ class Inv3SplashScreen(SplashScreen):
143 143 # should be created
144 144 create_session = LANG is None
145 145  
146   - install_lang = 0
  146 + install_lang = False
147 147 if lang:
148 148 _ = i18n.InstallLanguage(lang)
149   - install_lang = 1
150   - else:
151   - install_lang = 0
  149 + install_lang = True
152 150  
153 151 # If no language is set into session file, show dialog so
154 152 # user can select language
155   - if install_lang == 0:
  153 + if not install_lang:
156 154 dialog = lang_dlg.LanguageDialog()
157 155  
158 156 # FIXME: This works ok in linux2, darwin and win32,
159 157 # except on win64, due to wxWidgets bug
160 158 try:
161   - ok = (dialog.ShowModal() == wx.ID_OK)
  159 + ok = dialog.ShowModal() == wx.ID_OK
162 160 except wx._core.PyAssertionError:
163 161 ok = True
164 162 finally:
... ... @@ -174,9 +172,8 @@ class Inv3SplashScreen(SplashScreen):
174 172  
175 173 dialog.Destroy()
176 174  
177   - # Session file should be created... So we set the recent
178   - # choosen language
179   - if (create_session):
  175 + # Session file should be created... So we set the recently chosen language.
  176 + if create_session:
180 177 session.CreateItens()
181 178 session.SetLanguage(lang)
182 179 session.WriteSessionFile()
... ... @@ -184,8 +181,7 @@ class Inv3SplashScreen(SplashScreen):
184 181 # session.SaveConfigFileBackup()
185 182  
186 183  
187   - # Only after language was defined, splash screen will be
188   - # shown
  184 + # Only after language was defined, splash screen will be shown.
189 185 if lang:
190 186  
191 187 #import locale
... ... @@ -206,9 +202,7 @@ class Inv3SplashScreen(SplashScreen):
206 202 abs_file_path = os.path.abspath(".." + os.sep)
207 203 path = abs_file_path
208 204 path = os.path.join(path, 'icons', icon_file)
209   -
210 205 else:
211   -
212 206 path = os.path.join(inv_paths.ICON_DIR, icon_file)
213 207 if not os.path.exists(path):
214 208 path = os.path.join(inv_paths.ICON_DIR, "splash_en.png")
... ... @@ -241,7 +235,7 @@ class Inv3SplashScreen(SplashScreen):
241 235 self.control = Controller(self.main)
242 236  
243 237 self.fc = wx.CallLater(200, self.ShowMain)
244   - options, args = parse_comand_line()
  238 + options, args = parse_command_line()
245 239 wx.CallLater(1, use_cmd_optargs, options, args)
246 240  
247 241 # Check for updates
... ... @@ -292,7 +286,7 @@ def non_gui_startup(options, args):
292 286 # ------------------------------------------------------------------
293 287  
294 288  
295   -def parse_comand_line():
  289 +def parse_command_line():
296 290 """
297 291 Handle command line arguments.
298 292 """
... ... @@ -302,7 +296,7 @@ def parse_comand_line():
302 296 # Parse command line arguments
303 297 parser = op.OptionParser()
304 298  
305   - # -d or --debug: print all pubsub messagessent
  299 + # -d or --debug: print all pubsub messages sent
306 300 parser.add_option("-d", "--debug",
307 301 action="store_true",
308 302 dest="debug")
... ... @@ -353,12 +347,6 @@ def parse_comand_line():
353 347  
354 348  
355 349 def use_cmd_optargs(options, args):
356   - # If debug argument...
357   - if options.debug:
358   - Publisher.subscribe(print_events, Publisher.ALL_TOPICS)
359   - session = ses.Session()
360   - session.debug = 1
361   -
362 350 # If import DICOM argument...
363 351 if options.dicom_dir:
364 352 import_dir = options.dicom_dir
... ... @@ -494,11 +482,18 @@ def print_events(topic=Publisher.AUTO_TOPIC, **msg_data):
494 482 """
495 483 utils.debug("%s\n\tParameters: %s" % (topic, msg_data))
496 484  
  485 +
497 486 def main():
498 487 """
499 488 Initialize InVesalius GUI
500 489 """
501   - options, args = parse_comand_line()
  490 + options, args = parse_command_line()
  491 +
  492 + session = ses.Session()
  493 + session.debug = options.debug
  494 +
  495 + if options.debug:
  496 + Publisher.subscribe(print_events, Publisher.ALL_TOPICS)
502 497  
503 498 if options.remote_host is not None:
504 499 from invesalius.net.remote_control import RemoteControl
... ... @@ -517,6 +512,7 @@ def main():
517 512 application = InVesalius(0)
518 513 application.MainLoop()
519 514  
  515 +
520 516 if __name__ == '__main__':
521 517 #Is needed because of pyinstaller
522 518 multiprocessing.freeze_support()
... ...
invesalius/constants.py
  Diff comments   View file @3185fc2
... ... @@ -655,6 +655,18 @@ BOOLEAN_DIFF = 2
655 655 BOOLEAN_AND = 3
656 656 BOOLEAN_XOR = 4
657 657  
  658 +# -------------- User interface ---------------------
  659 +
  660 +# The column order in the marker panel
  661 +#
  662 +ID_COLUMN = 0
  663 +SESSION_COLUMN = 1
  664 +LABEL_COLUMN = 2
  665 +TARGET_COLUMN = 3
  666 +X_COLUMN = 4
  667 +Y_COLUMN = 5
  668 +Z_COLUMN = 6
  669 +
658 670 #------------ Navigation defaults -------------------
659 671  
660 672 MARKER_COLOUR = (1.0, 1.0, 0.)
... ... @@ -672,18 +684,19 @@ CAMERA = 5
672 684 POLARIS = 6
673 685 POLARISP4 = 7
674 686 OPTITRACK = 8
675   -DEBUGTRACKRANDOM = 9
676   -DEBUGTRACKAPPROACH = 10
  687 +ROBOT = 9
  688 +DEBUGTRACKRANDOM = 10
  689 +DEBUGTRACKAPPROACH = 11
677 690 DEFAULT_TRACKER = SELECT
678 691  
679 692 NDICOMPORT = b'COM1'
680 693  
681 694 TRACKERS = [_("Claron MicronTracker"),
682   - _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
683   - _("Polhemus PATRIOT"), _("Camera tracker"),
684   - _("NDI Polaris"), _("NDI Polaris P4"),
685   - _("Optitrack"), _("Debug tracker (random)"),
686   - _("Debug tracker (approach)")]
  695 + _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
  696 + _("Polhemus PATRIOT"), _("Camera tracker"),
  697 + _("NDI Polaris"), _("NDI Polaris P4"),
  698 + _("Optitrack"), _("Robot tracker"),
  699 + _("Debug tracker (random)"), _("Debug tracker (approach)")]
687 700  
688 701 STATIC_REF = 0
689 702 DYNAMIC_REF = 1
... ... @@ -835,3 +848,13 @@ WILDCARD_MARKER_FILES = _("Marker scanner coord files (*.mkss)|*.mkss")
835 848 BAUD_RATES = [300, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200]
836 849 BAUD_RATE_DEFAULT_SELECTION = 4
837 850 PULSE_DURATION_IN_MILLISECONDS = 0.2
  851 +
  852 +#Robot
  853 +ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1']
  854 +ROBOT_ElFIN_PORT = 10003
  855 +ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2}
  856 +ROBOT_HEAD_VELOCITY_THRESHOLD = 10 #mm/s
  857 +ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm
  858 +ROBOT_VERSOR_SCALE_FACTOR = 70
  859 +#Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%.
  860 +ROBOT_WORKING_SPACE = 760 #mm
... ...
invesalius/control.py
  Diff comments   View file @3185fc2
... ... @@ -965,7 +965,7 @@ class Controller():
965 965 re_dialog = dialog.ResizeImageDialog()
966 966 re_dialog.SetValue(int(resolution_percentage*100))
967 967 re_dialog_value = re_dialog.ShowModal()
968   - re_dialog.Close()
  968 + re_dialog.Close()
969 969  
970 970 if re_dialog_value == wx.ID_OK:
971 971 percentage = re_dialog.GetValue()
... ... @@ -1143,7 +1143,7 @@ class Controller():
1143 1143 def show_mask_preview(self, index, flag=True):
1144 1144 proj = prj.Project()
1145 1145 mask = proj.mask_dict[index]
1146   - slc = self.Slice.do_threshold_to_all_slices(mask)
  1146 + self.Slice.do_threshold_to_all_slices(mask)
1147 1147 mask.create_3d_preview()
1148 1148 Publisher.sendMessage("Load mask preview", mask_3d_actor=mask.volume._actor, flag=flag)
1149 1149 Publisher.sendMessage("Reload actual slice")
... ...
invesalius/data/bases.py
  Diff comments   View file @3185fc2
... ... @@ -244,3 +244,47 @@ def object_registration(fiducials, orients, coord_raw, m_change):
244 244 )
245 245  
246 246 return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
  247 +
  248 +def compute_robot_to_head_matrix(raw_target_robot):
  249 + """
  250 + :param head: nx6 array of head coordinates from tracking device in robot space
  251 + :param robot: nx6 array of robot coordinates
  252 +
  253 + :return: target_robot_matrix: 3x3 array representing change of basis from robot to head in robot system
  254 + """
  255 + head, robot = raw_target_robot
  256 + # compute head target matrix
  257 + m_head_target = dco.coordinates_to_transformation_matrix(
  258 + position=head[:3],
  259 + orientation=head[3:],
  260 + axes='rzyx',
  261 + )
  262 +
  263 + # compute robot target matrix
  264 + m_robot_target = dco.coordinates_to_transformation_matrix(
  265 + position=robot[:3],
  266 + orientation=robot[3:],
  267 + axes='rzyx',
  268 + )
  269 + robot_to_head_matrix = np.linalg.inv(m_head_target) @ m_robot_target
  270 +
  271 + return robot_to_head_matrix
  272 +
  273 +
  274 +class transform_tracker_to_robot(object):
  275 + M_tracker_to_robot = np.array([])
  276 + def transformation_tracker_to_robot(self, tracker_coord):
  277 + if not transform_tracker_to_robot.M_tracker_to_robot.any():
  278 + return None
  279 +
  280 + M_tracker = dco.coordinates_to_transformation_matrix(
  281 + position=tracker_coord[:3],
  282 + orientation=tracker_coord[3:6],
  283 + axes='rzyx',
  284 + )
  285 + M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker
  286 +
  287 + translation, angles_as_deg = dco.transformation_matrix_to_coordinates(M_tracker_in_robot, axes='rzyx')
  288 + tracker_in_robot = list(translation) + list(angles_as_deg)
  289 +
  290 + return tracker_in_robot
... ...
invesalius/data/coordinates.py
  Diff comments   View file @3185fc2
... ... @@ -19,7 +19,6 @@
19 19  
20 20 from math import sin, cos
21 21 import numpy as np
22   -import queue
23 22 import threading
24 23 import wx
25 24  
... ... @@ -35,13 +34,28 @@ class TrackerCoordinates():
35 34 def __init__(self):
36 35 self.coord = None
37 36 self.markers_flag = [False, False, False]
  37 + self.previous_markers_flag = self.markers_flag
  38 + self.nav_status = False
  39 + self.__bind_events()
  40 +
  41 + def __bind_events(self):
  42 + Publisher.subscribe(self.OnUpdateNavigationStatus, 'Navigation status')
  43 +
  44 + def OnUpdateNavigationStatus(self, nav_status, vis_status):
  45 + self.nav_status = nav_status
38 46  
39 47 def SetCoordinates(self, coord, markers_flag):
40 48 self.coord = coord
41 49 self.markers_flag = markers_flag
42   - wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
  50 + if self.previous_markers_flag != self.markers_flag and not self.nav_status:
  51 + wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
  52 + self.previous_markers_flag = self.markers_flag
43 53  
44 54 def GetCoordinates(self):
  55 + if self.previous_markers_flag != self.markers_flag and self.nav_status:
  56 + wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
  57 + self.previous_markers_flag = self.markers_flag
  58 +
45 59 return self.coord, self.markers_flag
46 60  
47 61  
... ... @@ -65,15 +79,57 @@ def GetCoordinatesForThread(trck_init, trck_id, ref_mode):
65 79 const.POLARIS: PolarisCoord,
66 80 const.POLARISP4: PolarisP4Coord,
67 81 const.OPTITRACK: OptitrackCoord,
  82 + const.ROBOT: RobotCoord,
68 83 const.DEBUGTRACKRANDOM: DebugCoordRandom,
69 84 const.DEBUGTRACKAPPROACH: DebugCoordRandom}
70   -
71 85 coord, markers_flag = getcoord[trck_id](trck_init, trck_id, ref_mode)
72 86 else:
73 87 print("Select Tracker")
74 88  
75 89 return coord, markers_flag
76 90  
  91 +def PolarisP4Coord(trck_init, trck_id, ref_mode):
  92 + trck = trck_init[0]
  93 + trck.Run()
  94 +
  95 + probe = trck.probe.decode(const.FS_ENCODE)
  96 + ref = trck.ref.decode(const.FS_ENCODE)
  97 + obj = trck.obj.decode(const.FS_ENCODE)
  98 +
  99 + probe = probe[2:]
  100 + ref = ref[2:]
  101 + obj = obj[2:]
  102 +
  103 + if probe[:7] == "MISSING":
  104 + coord1 = np.hstack(([0, 0, 0], [0, 0, 0]))
  105 + else:
  106 + q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
  107 + t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
  108 + angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
  109 + trans_probe = np.array(t).astype(float)
  110 + coord1 = np.hstack((trans_probe, angles_probe))
  111 +
  112 + if ref[:7] == "MISSING":
  113 + coord2 = np.hstack(([0, 0, 0], [0, 0, 0]))
  114 + else:
  115 + q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
  116 + t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
  117 + angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
  118 + trans_ref = np.array(t).astype(float)
  119 + coord2 = np.hstack((trans_ref, angles_ref))
  120 +
  121 + if obj[:7] == "MISSING":
  122 + coord3 = np.hstack(([0, 0, 0], [0, 0, 0]))
  123 + else:
  124 + q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
  125 + t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
  126 + angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
  127 + trans_obj = np.array(t).astype(float)
  128 + coord3 = np.hstack((trans_obj, angles_obj))
  129 +
  130 + coord = np.vstack([coord1, coord2, coord3])
  131 +
  132 + return coord, [trck.probeID, trck.refID, trck.objID]
77 133  
78 134 def OptitrackCoord(trck_init, trck_id, ref_mode):
79 135 """
... ... @@ -139,49 +195,16 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
139 195  
140 196 return coord, [trck.probeID, trck.refID, trck.objID]
141 197  
142   -
143   -def PolarisP4Coord(trck_init, trck_id, ref_mode):
144   - trck = trck_init[0]
145   - trck.Run()
146   -
147   - probe = trck.probe.decode(const.FS_ENCODE)
148   - ref = trck.ref.decode(const.FS_ENCODE)
149   - obj = trck.obj.decode(const.FS_ENCODE)
150   -
151   - probe = probe[2:]
152   - ref = ref[2:]
153   - obj = obj[2:]
154   -
155   - if probe[:7] == "MISSING":
156   - coord1 = np.hstack(([0, 0, 0], [0, 0, 0]))
  198 +def ElfinCoord(trck_init):
  199 + if len(trck_init) > 2:
  200 + robotcoordinates = trck_init[-1]
  201 + coord = robotcoordinates.GetRobotCoordinates()
  202 + if coord is None:
  203 + coord = np.array([0, 0, 0, 0, 0, 0])
157 204 else:
158   - q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
159   - t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
160   - angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
161   - trans_probe = np.array(t).astype(float)
162   - coord1 = np.hstack((trans_probe, angles_probe))
163   - if ref[:7] == "MISSING":
164   - coord2 = np.hstack(([0, 0, 0], [0, 0, 0]))
165   - else:
166   - q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
167   - t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
168   - angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
169   - trans_ref = np.array(t).astype(float)
170   - coord2 = np.hstack((trans_ref, angles_ref))
171   -
172   - if obj[:7] == "MISSING":
173   - coord3 = np.hstack(([0, 0, 0], [0, 0, 0]))
174   - else:
175   - q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
176   - t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
177   - angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
178   - trans_obj = np.array(t).astype(float)
179   - coord3 = np.hstack((trans_obj, angles_obj))
180   -
181   - coord = np.vstack([coord1, coord2, coord3])
182   -
183   - return coord, [trck.probeID, trck.refID, trck.objID]
  205 + coord = np.array([0, 0, 0, 0, 0, 0])
184 206  
  207 + return coord
185 208  
186 209 def CameraCoord(trck_init, trck_id, ref_mode):
187 210 trck = trck_init[0]
... ... @@ -223,11 +246,10 @@ def PolhemusCoord(trck, trck_id, ref_mode):
223 246 elif trck[1] == 'wrapper':
224 247 coord = PolhemusWrapperCoord(trck[0], trck_id, ref_mode)
225 248  
226   - return coord, None
  249 + return coord, [False, False, False]
227 250  
228 251  
229 252 def PolhemusWrapperCoord(trck, trck_id, ref_mode):
230   -
231 253 trck.Run()
232 254 scale = 10.0 * np.array([1., 1., 1.])
233 255  
... ... @@ -475,6 +497,22 @@ def dynamic_reference_m(probe, reference):
475 497 return coord_rot
476 498  
477 499  
  500 +def RobotCoord(trk_init, trck_id, ref_mode):
  501 + coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], trk_init[2], ref_mode)
  502 + coord_robot = ElfinCoord(trk_init[1:])
  503 +
  504 + probe_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[0])
  505 + ref_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[1])
  506 + obj_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[2])
  507 +
  508 + if probe_tracker_in_robot is None:
  509 + probe_tracker_in_robot = coord_tracker[0]
  510 + ref_tracker_in_robot = coord_tracker[1]
  511 + obj_tracker_in_robot = coord_tracker[2]
  512 +
  513 + return np.vstack([probe_tracker_in_robot, ref_tracker_in_robot, coord_robot, obj_tracker_in_robot]), markers_flag
  514 +
  515 +
478 516 def dynamic_reference_m2(probe, reference):
479 517 """
480 518 Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
... ...
invesalius/data/coregistration.py
  Diff comments   View file @3185fc2
... ... @@ -23,9 +23,9 @@ import threading
23 23 from time import sleep
24 24  
25 25 import invesalius.constants as const
26   -import invesalius.data.coordinates as dco
27 26 import invesalius.data.transformations as tr
28 27 import invesalius.data.bases as bases
  28 +import invesalius.data.coordinates as dco
29 29  
30 30  
31 31 # TODO: Replace the use of degrees by radians in every part of the navigation pipeline
... ... @@ -88,9 +88,9 @@ def tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn):
88 88 :type m_probe_ref: numpy.ndarray
89 89 :param r_obj_img: Object coordinate system in image space (3d model)
90 90 :type r_obj_img: numpy.ndarray
91   - :param m_obj_raw: Object basis in raw coordinates from tacker
  91 + :param m_obj_raw: Object basis in raw coordinates from tracker
92 92 :type m_obj_raw: numpy.ndarray
93   - :param s0_dyn:
  93 + :param s0_dyn: Initial alignment of probe fixed in the object in reference (or static) frame
94 94 :type s0_dyn: numpy.ndarray
95 95 :return: 4 x 4 numpy double array
96 96 :rtype: numpy.ndarray
... ... @@ -190,11 +190,14 @@ class CoordinateCorregistrate(threading.Thread):
190 190 self.event = event
191 191 self.sle = sle
192 192 self.icp_queue = queues[2]
  193 + self.object_at_target_queue = queues[3]
193 194 self.icp = None
194 195 self.m_icp = None
  196 + self.robot_tracker_flag = None
195 197 self.last_coord = None
196 198 self.tracker_id = tracker_id
197 199 self.target = target
  200 + self.target_flag = False
198 201  
199 202 if self.target is not None:
200 203 self.target = np.array(self.target)
... ... @@ -214,10 +217,12 @@ class CoordinateCorregistrate(threading.Thread):
214 217 # print('CoordCoreg: event {}'.format(self.event.is_set()))
215 218 while not self.event.is_set():
216 219 try:
217   - if self.icp_queue.empty():
218   - None
219   - else:
  220 + if not self.icp_queue.empty():
220 221 self.icp, self.m_icp = self.icp_queue.get_nowait()
  222 +
  223 + if not self.object_at_target_queue.empty():
  224 + self.target_flag = self.object_at_target_queue.get_nowait()
  225 +
221 226 # print(f"Set the coordinate")
222 227 coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
223 228 coord, m_img = corregistrate_object_dynamic(coreg_data, coord_raw, self.ref_mode_id, [self.icp, self.m_icp])
... ... @@ -249,7 +254,7 @@ class CoordinateCorregistrate(threading.Thread):
249 254 if self.icp:
250 255 m_img = bases.transform_icp(m_img, self.m_icp)
251 256  
252   - self.coord_queue.put_nowait([coord, m_img, view_obj])
  257 + self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
253 258 # print('CoordCoreg: put {}'.format(count))
254 259 # count += 1
255 260  
... ... @@ -258,7 +263,6 @@ class CoordinateCorregistrate(threading.Thread):
258 263  
259 264 if not self.icp_queue.empty():
260 265 self.icp_queue.task_done()
261   -
262 266 # The sleep has to be in both threads
263 267 sleep(self.sle)
264 268 except queue.Full:
... ... @@ -303,7 +307,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
303 307 if self.icp:
304 308 m_img = bases.transform_icp(m_img, self.m_icp)
305 309  
306   - self.coord_queue.put_nowait([coord, m_img, view_obj])
  310 + self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
307 311  
308 312 if self.view_tracts:
309 313 self.coord_tracts_queue.put_nowait(m_img_flip)
... ...
invesalius/data/elfin.py 0 → 100644
  Diff comments   View file @3185fc2
... ... @@ -0,0 +1,249 @@
  1 +#!/usr/bin/env python3
  2 +
  3 +import sys
  4 +from time import sleep
  5 +from socket import socket, AF_INET, SOCK_STREAM
  6 +import invesalius.constants as const
  7 +
  8 +class Elfin_Server():
  9 + """
  10 + This class is similar to tracker devices wrappers.
  11 + It follows the same functions as the others (Initialize, Run and Close)
  12 + """
  13 + def __init__(self, server_ip, port_number):
  14 + self.server_ip = server_ip
  15 + self.port_number = port_number
  16 +
  17 + def Initialize(self):
  18 + message_size = 1024
  19 + robot_id = 0
  20 + self.cobot = Elfin()
  21 + self.cobot.connect(self.server_ip, self.port_number, message_size, robot_id)
  22 + print("connected!")
  23 +
  24 + def Run(self):
  25 + return self.cobot.ReadPcsActualPos()
  26 +
  27 + def SendCoordinates(self, target, type=const.ROBOT_MOTIONS["normal"]):
  28 + """
  29 + It's not possible to send a move command to elfin if the robot is during a move.
  30 + Status 1009 means robot in motion.
  31 + """
  32 + status = self.cobot.ReadMoveState()
  33 + if status != 1009:
  34 + if type == const.ROBOT_MOTIONS["normal"] or type == const.ROBOT_MOTIONS["linear out"]:
  35 + self.cobot.MoveL(target)
  36 + elif type == const.ROBOT_MOTIONS["arc"]:
  37 + self.cobot.MoveC(target)
  38 +
  39 + def StopRobot(self):
  40 + # Takes some microseconds to the robot actual stops after the command.
  41 + # The sleep time is required to guarantee the stop
  42 + self.cobot.GrpStop()
  43 + sleep(0.1)
  44 +
  45 + def Close(self):
  46 + self.cobot.close()
  47 +
  48 +class Elfin:
  49 + def __init__(self):
  50 + """
  51 + Class to communicate with elfin robot. This class follows "HansRobot Communication Protocol Interface".
  52 + """
  53 + self.end_msg = ",;"
  54 +
  55 + def connect(self, server_ip, port_number, message_size, robot_id):
  56 + mySocket = socket(AF_INET, SOCK_STREAM)
  57 + mySocket.connect((server_ip, port_number))
  58 +
  59 + self.message_size = message_size
  60 + self.robot_id = str(robot_id)
  61 + self.mySocket = mySocket
  62 +
  63 + def send(self, message):
  64 + self.mySocket.sendall(message.encode('utf-8'))
  65 + data = self.mySocket.recv(self.message_size).decode('utf-8').split(',')
  66 + status = self.check_status(data)
  67 + if status and type(data) != bool:
  68 + if len(data) > 3:
  69 + return data[2:-1]
  70 + return status
  71 +
  72 + def check_status(self, recv_message):
  73 + status = recv_message[1]
  74 + if status == 'OK':
  75 + return True
  76 +
  77 + elif status == 'Fail':
  78 + print("Error code: ", recv_message[2])
  79 + return False
  80 +
  81 + def Electrify(self):
  82 + """
  83 + Function: Power the robot
  84 + Notes: successful completion of power up before returning, power up time is
  85 + about 44s.
  86 + :return:
  87 + if Error Return False
  88 + if not Error Return True
  89 + """
  90 + message = "Electrify" + self.end_msg
  91 + status = self.send(message)
  92 + return status
  93 +
  94 + def BlackOut(self):
  95 + """
  96 + Function: Robot blackout
  97 + Notes: successful power outage will only return, power failure time is 3s.
  98 + :return:
  99 + if Error Return False
  100 + if not Error Return True
  101 + """
  102 + message = "BlackOut" + self.end_msg
  103 + status = self.send(message)
  104 + return status
  105 +
  106 + def StartMaster(self):
  107 + """
  108 + Function: Start master station
  109 + Notes: the master station will not be returned until successfully started, startup
  110 + master time is about 4s.
  111 + :return:
  112 + if Error Return False
  113 + if not Error Return True
  114 + """
  115 + message = "StartMaster" + self.end_msg
  116 + status = self.send(message)
  117 + return status
  118 +
  119 + def CloseMaster(self):
  120 + """
  121 + Function: Close master station
  122 + Notes: the master station will not be returned until successfully closed, shut
  123 + down the master station time is about 2s.
  124 + :return:
  125 + if Error Return False
  126 + if not Error Return True
  127 + """
  128 + message = "CloseMaster" + self.end_msg
  129 + status = self.send(message)
  130 + return status
  131 +
  132 + def GrpPowerOn(self):
  133 + """
  134 + Function: Robot servo on
  135 + :return:
  136 + if Error Return False
  137 + if not Error Return True
  138 + """
  139 + message = "GrpPowerOn," + self.robot_id + self.end_msg
  140 + status = self.send(message)
  141 + return status
  142 +
  143 + def GrpPowerOff(self):
  144 + """
  145 + Function: Robot servo off
  146 + :return:
  147 + if Error Return False
  148 + if not Error Return True
  149 + """
  150 + message = "GrpPowerOff," + self.robot_id + self.end_msg
  151 + status = self.send(message)
  152 + return status
  153 +
  154 + def GrpStop(self):
  155 + """
  156 + Function: Stop robot
  157 + :return:
  158 + if Error Return False
  159 + if not Error Return True
  160 + """
  161 + message = "GrpStop," + self.robot_id + self.end_msg
  162 + status = self.send(message)
  163 + return status
  164 +
  165 + def SetOverride(self, override):
  166 + """
  167 + function: Set speed ratio
  168 + :param override:
  169 + double: set speed ratio, range of 0.01~1
  170 + :return:
  171 + if Error Return False
  172 + if not Error Return True
  173 + """
  174 +
  175 + message = "SetOverride," + self.robot_id + ',' + str(override) + self.end_msg
  176 + status = self.send(message)
  177 + return status
  178 +
  179 + def ReadPcsActualPos(self):
  180 + """Function: Get the actual position of the space coordinate
  181 + :return:
  182 + if True Return x,y,z,a,b,c
  183 + if Error Return False
  184 + """
  185 + message = "ReadPcsActualPos," + self.robot_id + self.end_msg
  186 + coord = self.send(message)
  187 + if coord:
  188 + return [float(s) for s in coord]
  189 +
  190 + return coord
  191 +
  192 + def MoveL(self, target):
  193 + """
  194 + function: Robot moves straight to the specified space coordinates
  195 + :param: target:[X,Y,Z,RX,RY,RZ]
  196 + :return:
  197 + """
  198 + target = [str(s) for s in target]
  199 + target = (",".join(target))
  200 + message = "MoveL," + self.robot_id + ',' + target + self.end_msg
  201 + return self.send(message)
  202 +
  203 + def SetToolCoordinateMotion(self, status):
  204 + """
  205 + function: Function: Set tool coordinate motion
  206 + :param: int Switch 0=close 1=open
  207 + :return:
  208 + if Error Return False
  209 + if not Error Return True
  210 + """
  211 + message = "SetToolCoordinateMotion," + self.robot_id + ',' + str(status) + self.end_msg
  212 + status = self.send(message)
  213 + return status
  214 +
  215 + def ReadMoveState(self):
  216 + """
  217 + Function: Get the motion state of the robot
  218 + :return:
  219 + Current state of motion of robot:
  220 + 0=motion completion;
  221 + 1009=in motion;
  222 + 1013=waiting for execution;
  223 + 1025 =Error reporting
  224 + """
  225 + message = "ReadMoveState," + self.robot_id + self.end_msg
  226 + status = int(self.send(message)[0])
  227 + return status
  228 +
  229 + def MoveHoming(self):
  230 + """
  231 + Function: Robot returns to the origin
  232 + :return:
  233 + if Error Return False
  234 + if not Error Return True
  235 + """
  236 + message = "MoveHoming," + self.robot_id + self.end_msg
  237 + status = self.send(message)
  238 + return status
  239 +
  240 + def MoveC(self, target):
  241 + """
  242 + function: Arc motion
  243 + :param: Through position[X,Y,Z],GoalCoord[X,Y,Z,RX,RY,RZ],Type[0 or 1],;
  244 + :return:
  245 + """
  246 + target = [str(s) for s in target]
  247 + target = (",".join(target))
  248 + message = "MoveC," + self.robot_id + ',' + target + self.end_msg
  249 + return self.send(message)
... ...
invesalius/data/elfin_processing.py 0 → 100644
  Diff comments   View file @3185fc2
... ... @@ -0,0 +1,212 @@
  1 +#--------------------------------------------------------------------------
  2 +# Software: InVesalius - Software de Reconstrucao 3D de Imagens Medicas
  3 +# Copyright: (C) 2001 Centro de Pesquisas Renato Archer
  4 +# Homepage: http://www.softwarepublico.gov.br
  5 +# Contact: invesalius@cti.gov.br
  6 +# License: GNU - GPL 2 (LICENSE.txt/LICENCA.txt)
  7 +#--------------------------------------------------------------------------
  8 +# Este programa e software livre; voce pode redistribui-lo e/ou
  9 +# modifica-lo sob os termos da Licenca Publica Geral GNU, conforme
  10 +# publicada pela Free Software Foundation; de acordo com a versao 2
  11 +# da Licenca.
  12 +#
  13 +# Este programa eh distribuido na expectativa de ser util, mas SEM
  14 +# QUALQUER GARANTIA; sem mesmo a garantia implicita de
  15 +# COMERCIALIZACAO ou de ADEQUACAO A QUALQUER PROPOSITO EM
  16 +# PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais
  17 +# detalhes.
  18 +#--------------------------------------------------------------------------
  19 +import numpy as np
  20 +import cv2
  21 +from time import time
  22 +
  23 +import invesalius.data.coregistration as dcr
  24 +import invesalius.data.coordinates as dco
  25 +import invesalius.constants as const
  26 +
  27 +
  28 +class KalmanTracker:
  29 + """
  30 + Kalman filter to avoid sudden fluctuation from tracker device.
  31 + The filter strength can be set by the cov_process, and cov_measure parameter
  32 + It is required to create one instance for each variable (x, y, z, a, b, g)
  33 + """
  34 + def __init__(self,
  35 + state_num=2,
  36 + covariance_process=0.001,
  37 + covariance_measure=0.1):
  38 +
  39 + self.state_num = state_num
  40 + measure_num = 1
  41 +
  42 + # The filter itself.
  43 + self.filter = cv2.KalmanFilter(state_num, measure_num, 0)
  44 +
  45 + self.state = np.zeros((state_num, 1), dtype=np.float32)
  46 + self.measurement = np.array((measure_num, 1), np.float32)
  47 + self.prediction = np.zeros((state_num, 1), np.float32)
  48 +
  49 +
  50 + self.filter.transitionMatrix = np.array([[1, 1],
  51 + [0, 1]], np.float32)
  52 + self.filter.measurementMatrix = np.array([[1, 1]], np.float32)
  53 + self.filter.processNoiseCov = np.array([[1, 0],
  54 + [0, 1]], np.float32) * covariance_process
  55 + self.filter.measurementNoiseCov = np.array([[1]], np.float32) * covariance_measure
  56 +
  57 + def update_kalman(self, measurement):
  58 + self.prediction = self.filter.predict()
  59 + self.measurement = np.array([[np.float32(measurement[0])]])
  60 +
  61 + self.filter.correct(self.measurement)
  62 + self.state = self.filter.statePost
  63 +
  64 +
  65 +class TrackerProcessing:
  66 + def __init__(self):
  67 + self.coord_vel = []
  68 + self.timestamp = []
  69 + self.velocity_vector = []
  70 + self.kalman_coord_vector = []
  71 + self.velocity_std = 0
  72 +
  73 + self.tracker_stabilizers = [KalmanTracker(
  74 + state_num=2,
  75 + covariance_process=0.001,
  76 + covariance_measure=0.1) for _ in range(6)]
  77 +
  78 + def kalman_filter(self, coord_tracker):
  79 + kalman_array = []
  80 + pose_np = np.array((coord_tracker[:3], coord_tracker[3:])).flatten()
  81 + for value, ps_stb in zip(pose_np, self.tracker_stabilizers):
  82 + ps_stb.update_kalman([value])
  83 + kalman_array.append(ps_stb.state[0])
  84 + coord_kalman = np.hstack(kalman_array)
  85 +
  86 + self.kalman_coord_vector.append(coord_kalman[:3])
  87 + if len(self.kalman_coord_vector) < 20: #avoid initial fluctuations
  88 + coord_kalman = coord_tracker
  89 + print('initializing filter')
  90 + else:
  91 + del self.kalman_coord_vector[0]
  92 +
  93 + return coord_kalman
  94 +
  95 + def estimate_head_velocity(self, coord_vel, timestamp):
  96 + coord_vel = np.vstack(np.array(coord_vel))
  97 + coord_init = coord_vel[:int(len(coord_vel) / 2)].mean(axis=0)
  98 + coord_final = coord_vel[int(len(coord_vel) / 2):].mean(axis=0)
  99 + velocity = (coord_final - coord_init)/(timestamp[-1] - timestamp[0])
  100 + distance = (coord_final - coord_init)
  101 +
  102 + return velocity, distance
  103 +
  104 + def compute_versors(self, init_point, final_point):
  105 + init_point = np.array(init_point)
  106 + final_point = np.array(final_point)
  107 + norm = (sum((final_point - init_point) ** 2)) ** 0.5
  108 + versor_factor = (((final_point-init_point) / norm) * const.ROBOT_VERSOR_SCALE_FACTOR).tolist()
  109 +
  110 + return versor_factor
  111 +
  112 +
  113 + def compute_arc_motion(self, current_robot_coordinates, head_center_coordinates, new_robot_coordinates):
  114 + head_center = head_center_coordinates[0], head_center_coordinates[1], head_center_coordinates[2], \
  115 + new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5]
  116 +
  117 + versor_factor_move_out = self.compute_versors(head_center, current_robot_coordinates)
  118 + init_move_out_point = current_robot_coordinates[0] + versor_factor_move_out[0], \
  119 + current_robot_coordinates[1] + versor_factor_move_out[1], \
  120 + current_robot_coordinates[2] + versor_factor_move_out[2], \
  121 + current_robot_coordinates[3], current_robot_coordinates[4], current_robot_coordinates[5]
  122 +
  123 + middle_point = ((new_robot_coordinates[0] + current_robot_coordinates[0]) / 2,
  124 + (new_robot_coordinates[1] + current_robot_coordinates[1]) / 2,
  125 + (new_robot_coordinates[2] + current_robot_coordinates[2]) / 2,
  126 + 0, 0, 0)
  127 + versor_factor_middle_arc = (np.array(self.compute_versors(head_center, middle_point))) * 2
  128 + middle_arc_point = middle_point[0] + versor_factor_middle_arc[0], \
  129 + middle_point[1] + versor_factor_middle_arc[1], \
  130 + middle_point[2] + versor_factor_middle_arc[2]
  131 +
  132 + versor_factor_arc = self.compute_versors(head_center, new_robot_coordinates)
  133 + final_ext_arc_point = new_robot_coordinates[0] + versor_factor_arc[0], \
  134 + new_robot_coordinates[1] + versor_factor_arc[1], \
  135 + new_robot_coordinates[2] + versor_factor_arc[2], \
  136 + new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5], 0
  137 +
  138 + target_arc = middle_arc_point + final_ext_arc_point
  139 +
  140 + return init_move_out_point, target_arc
  141 +
  142 + def compute_head_move_threshold(self, current_ref):
  143 + """
  144 + Checks if the head velocity is bellow the threshold
  145 + """
  146 + self.coord_vel.append(current_ref)
  147 + self.timestamp.append(time())
  148 + if len(self.coord_vel) >= 10:
  149 + head_velocity, head_distance = self.estimate_head_velocity(self.coord_vel, self.timestamp)
  150 + self.velocity_vector.append(head_velocity)
  151 +
  152 + del self.coord_vel[0]
  153 + del self.timestamp[0]
  154 +
  155 + if len(self.velocity_vector) >= 30:
  156 + self.velocity_std = np.std(self.velocity_vector)
  157 + del self.velocity_vector[0]
  158 +
  159 + if self.velocity_std > const.ROBOT_HEAD_VELOCITY_THRESHOLD:
  160 + print('Velocity threshold activated')
  161 + return False
  162 + else:
  163 + return True
  164 +
  165 + return False
  166 +
  167 + def compute_head_move_compensation(self, current_head, m_change_robot_to_head):
  168 + """
  169 + Estimates the new robot position to reach the target
  170 + """
  171 + M_current_head = dco.coordinates_to_transformation_matrix(
  172 + position=current_head[:3],
  173 + orientation=current_head[3:6],
  174 + axes='rzyx',
  175 + )
  176 + m_robot_new = M_current_head @ m_change_robot_to_head
  177 +
  178 + translation, angles_as_deg = dco.transformation_matrix_to_coordinates(m_robot_new, axes='sxyz')
  179 + new_robot_position = list(translation) + list(angles_as_deg)
  180 +
  181 + return new_robot_position
  182 +
  183 + def estimate_head_center(self, tracker, current_head):
  184 + """
  185 + Estimates the actual head center position using fiducials
  186 + """
  187 + m_probe_head_left, m_probe_head_right, m_probe_head_nasion = tracker.GetMatrixTrackerFiducials()
  188 + m_current_head = dcr.compute_marker_transformation(np.array([current_head]), 0)
  189 +
  190 + m_ear_left_new = m_current_head @ m_probe_head_left
  191 + m_ear_right_new = m_current_head @ m_probe_head_right
  192 +
  193 + return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2
  194 +
  195 + def correction_distance_calculation_target(self, coord_inv, actual_point):
  196 + """
  197 + Estimates the Euclidean distance between the actual position and the target
  198 + """
  199 + correction_distance_compensation = np.sqrt((coord_inv[0]-actual_point[0]) ** 2 +
  200 + (coord_inv[1]-actual_point[1]) ** 2 +
  201 + (coord_inv[2]-actual_point[2]) ** 2)
  202 +
  203 + return correction_distance_compensation
  204 +
  205 + def estimate_robot_target_length(self, robot_target):
  206 + """
  207 + Estimates the length of the 3D vector of the robot target
  208 + """
  209 + robot_target_length = np.sqrt(robot_target[0] ** 2 + robot_target[1] ** 2 + robot_target[2] ** 2)
  210 +
  211 + return robot_target_length
  212 +
... ...
invesalius/data/slice_.py
  Diff comments   View file @3185fc2
... ... @@ -264,8 +264,6 @@ class Slice(metaclass=utils.Singleton):
264 264 def OnRemoveMasks(self, mask_indexes):
265 265 proj = Project()
266 266 for item in mask_indexes:
267   - proj.RemoveMask(item)
268   -
269 267 # if the deleted mask is the current mask, cleans the current mask
270 268 # and discard from buffer all datas related to mask.
271 269 if self.current_mask is not None and item == self.current_mask.index:
... ... @@ -277,6 +275,7 @@ class Slice(metaclass=utils.Singleton):
277 275  
278 276 Publisher.sendMessage("Show mask", index=item, value=False)
279 277 Publisher.sendMessage("Reload actual slice")
  278 + proj.RemoveMask(item)
280 279  
281 280 def OnDuplicateMasks(self, mask_indexes):
282 281 proj = Project()
... ... @@ -1185,6 +1184,7 @@ class Slice(metaclass=utils.Singleton):
1185 1184 future_mask = proj.GetMask(index)
1186 1185 future_mask.is_shown = True
1187 1186 self.current_mask = future_mask
  1187 + self.current_mask.on_show()
1188 1188  
1189 1189 colour = future_mask.colour
1190 1190 self.SetMaskColour(index, colour, update=False)
... ...
invesalius/data/trackers.py
  Diff comments   View file @3185fc2
... ... @@ -41,7 +41,8 @@ def TrackerConnection(tracker_id, trck_init, action):
41 41 const.CAMERA: CameraTracker, # CAMERA
42 42 const.POLARIS: PolarisTracker, # POLARIS
43 43 const.POLARISP4: PolarisP4Tracker, # POLARISP4
44   - const.OPTITRACK: OptitrackTracker, #Optitrack
  44 + const.OPTITRACK: OptitrackTracker, #Optitrack
  45 + const.ROBOT: RobotTracker, #Robot
45 46 const.DEBUGTRACKRANDOM: DebugTrackerRandom,
46 47 const.DEBUGTRACKAPPROACH: DebugTrackerApproach}
47 48  
... ... @@ -65,36 +66,68 @@ def DefaultTracker(tracker_id):
65 66 # return tracker initialization variable and type of connection
66 67 return trck_init, 'wrapper'
67 68  
68   -def OptitrackTracker(tracker_id):
69   - """
70   - Imports optitrack wrapper from Motive 2.2. Initialize cameras, attach listener, loads Calibration, loads User Profile
71   - (Rigid bodies information).
  69 +def ClaronTracker(tracker_id):
  70 + import invesalius.constants as const
  71 + from invesalius import inv_paths
72 72  
73   - Parameters
74   - ----------
75   - tracker_id : Optitrack ID
  73 + trck_init = None
  74 + try:
  75 + import pyclaron
76 76  
77   - Returns
78   - -------
79   - trck_init : local name for Optitrack module
80   - """
81   - from wx import ID_OK
  77 + lib_mode = 'wrapper'
  78 + trck_init = pyclaron.pyclaron()
  79 + trck_init.CalibrationDir = inv_paths.MTC_CAL_DIR.encode(const.FS_ENCODE)
  80 + trck_init.MarkerDir = inv_paths.MTC_MAR_DIR.encode(const.FS_ENCODE)
  81 + trck_init.NumberFramesProcessed = 1
  82 + trck_init.FramesExtrapolated = 0
  83 + trck_init.PROBE_NAME = const.MTC_PROBE_NAME.encode(const.FS_ENCODE)
  84 + trck_init.REF_NAME = const.MTC_REF_NAME.encode(const.FS_ENCODE)
  85 + trck_init.OBJ_NAME = const.MTC_OBJ_NAME.encode(const.FS_ENCODE)
  86 + trck_init.Initialize()
  87 +
  88 + if trck_init.GetIdentifyingCamera():
  89 + trck_init.Run()
  90 + print("MicronTracker camera identified.")
  91 + else:
  92 + trck_init = None
  93 +
  94 + except ImportError:
  95 + lib_mode = 'error'
  96 + print('The ClaronTracker library is not installed.')
  97 +
  98 + return trck_init, lib_mode
  99 +
  100 +def PolhemusTracker(tracker_id):
  101 + try:
  102 + trck_init = PlhWrapperConnection(tracker_id)
  103 + lib_mode = 'wrapper'
  104 + if not trck_init:
  105 + print('Could not connect with Polhemus wrapper, trying USB connection...')
  106 + trck_init = PlhUSBConnection(tracker_id)
  107 + lib_mode = 'usb'
  108 + if not trck_init:
  109 + print('Could not connect with Polhemus USB, trying serial connection...')
  110 + trck_init = PlhSerialConnection(tracker_id)
  111 + lib_mode = 'serial'
  112 + except:
  113 + trck_init = None
  114 + lib_mode = 'error'
  115 + print('Could not connect to Polhemus by any method.')
  116 +
  117 + return trck_init, lib_mode
  118 +
  119 +def CameraTracker(tracker_id):
82 120 trck_init = None
83   - dlg_port = dlg.SetOptitrackconfigs()
84   - if dlg_port.ShowModal() == ID_OK:
85   - Cal_optitrack, User_profile_optitrack = dlg_port.GetValue()
86   - try:
87   - import optitrack
88   - trck_init = optitrack.optr()
  121 + try:
  122 + import invesalius.data.camera_tracker as cam
  123 + trck_init = cam.camera()
  124 + trck_init.Initialize()
  125 + print('Connect to camera tracking device.')
89 126  
90   - if trck_init.Initialize(Cal_optitrack, User_profile_optitrack)==0:
91   - trck_init.Run() #Runs once Run function, to update cameras.
92   - else:
93   - trck_init = None
94   - except ImportError:
95   - print('Error')
96   - else:
97   - print('#####')
  127 + except:
  128 + print('Could not connect to camera tracker.')
  129 +
  130 + # return tracker initialization variable and type of connection
98 131 return trck_init, 'wrapper'
99 132  
100 133 def PolarisTracker(tracker_id):
... ... @@ -111,22 +144,21 @@ def PolarisTracker(tracker_id):
111 144 if trck_init.Initialize(com_port, PROBE_DIR, REF_DIR, OBJ_DIR) != 0:
112 145 trck_init = None
113 146 lib_mode = None
114   - print('Could not connect to default tracker.')
  147 + print('Could not connect to polaris tracker.')
115 148 else:
116 149 print('Connect to polaris tracking device.')
117 150  
118 151 except:
119 152 lib_mode = 'error'
120 153 trck_init = None
121   - print('Could not connect to default tracker.')
  154 + print('Could not connect to polaris tracker.')
122 155 else:
123 156 lib_mode = None
124   - print('Could not connect to default tracker.')
  157 + print('Could not connect to polaris tracker.')
125 158  
126 159 # return tracker initialization variable and type of connection
127 160 return trck_init, lib_mode
128 161  
129   -
130 162 def PolarisP4Tracker(tracker_id):
131 163 from wx import ID_OK
132 164 trck_init = None
... ... @@ -155,73 +187,74 @@ def PolarisP4Tracker(tracker_id):
155 187 # return tracker initialization variable and type of connection
156 188 return trck_init, lib_mode
157 189  
  190 +def OptitrackTracker(tracker_id):
  191 + """
  192 + Imports optitrack wrapper from Motive 2.2. Initialize cameras, attach listener, loads Calibration, loads User Profile
  193 + (Rigid bodies information).
  194 +
  195 + Parameters
  196 + ----------
  197 + tracker_id : Optitrack ID
158 198  
159   -def CameraTracker(tracker_id):
  199 + Returns
  200 + -------
  201 + trck_init : local name for Optitrack module
  202 + """
  203 + from wx import ID_OK
160 204 trck_init = None
161   - try:
162   - import invesalius.data.camera_tracker as cam
163   - trck_init = cam.camera()
164   - trck_init.Initialize()
165   - print('Connect to camera tracking device.')
166   -
167   - except:
168   - print('Could not connect to default tracker.')
  205 + dlg_port = dlg.SetOptitrackconfigs()
  206 + if dlg_port.ShowModal() == ID_OK:
  207 + Cal_optitrack, User_profile_optitrack = dlg_port.GetValue()
  208 + try:
  209 + import optitrack
  210 + trck_init = optitrack.optr()
169 211  
170   - # return tracker initialization variable and type of connection
  212 + if trck_init.Initialize(Cal_optitrack, User_profile_optitrack)==0:
  213 + trck_init.Run() #Runs once Run function, to update cameras.
  214 + else:
  215 + trck_init = None
  216 + except ImportError:
  217 + print('Error')
  218 + else:
  219 + print('#####')
171 220 return trck_init, 'wrapper'
172 221  
173   -
174   -def ClaronTracker(tracker_id):
175   - import invesalius.constants as const
176   - from invesalius import inv_paths
177   -
  222 +def ElfinRobot(robot_IP):
178 223 trck_init = None
179 224 try:
180   - import pyclaron
181   -
182   - lib_mode = 'wrapper'
183   - trck_init = pyclaron.pyclaron()
184   - trck_init.CalibrationDir = inv_paths.MTC_CAL_DIR.encode(const.FS_ENCODE)
185   - trck_init.MarkerDir = inv_paths.MTC_MAR_DIR.encode(const.FS_ENCODE)
186   - trck_init.NumberFramesProcessed = 1
187   - trck_init.FramesExtrapolated = 0
188   - trck_init.PROBE_NAME = const.MTC_PROBE_NAME.encode(const.FS_ENCODE)
189   - trck_init.REF_NAME = const.MTC_REF_NAME.encode(const.FS_ENCODE)
190   - trck_init.OBJ_NAME = const.MTC_OBJ_NAME.encode(const.FS_ENCODE)
  225 + import invesalius.data.elfin as elfin
  226 + print("Trying to connect Robot via: ", robot_IP)
  227 + trck_init = elfin.Elfin_Server(robot_IP, const.ROBOT_ElFIN_PORT)
191 228 trck_init.Initialize()
192   -
193   - if trck_init.GetIdentifyingCamera():
194   - trck_init.Run()
195   - print("MicronTracker camera identified.")
196   - else:
197   - trck_init = None
198   -
199   - except ImportError:
200   - lib_mode = 'error'
201   - print('The ClaronTracker library is not installed.')
202   -
203   - return trck_init, lib_mode
204   -
205   -
206   -def PolhemusTracker(tracker_id):
207   - try:
208   - trck_init = PlhWrapperConnection(tracker_id)
209 229 lib_mode = 'wrapper'
210   - if not trck_init:
211   - print('Could not connect with Polhemus wrapper, trying USB connection...')
212   - trck_init = PlhUSBConnection(tracker_id)
213   - lib_mode = 'usb'
214   - if not trck_init:
215   - print('Could not connect with Polhemus USB, trying serial connection...')
216   - trck_init = PlhSerialConnection(tracker_id)
217   - lib_mode = 'serial'
  230 + print('Connect to elfin robot tracking device.')
  231 +
218 232 except:
  233 + lib_mode = 'disconnect'
219 234 trck_init = None
220   - lib_mode = 'error'
221   - print('Could not connect to Polhemus by any method.')
  235 + print('Could not connect to elfin robot tracker.')
222 236  
  237 + # return tracker initialization variable and type of connection
223 238 return trck_init, lib_mode
224 239  
  240 +def RobotTracker(tracker_id):
  241 + from wx import ID_OK
  242 + trck_init = None
  243 + trck_init_robot = None
  244 + tracker_id = None
  245 + dlg_device = dlg.SetTrackerDeviceToRobot()
  246 + if dlg_device.ShowModal() == ID_OK:
  247 + tracker_id = dlg_device.GetValue()
  248 + if tracker_id:
  249 + trck_connection = TrackerConnection(tracker_id, None, 'connect')
  250 + if trck_connection[0]:
  251 + dlg_ip = dlg.SetRobotIP()
  252 + if dlg_ip.ShowModal() == ID_OK:
  253 + robot_IP = dlg_ip.GetValue()
  254 + trck_init = trck_connection
  255 + trck_init_robot = ElfinRobot(robot_IP)
  256 +
  257 + return [trck_init, trck_init_robot, tracker_id]
225 258  
226 259 def DebugTrackerRandom(tracker_id):
227 260 trck_init = True
... ... @@ -294,10 +327,10 @@ def PlhSerialConnection(tracker_id):
294 327 except:
295 328 lib_mode = 'error'
296 329 trck_init = None
297   - print('Could not connect to default tracker.')
  330 + print('Could not connect to Polhemus tracker.')
298 331 else:
299 332 lib_mode = None
300   - print('Could not connect to default tracker.')
  333 + print('Could not connect to Polhemus tracker.')
301 334  
302 335 return trck_init
303 336  
... ... @@ -335,7 +368,6 @@ def PlhUSBConnection(tracker_id):
335 368  
336 369 return trck_init
337 370  
338   -
339 371 def DisconnectTracker(tracker_id, trck_init):
340 372 """
341 373 Disconnect current spatial tracker
... ... @@ -355,6 +387,11 @@ def DisconnectTracker(tracker_id, trck_init):
355 387 trck_init = False
356 388 lib_mode = 'serial'
357 389 print('Tracker disconnected.')
  390 + elif tracker_id == const.ROBOT:
  391 + trck_init[0].Close()
  392 + trck_init = False
  393 + lib_mode = 'wrapper'
  394 + print('Tracker disconnected.')
358 395 else:
359 396 trck_init.Close()
360 397 trck_init = False
... ...
invesalius/data/tractography.py
  Diff comments   View file @3185fc2
... ... @@ -633,7 +633,7 @@ class ComputeTractsThreadSingleBlock(threading.Thread):
633 633 # print('ComputeTractsThread: event {}'.format(self.event.is_set()))
634 634 while not self.event.is_set():
635 635 try:
636   - coord, m_img, m_img_flip = self.coord_queue.get_nowait()
  636 + coord, [coord_raw, markers_flag], m_img, m_img_flip = self.coord_queue.get_nowait()
637 637  
638 638 # translate the coordinate along the normal vector of the object/coil
639 639 coord_offset = m_img_flip[:3, -1] - offset * m_img_flip[:3, 2]
... ...
invesalius/data/viewer_volume.py
  Diff comments   View file @3185fc2
... ... @@ -987,10 +987,10 @@ class Viewer(wx.Panel):
987 987  
988 988 if thrdist and thrcoordx and thrcoordy and thrcoordz:
989 989 self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('Green'))
990   - Publisher.sendMessage('Coil at target', state=True)
  990 + wx.CallAfter(Publisher.sendMessage, 'Coil at target', state=True)
991 991 else:
992 992 self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('DarkOrange'))
993   - Publisher.sendMessage('Coil at target', state=False)
  993 + wx.CallAfter(Publisher.sendMessage, 'Coil at target', state=False)
994 994  
995 995 self.arrow_actor_list = arrow_roll_x1, arrow_roll_x2, arrow_yaw_z1, arrow_yaw_z2, \
996 996 arrow_pitch_y1, arrow_pitch_y2
... ...
invesalius/gui/dialogs.py
  Diff comments   View file @3185fc2
... ... @@ -48,6 +48,7 @@ from wx.lib import masked
48 48 from wx.lib.agw import floatspin
49 49 from wx.lib.wordwrap import wordwrap
50 50 from invesalius.pubsub import pub as Publisher
  51 +import csv
51 52  
52 53 try:
53 54 from wx.adv import AboutDialogInfo, AboutBox
... ... @@ -56,6 +57,7 @@ except ImportError:
56 57  
57 58 import invesalius.constants as const
58 59 import invesalius.data.coordinates as dco
  60 +import invesalius.data.transformations as tr
59 61 import invesalius.gui.widgets.gradient as grad
60 62 import invesalius.session as ses
61 63 import invesalius.utils as utils
... ... @@ -873,6 +875,7 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode):
873 875 const.POLARIS: 'NDI Polaris',
874 876 const.POLARISP4: 'NDI Polaris P4',
875 877 const.OPTITRACK: 'Optitrack',
  878 + const.ROBOT: 'Robotic navigation',
876 879 const.DEBUGTRACKRANDOM: 'Debug tracker device (random)',
877 880 const.DEBUGTRACKAPPROACH: 'Debug tracker device (approach)'}
878 881  
... ... @@ -3309,7 +3312,6 @@ class ObjectCalibrationDialog(wx.Dialog):
3309 3312 self.pedal_connection = pedal_connection
3310 3313  
3311 3314 self.trk_init, self.tracker_id = tracker.GetTrackerInfo()
3312   -
3313 3315 self.obj_ref_id = 2
3314 3316 self.obj_name = None
3315 3317 self.polydata = None
... ... @@ -3594,8 +3596,14 @@ class ObjectCalibrationDialog(wx.Dialog):
3594 3596 # and not change the function to the point of potentially breaking it.)
3595 3597 #
3596 3598 if self.obj_ref_id and fiducial_index == 4:
3597   - coord = coord_raw[self.obj_ref_id, :]
3598   - coord[2] = -coord[2]
  3599 + if self.tracker_id == const.ROBOT:
  3600 + trck_init_robot = self.trk_init[1][0]
  3601 + coord = trck_init_robot.Run()
  3602 + else:
  3603 + coord = coord_raw[self.obj_ref_id, :]
  3604 + else:
  3605 + coord = coord_raw[0, :]
  3606 + coord[2] = -coord[2]
3599 3607  
3600 3608 if fiducial_index == 3:
3601 3609 coord = np.zeros([6,])
... ... @@ -4301,6 +4309,312 @@ class SetOptitrackconfigs(wx.Dialog):
4301 4309  
4302 4310 return fn_cal, fn_userprofile
4303 4311  
  4312 +class SetTrackerDeviceToRobot(wx.Dialog):
  4313 + """
  4314 + Robot navigation requires a tracker device to tracker the head position and the object (coil) position.
  4315 + A dialog pops up showing a combobox with all trackers but debugs and the robot itself (const.TRACKERS[:-3])
  4316 + """
  4317 + def __init__(self, title=_("Setting tracker device:")):
  4318 + wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200),
  4319 + style=wx.DEFAULT_DIALOG_STYLE|wx.FRAME_FLOAT_ON_PARENT|wx.STAY_ON_TOP|wx.RESIZE_BORDER)
  4320 + self.tracker_id = const.DEFAULT_TRACKER
  4321 + self._init_gui()
  4322 +
  4323 + def _init_gui(self):
  4324 + # ComboBox for spatial tracker device selection
  4325 + tooltip = wx.ToolTip(_("Choose the tracking device"))
  4326 + tracker_options = [_("Select tracker:")] + const.TRACKERS[:-3]
  4327 + choice_trck = wx.ComboBox(self, -1, "",
  4328 + choices=tracker_options, style=wx.CB_DROPDOWN | wx.CB_READONLY)
  4329 + choice_trck.SetToolTip(tooltip)
  4330 + choice_trck.SetSelection(const.DEFAULT_TRACKER)
  4331 + choice_trck.Bind(wx.EVT_COMBOBOX, partial(self.OnChoiceTracker, ctrl=choice_trck))
  4332 +
  4333 + btn_ok = wx.Button(self, wx.ID_OK)
  4334 + btn_ok.SetHelpText("")
  4335 + btn_ok.SetDefault()
  4336 +
  4337 + btn_cancel = wx.Button(self, wx.ID_CANCEL)
  4338 + btn_cancel.SetHelpText("")
  4339 +
  4340 + btnsizer = wx.StdDialogButtonSizer()
  4341 + btnsizer.AddButton(btn_ok)
  4342 + btnsizer.AddButton(btn_cancel)
  4343 + btnsizer.Realize()
  4344 +
  4345 + main_sizer = wx.BoxSizer(wx.VERTICAL)
  4346 +
  4347 + main_sizer.Add((5, 5))
  4348 + main_sizer.Add(choice_trck, 1, wx.EXPAND|wx.LEFT|wx.RIGHT, 5)
  4349 + main_sizer.Add((15, 15))
  4350 + main_sizer.Add(btnsizer, 0, wx.EXPAND)
  4351 + main_sizer.Add((5, 5))
  4352 +
  4353 + self.SetSizer(main_sizer)
  4354 + main_sizer.Fit(self)
  4355 +
  4356 + self.CenterOnParent()
  4357 +
  4358 + def OnChoiceTracker(self, evt, ctrl):
  4359 + choice = evt.GetSelection()
  4360 + self.tracker_id = choice
  4361 +
  4362 + def GetValue(self):
  4363 + return self.tracker_id
  4364 +
  4365 +class SetRobotIP(wx.Dialog):
  4366 + def __init__(self, title=_("Setting Robot IP")):
  4367 + wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200),
  4368 + style=wx.DEFAULT_DIALOG_STYLE|wx.FRAME_FLOAT_ON_PARENT|wx.STAY_ON_TOP|wx.RESIZE_BORDER)
  4369 + self.robot_ip = None
  4370 + self._init_gui()
  4371 +
  4372 + def _init_gui(self):
  4373 + # ComboBox for spatial tracker device selection
  4374 + tooltip = wx.ToolTip(_("Choose or type the robot IP"))
  4375 + robot_ip_options = [_("Select robot IP:")] + const.ROBOT_ElFIN_IP
  4376 + choice_IP = wx.ComboBox(self, -1, "",
  4377 + choices=robot_ip_options, style=wx.CB_DROPDOWN | wx.TE_PROCESS_ENTER)
  4378 + choice_IP.SetToolTip(tooltip)
  4379 + choice_IP.SetSelection(const.DEFAULT_TRACKER)
  4380 + choice_IP.Bind(wx.EVT_COMBOBOX, partial(self.OnChoiceIP, ctrl=choice_IP))
  4381 + choice_IP.Bind(wx.EVT_TEXT, partial(self.OnTxt_Ent, ctrl=choice_IP))
  4382 +
  4383 + btn_ok = wx.Button(self, wx.ID_OK)
  4384 + btn_ok.SetHelpText("")
  4385 + btn_ok.SetDefault()
  4386 +
  4387 + btn_cancel = wx.Button(self, wx.ID_CANCEL)
  4388 + btn_cancel.SetHelpText("")
  4389 +
  4390 + btnsizer = wx.StdDialogButtonSizer()
  4391 + btnsizer.AddButton(btn_ok)
  4392 + btnsizer.AddButton(btn_cancel)
  4393 + btnsizer.Realize()
  4394 +
  4395 + main_sizer = wx.BoxSizer(wx.VERTICAL)
  4396 +
  4397 + main_sizer.Add((5, 5))
  4398 + main_sizer.Add(choice_IP, 1, wx.EXPAND|wx.LEFT|wx.RIGHT, 5)
  4399 + main_sizer.Add((15, 15))
  4400 + main_sizer.Add(btnsizer, 0, wx.EXPAND)
  4401 + main_sizer.Add((5, 5))
  4402 +
  4403 + self.SetSizer(main_sizer)
  4404 + main_sizer.Fit(self)
  4405 +
  4406 + self.CenterOnParent()
  4407 +
  4408 + def OnTxt_Ent(self, evt, ctrl):
  4409 + self.robot_ip = str(ctrl.GetValue())
  4410 +
  4411 + def OnChoiceIP(self, evt, ctrl):
  4412 + self.robot_ip = ctrl.GetStringSelection()
  4413 +
  4414 + def GetValue(self):
  4415 + return self.robot_ip
  4416 +
  4417 +class CreateTransformationMatrixRobot(wx.Dialog):
  4418 + def __init__(self, tracker, title=_("Create transformation matrix to robot space")):
  4419 + wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, #size=wx.Size(1000, 200),
  4420 + style=wx.DEFAULT_DIALOG_STYLE|wx.FRAME_FLOAT_ON_PARENT|wx.STAY_ON_TOP|wx.RESIZE_BORDER)
  4421 + '''
  4422 + M_robot_2_tracker is created by an affine transformation. Robot TCP should be calibrated to the center of the tracker marker
  4423 + '''
  4424 + #TODO: make aboutbox
  4425 + self.tracker_coord = []
  4426 + self.tracker_angles = []
  4427 + self.robot_coord = []
  4428 + self.robot_angles = []
  4429 +
  4430 + self.tracker = tracker
  4431 +
  4432 + self.timer = wx.Timer(self)
  4433 + self.Bind(wx.EVT_TIMER, self.OnUpdate, self.timer)
  4434 +
  4435 + self._init_gui()
  4436 +
  4437 + def _init_gui(self):
  4438 + # Buttons to acquire and remove points
  4439 + txt_acquisition = wx.StaticText(self, -1, _('Poses acquisition for robot registration:'))
  4440 +
  4441 + btn_create_point = wx.Button(self, -1, label=_('Single'))
  4442 + btn_create_point.Bind(wx.EVT_BUTTON, self.OnCreatePoint)
  4443 +
  4444 + btn_cont_point = wx.ToggleButton(self, -1, label=_('Continuous'))
  4445 + btn_cont_point.Bind(wx.EVT_TOGGLEBUTTON, partial(self.OnContinuousAcquisition, btn=btn_cont_point))
  4446 + self.btn_cont_point = btn_cont_point
  4447 +
  4448 + txt_number = wx.StaticText(self, -1, _('0'))
  4449 + txt_recorded = wx.StaticText(self, -1, _('Poses recorded'))
  4450 + self.txt_number = txt_number
  4451 +
  4452 + btn_reset = wx.Button(self, -1, label=_('Reset'))
  4453 + btn_reset.Bind(wx.EVT_BUTTON, self.OnReset)
  4454 +
  4455 + btn_apply_reg = wx.Button(self, -1, label=_('Apply'))
  4456 + btn_apply_reg.Bind(wx.EVT_BUTTON, self.OnApply)
  4457 + btn_apply_reg.Enable(False)
  4458 + self.btn_apply_reg = btn_apply_reg
  4459 +
  4460 + # Buttons to save and load
  4461 + txt_file = wx.StaticText(self, -1, _('Registration file'))
  4462 +
  4463 + btn_save = wx.Button(self, -1, label=_('Save'), size=wx.Size(65, 23))
  4464 + btn_save.Bind(wx.EVT_BUTTON, self.OnSaveReg)
  4465 + btn_save.Enable(False)
  4466 + self.btn_save = btn_save
  4467 +
  4468 + btn_load = wx.Button(self, -1, label=_('Load'), size=wx.Size(65, 23))
  4469 + btn_load.Bind(wx.EVT_BUTTON, self.OnLoadReg)
  4470 +
  4471 + # Create a horizontal sizers
  4472 + border = 1
  4473 + acquisition = wx.BoxSizer(wx.HORIZONTAL)
  4474 + acquisition.AddMany([(btn_create_point, 1, wx.EXPAND | wx.GROW | wx.TOP | wx.RIGHT | wx.LEFT, border),
  4475 + (btn_cont_point, 1, wx.ALL | wx.EXPAND | wx.GROW, border)])
  4476 +
  4477 + txt_pose = wx.BoxSizer(wx.HORIZONTAL)
  4478 + txt_pose.AddMany([(txt_number, 1, wx.LEFT, 50),
  4479 + (txt_recorded, 1, wx.LEFT, border)])
  4480 +
  4481 + apply_reset = wx.BoxSizer(wx.HORIZONTAL)
  4482 + apply_reset.AddMany([(btn_reset, 1, wx.EXPAND | wx.GROW | wx.TOP | wx.RIGHT | wx.LEFT, border),
  4483 + (btn_apply_reg, 1, wx.ALL | wx.EXPAND | wx.GROW, border)])
  4484 +
  4485 + save_load = wx.BoxSizer(wx.HORIZONTAL)
  4486 + save_load.AddMany([(btn_save, 1, wx.EXPAND | wx.GROW | wx.TOP | wx.RIGHT | wx.LEFT, border),
  4487 + (btn_load, 1, wx.ALL | wx.EXPAND | wx.GROW, border)])
  4488 +
  4489 + btn_ok = wx.Button(self, wx.ID_OK)
  4490 + btn_ok.SetHelpText("")
  4491 + btn_ok.SetDefault()
  4492 + btn_ok.Enable(False)
  4493 + self.btn_ok = btn_ok
  4494 +
  4495 + btn_cancel = wx.Button(self, wx.ID_CANCEL)
  4496 + btn_cancel.SetHelpText("")
  4497 +
  4498 + btnsizer = wx.StdDialogButtonSizer()
  4499 + btnsizer.AddButton(btn_ok)
  4500 + btnsizer.AddButton(btn_cancel)
  4501 + btnsizer.Realize()
  4502 +
  4503 + # Add line sizers into main sizer
  4504 + border = 10
  4505 + border_last = 10
  4506 + main_sizer = wx.BoxSizer(wx.VERTICAL)
  4507 + main_sizer.Add(wx.StaticLine(self, -1), 0, wx.EXPAND | wx.TOP | wx.BOTTOM, border)
  4508 + main_sizer.Add(txt_acquisition, 0, wx.BOTTOM | wx.LEFT | wx.RIGHT | wx.ALIGN_CENTER_HORIZONTAL , border)
  4509 + main_sizer.Add(acquisition, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT | wx.BOTTOM, border)
  4510 + main_sizer.Add(txt_pose, 0, wx.ALIGN_CENTER_HORIZONTAL | wx.TOP | wx.BOTTOM, border)
  4511 + main_sizer.Add(apply_reset, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT , border_last)
  4512 + main_sizer.Add(wx.StaticLine(self, -1), 0, wx.EXPAND | wx.TOP | wx.BOTTOM, border)
  4513 + main_sizer.Add(txt_file, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT | wx.BOTTOM, border/2)
  4514 + main_sizer.Add(save_load, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT | wx.BOTTOM, border)
  4515 + main_sizer.Add(wx.StaticLine(self, -1), 0, wx.EXPAND | wx.TOP | wx.BOTTOM, border)
  4516 + main_sizer.Add(btnsizer, 0, wx.GROW | wx.EXPAND | wx.LEFT | wx.RIGHT | wx.BOTTOM, border)
  4517 + main_sizer.Fit(self)
  4518 +
  4519 + self.SetSizer(main_sizer)
  4520 + self.Update()
  4521 + main_sizer.Fit(self)
  4522 +
  4523 + self.CenterOnParent()
  4524 +
  4525 + def affine_correg(self, tracker, robot):
  4526 + m_change = tr.affine_matrix_from_points(robot[:].T, tracker[:].T,
  4527 + shear=False, scale=False, usesvd=False)
  4528 + return m_change
  4529 +
  4530 + def OnContinuousAcquisition(self, evt=None, btn=None):
  4531 + value = btn.GetValue()
  4532 + if value:
  4533 + self.timer.Start(500)
  4534 + else:
  4535 + self.timer.Stop()
  4536 +
  4537 + def OnUpdate(self, evt):
  4538 + self.OnCreatePoint(evt=None)
  4539 +
  4540 + def OnCreatePoint(self, evt):
  4541 + coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
  4542 + #robot thread is not initialized yet
  4543 + coord_raw_robot = self.tracker.trk_init[1][0].Run()
  4544 + coord_raw_tracker_obj = coord_raw[3]
  4545 +
  4546 + if markers_flag[2]:
  4547 + self.tracker_coord.append(coord_raw_tracker_obj[:3])
  4548 + self.tracker_angles.append(coord_raw_tracker_obj[3:])
  4549 + self.robot_coord.append(coord_raw_robot[:3])
  4550 + self.robot_angles.append(coord_raw_robot[3:])
  4551 + self.txt_number.SetLabel(str(int(self.txt_number.GetLabel())+1))
  4552 + else:
  4553 + print('Cannot detect the coil markers, pls try again')
  4554 +
  4555 + if len(self.tracker_coord) >= 3:
  4556 + self.btn_apply_reg.Enable(True)
  4557 +
  4558 + def OnReset(self, evt):
  4559 + if self.btn_cont_point:
  4560 + self.btn_cont_point.SetValue(False)
  4561 + self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point)
  4562 +
  4563 + self.tracker_coord = []
  4564 + self.tracker_angles = []
  4565 + self.robot_coord = []
  4566 + self.robot_angles = []
  4567 + self.M_tracker_2_robot = []
  4568 + self.txt_number.SetLabel('0')
  4569 +
  4570 + self.btn_apply_reg.Enable(False)
  4571 + self.btn_save.Enable(False)
  4572 + self.btn_ok.Enable(False)
  4573 +
  4574 + def OnApply(self, evt):
  4575 + if self.btn_cont_point:
  4576 + self.btn_cont_point.SetValue(False)
  4577 + self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point)
  4578 +
  4579 + tracker_coord = np.array(self.tracker_coord)
  4580 + robot_coord = np.array(self.robot_coord)
  4581 +
  4582 + M_robot_2_tracker = self.affine_correg(tracker_coord, robot_coord)
  4583 + M_tracker_2_robot = tr.inverse_matrix(M_robot_2_tracker)
  4584 + self.M_tracker_2_robot = M_tracker_2_robot
  4585 +
  4586 + self.btn_save.Enable(True)
  4587 + self.btn_ok.Enable(True)
  4588 +
  4589 + #TODO: make a colored circle to sinalize that the transformation was made (green) (red if not)
  4590 +
  4591 + def OnSaveReg(self, evt):
  4592 + filename = ShowLoadSaveDialog(message=_(u"Save robot transformation file as..."),
  4593 + wildcard=_("Robot transformation files (*.rbtf)|*.rbtf"),
  4594 + style=wx.FD_SAVE | wx.FD_OVERWRITE_PROMPT,
  4595 + default_filename="robottransform.rbtf", save_ext="rbtf")
  4596 +
  4597 + if filename:
  4598 + if self.M_tracker_2_robot is not None:
  4599 + with open(filename, 'w', newline='') as file:
  4600 + writer = csv.writer(file, delimiter='\t')
  4601 + writer.writerows(self.M_tracker_2_robot)
  4602 +
  4603 + def OnLoadReg(self, evt):
  4604 + filename = ShowLoadSaveDialog(message=_(u"Load robot transformation"),
  4605 + wildcard=_("Robot transformation files (*.rbtf)|*.rbtf"))
  4606 + if filename:
  4607 + with open(filename, 'r') as file:
  4608 + reader = csv.reader(file, delimiter='\t')
  4609 + content = [row for row in reader]
  4610 +
  4611 + self.M_tracker_2_robot = np.vstack(list(np.float_(content)))
  4612 + print("Matrix tracker to robot:", self.M_tracker_2_robot)
  4613 + self.btn_ok.Enable(True)
  4614 +
  4615 + def GetValue(self):
  4616 + return self.M_tracker_2_robot
  4617 +
4304 4618 class SetNDIconfigs(wx.Dialog):
4305 4619 def __init__(self, title=_("Setting NDI polaris configs:")):
4306 4620 wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200),
... ... @@ -4329,16 +4643,19 @@ class SetNDIconfigs(wx.Dialog):
4329 4643 return port_list, port_selec
4330 4644  
4331 4645 def _init_gui(self):
4332   - self.com_ports = wx.ComboBox(self, -1, style=wx.CB_DROPDOWN|wx.CB_READONLY)
  4646 + com_ports = wx.ComboBox(self, -1, style=wx.CB_DROPDOWN|wx.CB_READONLY)
  4647 + com_ports.Bind(wx.EVT_COMBOBOX, partial(self.OnChoicePort, ctrl=com_ports))
4333 4648 row_com = wx.BoxSizer(wx.VERTICAL)
4334 4649 row_com.Add(wx.StaticText(self, wx.ID_ANY, "Select the COM port"), 0, wx.TOP|wx.RIGHT,5)
4335   - row_com.Add(self.com_ports, 0, wx.EXPAND)
  4650 + row_com.Add(com_ports, 0, wx.EXPAND)
4336 4651  
4337 4652 port_list, port_selec = self.serial_ports()
4338 4653  
4339   - self.com_ports.Append(port_list)
  4654 + com_ports.Append(port_list)
4340 4655 if port_selec:
4341   - self.com_ports.SetSelection(port_selec[0])
  4656 + com_ports.SetSelection(port_selec[0])
  4657 +
  4658 + self.com_ports = com_ports
4342 4659  
4343 4660 session = ses.Session()
4344 4661 last_ndi_probe_marker = session.get('paths', 'last_ndi_probe_marker', '')
... ... @@ -4374,6 +4691,9 @@ class SetNDIconfigs(wx.Dialog):
4374 4691 btn_ok = wx.Button(self, wx.ID_OK)
4375 4692 btn_ok.SetHelpText("")
4376 4693 btn_ok.SetDefault()
  4694 + if not port_selec:
  4695 + btn_ok.Enable(False)
  4696 + self.btn_ok = btn_ok
4377 4697  
4378 4698 btn_cancel = wx.Button(self, wx.ID_CANCEL)
4379 4699 btn_cancel.SetHelpText("")
... ... @@ -4402,6 +4722,9 @@ class SetNDIconfigs(wx.Dialog):
4402 4722  
4403 4723 self.CenterOnParent()
4404 4724  
  4725 + def OnChoicePort(self, evt, ctrl):
  4726 + self.btn_ok.Enable(True)
  4727 +
4405 4728 def GetValue(self):
4406 4729 fn_probe = self.dir_probe.GetPath().encode(const.FS_ENCODE)
4407 4730 fn_ref = self.dir_ref.GetPath().encode(const.FS_ENCODE)
... ...
invesalius/gui/task_navigator.py
  Diff comments   View file @3185fc2
... ... @@ -20,7 +20,10 @@
20 20 import dataclasses
21 21 from functools import partial
22 22 import itertools
  23 +import csv
  24 +import queue
23 25 import time
  26 +import threading
24 27  
25 28 import nibabel as nb
26 29 import numpy as np
... ... @@ -29,6 +32,13 @@ try:
29 32 has_trekker = True
30 33 except ImportError:
31 34 has_trekker = False
  35 +try:
  36 + import invesalius.data.elfin as elfin
  37 + import invesalius.data.elfin_processing as elfin_process
  38 + has_robot = True
  39 +except ImportError:
  40 + has_robot = False
  41 +
32 42 import wx
33 43  
34 44 try:
... ... @@ -51,13 +61,17 @@ import invesalius.data.slice_ as sl
51 61 import invesalius.data.tractography as dti
52 62 import invesalius.data.record_coords as rec
53 63 import invesalius.data.vtk_utils as vtk_utils
  64 +import invesalius.data.bases as db
54 65 import invesalius.gui.dialogs as dlg
55 66 import invesalius.project as prj
  67 +import invesalius.session as ses
  68 +
56 69 from invesalius import utils
57 70 from invesalius.gui import utils as gui_utils
58 71 from invesalius.navigation.icp import ICP
59 72 from invesalius.navigation.navigation import Navigation
60 73 from invesalius.navigation.tracker import Tracker
  74 +from invesalius.navigation.robot import Robot
61 75  
62 76 HAS_PEDAL_CONNECTION = True
63 77 try:
... ... @@ -183,7 +197,7 @@ class InnerFoldPanel(wx.Panel):
183 197  
184 198 # Fold 3 - Markers panel
185 199 item = fold_panel.AddFoldPanel(_("Markers"), collapsed=True)
186   - mtw = MarkersPanel(item)
  200 + mtw = MarkersPanel(item, tracker)
187 201  
188 202 fold_panel.ApplyCaptionStyle(item, style)
189 203 fold_panel.AddFoldPanelWindow(item, mtw, spacing= 0,
... ... @@ -341,6 +355,7 @@ class NeuronavigationPanel(wx.Panel):
341 355 )
342 356 self.icp = ICP()
343 357 self.tracker = tracker
  358 + self.robot = Robot(tracker)
344 359  
345 360 self.nav_status = False
346 361 self.tracker_fiducial_being_set = None
... ... @@ -640,6 +655,8 @@ class NeuronavigationPanel(wx.Panel):
640 655 self.checkbox_icp.SetValue(False)
641 656  
642 657 def OnDisconnectTracker(self):
  658 + if self.tracker.tracker_id == const.ROBOT:
  659 + self.robot.StopRobotThreadNavigation()
643 660 self.tracker.DisconnectTracker()
644 661 self.ResetICP()
645 662 self.tracker.UpdateUI(self.select_tracker_elem, self.numctrls_fiducial[3:6], self.txtctrl_fre)
... ... @@ -657,6 +674,9 @@ class NeuronavigationPanel(wx.Panel):
657 674 choice = None
658 675  
659 676 self.tracker.SetTracker(choice)
  677 + if self.tracker.tracker_id == const.ROBOT:
  678 + self.tracker.ConnectToRobot(self.navigation, self.tracker, self.robot)
  679 +
660 680 self.ResetICP()
661 681 self.tracker.UpdateUI(ctrl, self.numctrls_fiducial[3:6], self.txtctrl_fre)
662 682  
... ... @@ -739,6 +759,9 @@ class NeuronavigationPanel(wx.Panel):
739 759 choice_ref = self.choice_ref
740 760  
741 761 self.navigation.StopNavigation()
  762 + if self.tracker.tracker_id == const.ROBOT:
  763 + Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
  764 + m_change_robot_to_head=None)
742 765  
743 766 # Enable all navigation buttons
744 767 choice_ref.Enable(True)
... ... @@ -841,6 +864,7 @@ class NeuronavigationPanel(wx.Panel):
841 864 )
842 865 self.tracker.__init__()
843 866 self.icp.__init__()
  867 + self.robot.__init__(self.tracker)
844 868  
845 869  
846 870 class ObjectRegistrationPanel(wx.Panel):
... ... @@ -899,16 +923,16 @@ class ObjectRegistrationPanel(wx.Panel):
899 923  
900 924 # Change angles threshold
901 925 text_angles = wx.StaticText(self, -1, _("Angle threshold [degrees]:"))
902   - spin_size_angles = wx.SpinCtrl(self, -1, "", size=wx.Size(50, 23))
903   - spin_size_angles.SetRange(1, 99)
  926 + spin_size_angles = wx.SpinCtrlDouble(self, -1, "", size=wx.Size(50, 23))
  927 + spin_size_angles.SetRange(0.1, 99)
904 928 spin_size_angles.SetValue(const.COIL_ANGLES_THRESHOLD)
905 929 spin_size_angles.Bind(wx.EVT_TEXT, partial(self.OnSelectAngleThreshold, ctrl=spin_size_angles))
906 930 spin_size_angles.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectAngleThreshold, ctrl=spin_size_angles))
907 931  
908 932 # Change dist threshold
909 933 text_dist = wx.StaticText(self, -1, _("Distance threshold [mm]:"))
910   - spin_size_dist = wx.SpinCtrl(self, -1, "", size=wx.Size(50, 23))
911   - spin_size_dist.SetRange(1, 99)
  934 + spin_size_dist = wx.SpinCtrlDouble(self, -1, "", size=wx.Size(50, 23))
  935 + spin_size_dist.SetRange(0.1, 99)
912 936 spin_size_dist.SetValue(const.COIL_ANGLES_THRESHOLD)
913 937 spin_size_dist.Bind(wx.EVT_TEXT, partial(self.OnSelectDistThreshold, ctrl=spin_size_dist))
914 938 spin_size_dist.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectDistThreshold, ctrl=spin_size_dist))
... ... @@ -1200,7 +1224,20 @@ class MarkersPanel(wx.Panel):
1200 1224 if field.type is bool:
1201 1225 setattr(self, field.name, str_val=='True')
1202 1226  
1203   - def __init__(self, parent):
  1227 + @dataclasses.dataclass
  1228 + class Robot_Marker:
  1229 + """Class for storing robot target."""
  1230 + m_robot_target : list = None
  1231 +
  1232 + @property
  1233 + def robot_target_matrix(self):
  1234 + return self.m_robot_target
  1235 +
  1236 + @robot_target_matrix.setter
  1237 + def robot_target_matrix(self, new_m_robot_target):
  1238 + self.m_robot_target = new_m_robot_target
  1239 +
  1240 + def __init__(self, parent, tracker):
1204 1241 wx.Panel.__init__(self, parent)
1205 1242 try:
1206 1243 default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
... ... @@ -1210,13 +1247,20 @@ class MarkersPanel(wx.Panel):
1210 1247  
1211 1248 self.SetAutoLayout(1)
1212 1249  
  1250 + self.tracker = tracker
  1251 +
1213 1252 self.__bind_events()
1214 1253  
  1254 + self.session = ses.Session()
  1255 +
1215 1256 self.current_coord = 0, 0, 0, 0, 0, 0
1216 1257 self.current_angle = 0, 0, 0
1217 1258 self.current_seed = 0, 0, 0
  1259 + self.current_robot_target_matrix = [None] * 9
1218 1260 self.markers = []
  1261 + self.robot_markers = []
1219 1262 self.nav_status = False
  1263 + self.raw_target_robot = None, None
1220 1264  
1221 1265 self.marker_colour = const.MARKER_COLOUR
1222 1266 self.marker_size = const.MARKER_SIZE
... ... @@ -1269,21 +1313,27 @@ class MarkersPanel(wx.Panel):
1269 1313  
1270 1314 # List of markers
1271 1315 self.lc = wx.ListCtrl(self, -1, style=wx.LC_REPORT, size=wx.Size(0,120))
1272   - self.lc.InsertColumn(0, '#')
1273   - self.lc.InsertColumn(1, 'X')
1274   - self.lc.InsertColumn(2, 'Y')
1275   - self.lc.InsertColumn(3, 'Z')
1276   - self.lc.InsertColumn(4, 'Label')
1277   - self.lc.InsertColumn(5, 'Target')
1278   - self.lc.InsertColumn(6, 'Session')
1279   -
1280   - self.lc.SetColumnWidth(0, 28)
1281   - self.lc.SetColumnWidth(1, 50)
1282   - self.lc.SetColumnWidth(2, 50)
1283   - self.lc.SetColumnWidth(3, 50)
1284   - self.lc.SetColumnWidth(4, 60)
1285   - self.lc.SetColumnWidth(5, 60)
1286   - self.lc.SetColumnWidth(5, 50)
  1316 + self.lc.InsertColumn(const.ID_COLUMN, '#')
  1317 + self.lc.SetColumnWidth(const.ID_COLUMN, 28)
  1318 +
  1319 + self.lc.InsertColumn(const.SESSION_COLUMN, 'Session')
  1320 + self.lc.SetColumnWidth(const.SESSION_COLUMN, 52)
  1321 +
  1322 + self.lc.InsertColumn(const.LABEL_COLUMN, 'Label')
  1323 + self.lc.SetColumnWidth(const.LABEL_COLUMN, 118)
  1324 +
  1325 + self.lc.InsertColumn(const.TARGET_COLUMN, 'Target')
  1326 + self.lc.SetColumnWidth(const.TARGET_COLUMN, 45)
  1327 +
  1328 + if self.session.debug:
  1329 + self.lc.InsertColumn(const.X_COLUMN, 'X')
  1330 + self.lc.SetColumnWidth(const.X_COLUMN, 45)
  1331 +
  1332 + self.lc.InsertColumn(const.Y_COLUMN, 'Y')
  1333 + self.lc.SetColumnWidth(const.Y_COLUMN, 45)
  1334 +
  1335 + self.lc.InsertColumn(const.Z_COLUMN, 'Z')
  1336 + self.lc.SetColumnWidth(const.Z_COLUMN, 45)
1287 1337  
1288 1338 self.lc.Bind(wx.EVT_LIST_ITEM_RIGHT_CLICK, self.OnMouseRightDown)
1289 1339 self.lc.Bind(wx.EVT_LIST_ITEM_ACTIVATED, self.OnItemBlink)
... ... @@ -1301,7 +1351,6 @@ class MarkersPanel(wx.Panel):
1301 1351 self.Update()
1302 1352  
1303 1353 def __bind_events(self):
1304   - # Publisher.subscribe(self.UpdateCurrentCoord, 'Co-registered points')
1305 1354 Publisher.subscribe(self.UpdateCurrentCoord, 'Set cross focal point')
1306 1355 Publisher.subscribe(self.OnDeleteMultipleMarkers, 'Delete fiducial marker')
1307 1356 Publisher.subscribe(self.OnDeleteAllMarkers, 'Delete all markers')
... ... @@ -1309,15 +1358,18 @@ class MarkersPanel(wx.Panel):
1309 1358 Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status')
1310 1359 Publisher.subscribe(self.UpdateSeedCoordinates, 'Update tracts')
1311 1360 Publisher.subscribe(self.OnChangeCurrentSession, 'Current session changed')
  1361 + Publisher.subscribe(self.UpdateRobotCoordinates, 'Update raw coordinates')
1312 1362  
1313 1363 def __find_target_marker(self):
1314   - """Return the index of the marker currently selected as target (there
1315   - should be at most one). If there is no such marker, return -1."""
  1364 + """
  1365 + Return the index of the marker currently selected as target (there
  1366 + should be at most one). If there is no such marker, return None.
  1367 + """
1316 1368 for i in range(len(self.markers)):
1317 1369 if self.markers[i].is_target:
1318 1370 return i
1319 1371  
1320   - return -1
  1372 + return None
1321 1373  
1322 1374 def __get_selected_items(self):
1323 1375 """
... ... @@ -1334,18 +1386,22 @@ class MarkersPanel(wx.Panel):
1334 1386 return selection
1335 1387  
1336 1388 def __delete_multiple_markers(self, index):
1337   - """ Delete multiple markers indexed by index. index must be sorted in
  1389 + """
  1390 + Delete multiple markers indexed by index. index must be sorted in
1338 1391 the ascending order.
1339 1392 """
1340 1393 for i in reversed(index):
1341 1394 del self.markers[i]
  1395 + del self.robot_markers[i]
1342 1396 self.lc.DeleteItem(i)
1343 1397 for n in range(0, self.lc.GetItemCount()):
1344   - self.lc.SetItem(n, 0, str(n+1))
  1398 + self.lc.SetItem(n, 0, str(n + 1))
1345 1399 Publisher.sendMessage('Remove multiple markers', index=index)
1346 1400  
1347 1401 def __set_marker_as_target(self, idx):
1348   - """Set marker indexed by idx as the new target. idx must be a valid index."""
  1402 + """
  1403 + Set marker indexed by idx as the new target. idx must be a valid index.
  1404 + """
1349 1405 # Find the previous target
1350 1406 prev_idx = self.__find_target_marker()
1351 1407  
... ... @@ -1354,16 +1410,16 @@ class MarkersPanel(wx.Panel):
1354 1410 return
1355 1411  
1356 1412 # Unset the previous target
1357   - if prev_idx != -1:
  1413 + if prev_idx is not None:
1358 1414 self.markers[prev_idx].is_target = False
1359 1415 self.lc.SetItemBackgroundColour(prev_idx, 'white')
1360 1416 Publisher.sendMessage('Set target transparency', status=False, index=prev_idx)
1361   - self.lc.SetItem(prev_idx, 5, "")
  1417 + self.lc.SetItem(prev_idx, const.TARGET_COLUMN, "")
1362 1418  
1363 1419 # Set the new target
1364 1420 self.markers[idx].is_target = True
1365 1421 self.lc.SetItemBackgroundColour(idx, 'RED')
1366   - self.lc.SetItem(idx, 5, _("Yes"))
  1422 + self.lc.SetItem(idx, const.TARGET_COLUMN, _("Yes"))
1367 1423  
1368 1424 Publisher.sendMessage('Update target', coord=self.markers[idx].coord)
1369 1425 Publisher.sendMessage('Set target transparency', status=True, index=idx)
... ... @@ -1389,6 +1445,9 @@ class MarkersPanel(wx.Panel):
1389 1445 def UpdateSeedCoordinates(self, root=None, affine_vtk=None, coord_offset=(0, 0, 0)):
1390 1446 self.current_seed = coord_offset
1391 1447  
  1448 + def UpdateRobotCoordinates(self, coordinates_raw, markers_flag):
  1449 + self.raw_target_robot = coordinates_raw[1], coordinates_raw[2]
  1450 +
1392 1451 def OnMouseRightDown(self, evt):
1393 1452 # TODO: Enable the "Set as target" only when target is created with registered object
1394 1453 menu_id = wx.Menu()
... ... @@ -1399,6 +1458,16 @@ class MarkersPanel(wx.Panel):
1399 1458 menu_id.AppendSeparator()
1400 1459 target_menu = menu_id.Append(1, _('Set as target'))
1401 1460 menu_id.Bind(wx.EVT_MENU, self.OnMenuSetTarget, target_menu)
  1461 + menu_id.AppendSeparator()
  1462 + send_target_to_robot = menu_id.Append(3, _('Send target to robot'))
  1463 + menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot)
  1464 +
  1465 + # Enable "Send target to robot" button only if tracker is robot, if navigation is on and if target is not none
  1466 + m_target_robot = np.array([self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix])
  1467 + if self.tracker.tracker_id == const.ROBOT and self.nav_status and m_target_robot.any():
  1468 + send_target_to_robot.Enable(True)
  1469 + else:
  1470 + send_target_to_robot.Enable(False)
1402 1471 # TODO: Create the remove target option so the user can disable the target without removing the marker
1403 1472 # target_menu_rem = menu_id.Append(3, _('Remove target'))
1404 1473 # menu_id.Bind(wx.EVT_MENU, self.OnMenuRemoveTarget, target_menu_rem)
... ... @@ -1416,9 +1485,9 @@ class MarkersPanel(wx.Panel):
1416 1485 def OnMenuEditMarkerLabel(self, evt):
1417 1486 list_index = self.lc.GetFocusedItem()
1418 1487 if list_index != -1:
1419   - new_label = dlg.ShowEnterMarkerID(self.lc.GetItemText(list_index, 4))
  1488 + new_label = dlg.ShowEnterMarkerID(self.lc.GetItemText(list_index, const.LABEL_COLUMN))
1420 1489 self.markers[list_index].label = str(new_label)
1421   - self.lc.SetItem(list_index, 4, new_label)
  1490 + self.lc.SetItem(list_index, const.LABEL_COLUMN, new_label)
1422 1491 else:
1423 1492 wx.MessageBox(_("No data selected."), _("InVesalius 3"))
1424 1493  
... ... @@ -1445,25 +1514,32 @@ class MarkersPanel(wx.Panel):
1445 1514 # XXX: Seems like a slightly too early point for rounding; better to round only when the value
1446 1515 # is printed to the screen or file.
1447 1516 #
1448   - self.markers[index].colour = [round(s/255.0, 3) for s in color_new]
  1517 + self.markers[index].colour = [round(s / 255.0, 3) for s in color_new]
1449 1518  
1450 1519 Publisher.sendMessage('Set new color', index=index, color=color_new)
1451 1520  
  1521 + def OnMenuSendTargetToRobot(self, evt):
  1522 + if isinstance(evt, int):
  1523 + self.lc.Focus(evt)
  1524 +
  1525 + m_target_robot = self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix
  1526 +
  1527 + Publisher.sendMessage('Reset robot process')
  1528 + Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot_to_head=m_target_robot)
  1529 +
1452 1530 def OnDeleteAllMarkers(self, evt=None):
1453   - if evt is None:
1454   - result = wx.ID_OK
1455   - else:
  1531 + if evt is not None:
1456 1532 result = dlg.ShowConfirmationDialog(msg=_("Remove all markers? Cannot be undone."))
  1533 + if result != wx.ID_OK:
  1534 + return
1457 1535  
1458   - if result != wx.ID_OK:
1459   - return
1460   -
1461   - if self.__find_target_marker() != -1:
  1536 + if self.__find_target_marker() is not None:
1462 1537 Publisher.sendMessage('Disable or enable coil tracker', status=False)
1463 1538 if evt is not None:
1464 1539 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
1465 1540  
1466 1541 self.markers = []
  1542 + self.robot_markers = []
1467 1543 Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount())
1468 1544 self.lc.DeleteAllItems()
1469 1545 Publisher.sendMessage('Stop Blink Marker', index='DeleteAll')
... ... @@ -1472,22 +1548,27 @@ class MarkersPanel(wx.Panel):
1472 1548 # OnDeleteMultipleMarkers is used for both pubsub and button click events
1473 1549 # Pubsub is used for fiducial handle and button click for all others
1474 1550  
1475   - if not evt: # called through pubsub
  1551 + # called through pubsub
  1552 + if not evt:
1476 1553 index = []
1477 1554  
1478 1555 if label and (label in self.__list_fiducial_labels()):
1479 1556 for id_n in range(self.lc.GetItemCount()):
1480   - item = self.lc.GetItem(id_n, 4)
  1557 + item = self.lc.GetItem(id_n, const.LABEL_COLUMN)
1481 1558 if item.GetText() == label:
1482 1559 self.lc.Focus(item.GetId())
1483 1560 index = [self.lc.GetFocusedItem()]
1484 1561  
1485   - else: # called from button click
  1562 + # called from button click
  1563 + else:
1486 1564 index = self.__get_selected_items()
1487 1565  
1488 1566 if index:
1489 1567 if self.__find_target_marker() in index:
1490 1568 Publisher.sendMessage('Disable or enable coil tracker', status=False)
  1569 + if self.tracker.tracker_id == const.ROBOT:
  1570 + Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
  1571 + m_change_robot_to_head=[])
1491 1572 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
1492 1573  
1493 1574 self.__delete_multiple_markers(index)
... ... @@ -1532,13 +1613,12 @@ class MarkersPanel(wx.Panel):
1532 1613 # If the new marker has is_target=True, we first create
1533 1614 # a marker with is_target=False, and then call __set_marker_as_target
1534 1615 if marker.is_target:
1535   - self.__set_marker_as_target(len(self.markers)-1)
  1616 + self.__set_marker_as_target(len(self.markers) - 1)
1536 1617  
1537 1618 except:
1538 1619 wx.MessageBox(_("Invalid markers file."), _("InVesalius 3"))
1539 1620  
1540 1621 def OnMarkersVisibility(self, evt, ctrl):
1541   -
1542 1622 if ctrl.GetValue():
1543 1623 Publisher.sendMessage('Hide all markers', indexes=self.lc.GetItemCount())
1544 1624 ctrl.SetLabel('Show')
... ... @@ -1573,8 +1653,8 @@ class MarkersPanel(wx.Panel):
1573 1653 wx.MessageBox(_("Error writing markers file."), _("InVesalius 3"))
1574 1654  
1575 1655 def OnSelectColour(self, evt, ctrl):
1576   - #TODO: Make sure GetValue returns 3 numbers (without alpha)
1577   - self.marker_colour = [colour/255.0 for colour in ctrl.GetValue()][:3]
  1656 + # TODO: Make sure GetValue returns 3 numbers (without alpha)
  1657 + self.marker_colour = [colour / 255.0 for colour in ctrl.GetValue()][:3]
1578 1658  
1579 1659 def OnSelectSize(self, evt, ctrl):
1580 1660 self.marker_size = ctrl.GetValue()
... ... @@ -1592,6 +1672,14 @@ class MarkersPanel(wx.Panel):
1592 1672 new_marker.seed = seed or self.current_seed
1593 1673 new_marker.session_id = session_id or self.current_session
1594 1674  
  1675 + if self.tracker.tracker_id == const.ROBOT and self.nav_status:
  1676 + self.current_robot_target_matrix = db.compute_robot_to_head_matrix(self.raw_target_robot)
  1677 + else:
  1678 + self.current_robot_target_matrix = [None] * 9
  1679 +
  1680 + new_robot_marker = self.Robot_Marker()
  1681 + new_robot_marker.robot_target_matrix = self.current_robot_target_matrix
  1682 +
1595 1683 # Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added
1596 1684 Publisher.sendMessage('Add marker', ball_id=len(self.markers),
1597 1685 size=new_marker.size,
... ... @@ -1603,15 +1691,19 @@ class MarkersPanel(wx.Panel):
1603 1691 # coord=new_marker.coord[:3])
1604 1692  
1605 1693 self.markers.append(new_marker)
  1694 + self.robot_markers.append(new_robot_marker)
1606 1695  
1607 1696 # Add item to list control in panel
1608 1697 num_items = self.lc.GetItemCount()
1609 1698 self.lc.InsertItem(num_items, str(num_items + 1))
1610   - self.lc.SetItem(num_items, 1, str(round(new_marker.x, 2)))
1611   - self.lc.SetItem(num_items, 2, str(round(new_marker.y, 2)))
1612   - self.lc.SetItem(num_items, 3, str(round(new_marker.z, 2)))
1613   - self.lc.SetItem(num_items, 4, new_marker.label)
1614   - self.lc.SetItem(num_items, 6, str(new_marker.session_id))
  1699 + self.lc.SetItem(num_items, const.SESSION_COLUMN, str(new_marker.session_id))
  1700 + self.lc.SetItem(num_items, const.LABEL_COLUMN, new_marker.label)
  1701 +
  1702 + if self.session.debug:
  1703 + self.lc.SetItem(num_items, const.X_COLUMN, str(round(new_marker.x, 1)))
  1704 + self.lc.SetItem(num_items, const.Y_COLUMN, str(round(new_marker.y, 1)))
  1705 + self.lc.SetItem(num_items, const.Z_COLUMN, str(round(new_marker.z, 1)))
  1706 +
1615 1707 self.lc.EnsureVisible(num_items)
1616 1708  
1617 1709 class DbsPanel(wx.Panel):
... ...
invesalius/i18n.py
  Diff comments   View file @3185fc2
... ... @@ -21,7 +21,7 @@
21 21  
22 22 try:
23 23 import configparser as ConfigParser
24   -except(ImportError):
  24 +except ImportError:
25 25 import ConfigParser
26 26  
27 27 import locale
... ... @@ -66,22 +66,21 @@ def GetLocaleOS():
66 66 def InstallLanguage(language):
67 67 file_path = os.path.split(__file__)[0]
68 68  
69   - if hasattr(sys,"frozen") and (sys.frozen == "windows_exe"\
70   - or sys.frozen == "console_exe"):
  69 + abs_file_path = os.path.abspath(file_path + os.sep + "..")
71 70  
72   - abs_file_path = os.path.abspath(file_path + os.sep + ".." + os.sep + ".." + os.sep + "..")
73   - language_dir = os.path.join(abs_file_path, 'locale')
74   - else:
75   - abs_file_path = os.path.abspath(file_path + os.sep + "..")
76   - language_dir = os.path.join(abs_file_path, 'locale')
  71 + if hasattr(sys, "frozen") and (sys.frozen == "windows_exe" or sys.frozen == "console_exe"):
  72 + abs_file_path = os.path.abspath(abs_file_path + os.sep + ".." + os.sep + "..")
77 73  
78   - # MAC App
  74 + language_dir = os.path.join(abs_file_path, 'locale')
  75 +
  76 + # MAC app
79 77 if not os.path.exists(language_dir):
80 78 abs_file_path = os.path.abspath(os.path.join(file_path, '..', '..', '..', '..'))
81 79 language_dir = os.path.join(abs_file_path, 'locale')
82 80  
83   - lang = gettext.translation('invesalius', language_dir,\
84   - languages=[language], codeset='utf8')
  81 + lang = gettext.translation('invesalius', language_dir,
  82 + languages=[language], codeset='utf8')
  83 +
85 84 # Using unicode
86 85 try:
87 86 lang.install(unicode=1)
... ...
invesalius/navigation/navigation.py
  Diff comments   View file @3185fc2
... ... @@ -82,7 +82,7 @@ class UpdateNavigationScene(threading.Thread):
82 82  
83 83 threading.Thread.__init__(self, name='UpdateScene')
84 84 self.serial_port_enabled, self.view_tracts, self.peel_loaded = vis_components
85   - self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue = vis_queues
  85 + self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue = vis_queues
86 86 self.sle = sle
87 87 self.event = event
88 88  
... ... @@ -91,7 +91,7 @@ class UpdateNavigationScene(threading.Thread):
91 91 while not self.event.is_set():
92 92 got_coords = False
93 93 try:
94   - coord, m_img, view_obj = self.coord_queue.get_nowait()
  94 + coord, [coordinates_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait()
95 95 got_coords = True
96 96  
97 97 # print('UpdateScene: get {}'.format(count))
... ... @@ -116,6 +116,7 @@ class UpdateNavigationScene(threading.Thread):
116 116 # see the red cross in the position of the offset marker
117 117 wx.CallAfter(Publisher.sendMessage, 'Update slices position', position=coord[:3])
118 118 wx.CallAfter(Publisher.sendMessage, 'Set cross focal point', position=coord)
  119 + wx.CallAfter(Publisher.sendMessage, 'Update raw coordinates', coordinates_raw=coordinates_raw, markers_flag=markers_flag)
119 120 wx.CallAfter(Publisher.sendMessage, 'Update slice viewer')
120 121  
121 122 if view_obj:
... ... @@ -146,6 +147,8 @@ class Navigation():
146 147 self.event = threading.Event()
147 148 self.coord_queue = QueueCustom(maxsize=1)
148 149 self.icp_queue = QueueCustom(maxsize=1)
  150 + self.object_at_target_queue = QueueCustom(maxsize=1)
  151 + self.robot_target_queue = QueueCustom(maxsize=1)
149 152 # self.visualization_queue = QueueCustom(maxsize=1)
150 153 self.serial_port_queue = QueueCustom(maxsize=1)
151 154 self.coord_tracts_queue = QueueCustom(maxsize=1)
... ... @@ -243,7 +246,7 @@ class Navigation():
243 246 self.event.clear()
244 247  
245 248 vis_components = [self.serial_port_in_use, self.view_tracts, self.peel_loaded]
246   - vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue]
  249 + vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue]
247 250  
248 251 Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components)
249 252  
... ... @@ -277,7 +280,7 @@ class Navigation():
277 280 obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
278 281 coreg_data.extend(obj_data)
279 282  
280   - queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
  283 + queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.object_at_target_queue]
281 284 jobs_list.append(dcr.CoordinateCorregistrate(self.ref_mode_id, tracker, coreg_data,
282 285 self.view_tracts, queues,
283 286 self.event, self.sleep_nav, tracker.tracker_id,
... ...
invesalius/navigation/robot.py 0 → 100644
  Diff comments   View file @3185fc2
... ... @@ -0,0 +1,286 @@
  1 +#--------------------------------------------------------------------------
  2 +# Software: InVesalius - Software de Reconstrucao 3D de Imagens Medicas
  3 +# Copyright: (C) 2001 Centro de Pesquisas Renato Archer
  4 +# Homepage: http://www.softwarepublico.gov.br
  5 +# Contact: invesalius@cti.gov.br
  6 +# License: GNU - GPL 2 (LICENSE.txt/LICENCA.txt)
  7 +#--------------------------------------------------------------------------
  8 +# Este programa e software livre; voce pode redistribui-lo e/ou
  9 +# modifica-lo sob os termos da Licenca Publica Geral GNU, conforme
  10 +# publicada pela Free Software Foundation; de acordo com a versao 2
  11 +# da Licenca.
  12 +#
  13 +# Este programa eh distribuido na expectativa de ser util, mas SEM
  14 +# QUALQUER GARANTIA; sem mesmo a garantia implicita de
  15 +# COMERCIALIZACAO ou de ADEQUACAO A QUALQUER PROPOSITO EM
  16 +# PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais
  17 +# detalhes.
  18 +#--------------------------------------------------------------------------
  19 +import numpy as np
  20 +import wx
  21 +import queue
  22 +import threading
  23 +
  24 +import invesalius.data.bases as db
  25 +import invesalius.gui.dialogs as dlg
  26 +import invesalius.constants as const
  27 +from invesalius.pubsub import pub as Publisher
  28 +
  29 +try:
  30 + import invesalius.data.elfin as elfin
  31 + import invesalius.data.elfin_processing as elfin_process
  32 + has_robot = True
  33 +except ImportError:
  34 + has_robot = False
  35 +
  36 +class Robot():
  37 + def __init__(self, tracker):
  38 + """
  39 + Class to establish the connection between the robot and InVesalius
  40 + :param tracker: tracker.py instance
  41 + """
  42 + self.tracker = tracker
  43 + self.trk_init = None
  44 + self.robot_target_queue = None
  45 + self.object_at_target_queue = None
  46 + self.process_tracker = None
  47 +
  48 + self.thread_robot = None
  49 +
  50 + self.robotcoordinates = RobotCoordinates()
  51 +
  52 + self.__bind_events()
  53 +
  54 + def __bind_events(self):
  55 + Publisher.subscribe(self.OnSendCoordinates, 'Send coord to robot')
  56 + Publisher.subscribe(self.OnUpdateRobotTargetMatrix, 'Robot target matrix')
  57 + Publisher.subscribe(self.OnObjectTarget, 'Coil at target')
  58 + Publisher.subscribe(self.OnResetProcessTracker, 'Reset robot process')
  59 +
  60 + def OnRobotConnection(self):
  61 + if not self.tracker.trk_init[0][0] or not self.tracker.trk_init[1][0]:
  62 + dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, self.tracker.trk_init[1])
  63 + self.tracker.tracker_id = 0
  64 + self.tracker.tracker_connected = False
  65 + else:
  66 + self.tracker.trk_init.append(self.robotcoordinates)
  67 + self.process_tracker = elfin_process.TrackerProcessing()
  68 + dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker)
  69 + if dlg_correg_robot.ShowModal() == wx.ID_OK:
  70 + M_tracker_to_robot = dlg_correg_robot.GetValue()
  71 + db.transform_tracker_to_robot.M_tracker_to_robot = M_tracker_to_robot
  72 + self.robot_server = self.tracker.trk_init[1][0]
  73 + self.trk_init = self.tracker.trk_init
  74 + else:
  75 + dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, 'disconnect')
  76 + self.tracker.trk_init = None
  77 + self.tracker.tracker_id = 0
  78 + self.tracker.tracker_connected = False
  79 +
  80 + def StartRobotThreadNavigation(self, tracker, coord_queue):
  81 + if tracker.event_robot.is_set():
  82 + tracker.event_robot.clear()
  83 + self.thread_robot = ControlRobot(self.trk_init, tracker, self.robotcoordinates,
  84 + [coord_queue, self.robot_target_queue, self.object_at_target_queue],
  85 + self.process_tracker, tracker.event_robot)
  86 + self.thread_robot.start()
  87 +
  88 + def StopRobotThreadNavigation(self):
  89 + self.thread_robot.join()
  90 + self.OnResetProcessTracker()
  91 +
  92 + def OnResetProcessTracker(self):
  93 + self.process_tracker.__init__()
  94 +
  95 + def OnSendCoordinates(self, coord):
  96 + self.robot_server.SendCoordinates(coord)
  97 +
  98 + def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot_to_head):
  99 + try:
  100 + self.robot_target_queue.put_nowait([robot_tracker_flag, m_change_robot_to_head])
  101 + except queue.Full:
  102 + pass
  103 +
  104 + def OnObjectTarget(self, state):
  105 + try:
  106 + if self.object_at_target_queue:
  107 + self.object_at_target_queue.put_nowait(state)
  108 + except queue.Full:
  109 + pass
  110 +
  111 + def SetRobotQueues(self, queues):
  112 + self.robot_target_queue, self.object_at_target_queue = queues
  113 +
  114 +
  115 +class RobotCoordinates():
  116 + """
  117 + Class to set/get robot coordinates. Robot coordinates are acquired in ControlRobot thread.
  118 + The class is required to avoid acquisition conflict with different threads (coordinates and navigation)
  119 + """
  120 + def __init__(self):
  121 + self.coord = None
  122 +
  123 + def SetRobotCoordinates(self, coord):
  124 + self.coord = coord
  125 +
  126 + def GetRobotCoordinates(self):
  127 + return self.coord
  128 +
  129 +
  130 +class ControlRobot(threading.Thread):
  131 + def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event_robot):
  132 + """Class (threading) to perform the robot control.
  133 + A distinguish thread is required to increase perform and separate robot control from navigation thread
  134 + (safetly layer for robot positining).
  135 +
  136 + There is no GUI involved, them no Sleep is required
  137 +
  138 + :param trck_init: Initialization variable of tracking device and connection type. See tracker.py.
  139 + :param tracker: tracker.py instance
  140 + :param robotcoordinates: RobotCoordinates() instance
  141 + :param queues: Queue instance that manage coordinates and robot target
  142 + :param process_tracker: TrackerProcessing() instance from elfin_process
  143 + :param event_robot: Threading event to ControlRobot when tasks is done
  144 + """
  145 + threading.Thread.__init__(self, name='ControlRobot')
  146 +
  147 + self.trck_init_robot = trck_init[1][0]
  148 + self.trck_init_tracker = trck_init[0]
  149 + self.trk_id = trck_init[2]
  150 + self.tracker = tracker
  151 + self.robotcoordinates = robotcoordinates
  152 + self.robot_tracker_flag = False
  153 + self.objattarget_flag = False
  154 + self.target_flag = False
  155 + self.m_change_robot_to_head = None
  156 + self.coord_inv_old = None
  157 + self.coord_queue = queues[0]
  158 + self.robot_target_queue = queues[1]
  159 + self.object_at_target_queue = queues[2]
  160 + self.process_tracker = process_tracker
  161 + self.event_robot = event_robot
  162 + self.arc_motion_flag = False
  163 + self.arc_motion_step_flag = None
  164 + self.target_linear_out = None
  165 + self.target_linear_in = None
  166 + self.target_arc = None
  167 +
  168 + def get_coordinates_from_tracker_devices(self):
  169 + coord_robot_raw = self.trck_init_robot.Run()
  170 + coord_robot = np.array(coord_robot_raw)
  171 + coord_robot[3], coord_robot[5] = coord_robot[5], coord_robot[3]
  172 + self.robotcoordinates.SetRobotCoordinates(coord_robot)
  173 +
  174 + coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
  175 +
  176 + return coord_raw, coord_robot_raw, markers_flag
  177 +
  178 + def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered):
  179 + """
  180 + There are two types of robot movements.
  181 + We can imagine in two concentric spheres of different sizes. The inside sphere is to compensate for small head movements.
  182 + It was named "normal" moves.
  183 + The outside sphere is for the arc motion. The arc motion is a safety feature for long robot movements.
  184 + Even for a new target or a sudden huge head movement.
  185 + 1) normal:
  186 + A linear move from the actual position until the target position.
  187 + This movement just happens when move distance is below a threshold (const.ROBOT_ARC_THRESHOLD_DISTANCE)
  188 + 2) arc motion:
  189 + It can be divided into three parts.
  190 + The first one represents the movement from the inner sphere to the outer sphere.
  191 + The robot moves back using a radial move (it use the center of the head as a reference).
  192 + The second step is the actual arc motion (along the outer sphere).
  193 + A middle point, between the actual position and the target, is required.
  194 + The last step is to make a linear move until the target (goes to the inner sphere)
  195 +
  196 + """
  197 + #Check if the target is inside the working space
  198 + if self.process_tracker.estimate_robot_target_length(new_robot_coordinates) < const.ROBOT_WORKING_SPACE:
  199 + #Check the target distance to define the motion mode
  200 + if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag:
  201 + self.trck_init_robot.SendCoordinates(new_robot_coordinates, const.ROBOT_MOTIONS["normal"])
  202 +
  203 + elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag:
  204 + actual_point = current_robot_coordinates
  205 + if not self.arc_motion_flag:
  206 + head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
  207 + current_head_filtered).tolist()
  208 +
  209 + self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
  210 + new_robot_coordinates)
  211 + self.arc_motion_flag = True
  212 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"]
  213 +
  214 + if self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["linear out"]:
  215 + coord = self.target_linear_out
  216 + if np.allclose(np.array(actual_point), np.array(self.target_linear_out), 0, 1):
  217 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["arc"]
  218 + coord = self.target_arc
  219 +
  220 + elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]:
  221 + head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
  222 + current_head_filtered).tolist()
  223 +
  224 + _, new_target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
  225 + new_robot_coordinates)
  226 + if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1):
  227 + None
  228 + else:
  229 + if self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) >= \
  230 + const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8:
  231 + self.target_arc = new_target_arc
  232 +
  233 + coord = self.target_arc
  234 +
  235 + if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10):
  236 + self.arc_motion_flag = False
  237 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
  238 + coord = new_robot_coordinates
  239 +
  240 + self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag)
  241 + else:
  242 + print("Head is too far from the robot basis")
  243 +
  244 + def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag):
  245 + coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1]
  246 + marker_head_flag = markers_flag[1]
  247 + coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2]
  248 + marker_obj_flag = markers_flag[2]
  249 +
  250 + if self.robot_tracker_flag:
  251 + current_head = coord_head_tracker_in_robot
  252 + if current_head is not None and marker_head_flag:
  253 + current_head_filtered = self.process_tracker.kalman_filter(current_head)
  254 + if self.process_tracker.compute_head_move_threshold(current_head_filtered):
  255 + new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered,
  256 + self.m_change_robot_to_head)
  257 + if self.coord_inv_old is None:
  258 + self.coord_inv_old = new_robot_coordinates
  259 +
  260 + if np.allclose(np.array(new_robot_coordinates), np.array(current_robot_coordinates), 0, 0.01):
  261 + #avoid small movements (0.01 mm)
  262 + pass
  263 + elif not np.allclose(np.array(new_robot_coordinates), np.array(self.coord_inv_old), 0, 5):
  264 + #if the head moves (>5mm) before the robot reach the target
  265 + self.trck_init_robot.StopRobot()
  266 + self.coord_inv_old = new_robot_coordinates
  267 + else:
  268 + distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates)
  269 + self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered)
  270 + self.coord_inv_old = new_robot_coordinates
  271 + else:
  272 + self.trck_init_robot.StopRobot()
  273 +
  274 + def run(self):
  275 + while not self.event_robot.is_set():
  276 + current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices()
  277 +
  278 + if not self.robot_target_queue.empty():
  279 + self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait()
  280 + self.robot_target_queue.task_done()
  281 +
  282 + if not self.object_at_target_queue.empty():
  283 + self.target_flag = self.object_at_target_queue.get_nowait()
  284 + self.object_at_target_queue.task_done()
  285 +
  286 + self.robot_control(current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag)
... ...
invesalius/navigation/tracker.py
  Diff comments   View file @3185fc2
... ... @@ -24,6 +24,7 @@ import invesalius.constants as const
24 24 import invesalius.data.coordinates as dco
25 25 import invesalius.data.trackers as dt
26 26 import invesalius.gui.dialogs as dlg
  27 +import invesalius.data.coregistration as dcr
27 28 from invesalius.pubsub import pub as Publisher
28 29  
29 30  
... ... @@ -34,12 +35,14 @@ class Tracker():
34 35  
35 36 self.tracker_fiducials = np.full([3, 3], np.nan)
36 37 self.tracker_fiducials_raw = np.zeros((6, 6))
  38 + self.m_tracker_fiducials_raw = np.zeros((6, 4, 4))
37 39  
38 40 self.tracker_connected = False
39 41  
40 42 self.thread_coord = None
41 43  
42 44 self.event_coord = threading.Event()
  45 + self.event_robot = threading.Event()
43 46  
44 47 self.TrackerCoordinates = dco.TrackerCoordinates()
45 48  
... ... @@ -74,8 +77,11 @@ class Tracker():
74 77  
75 78 if self.thread_coord:
76 79 self.event_coord.set()
  80 + self.event_robot.set()
77 81 self.thread_coord.join()
78 82 self.event_coord.clear()
  83 + self.event_robot.clear()
  84 +
79 85  
80 86 Publisher.sendMessage('Update status text in GUI',
81 87 label=_("Tracker disconnected"))
... ... @@ -85,6 +91,13 @@ class Tracker():
85 91 label=_("Tracker still connected"))
86 92 print("Tracker still connected!")
87 93  
  94 + def ConnectToRobot(self, navigation, tracker, robot):
  95 + robot.SetRobotQueues([navigation.robot_target_queue, navigation.object_at_target_queue])
  96 + robot.OnRobotConnection()
  97 + trk_init_robot = self.trk_init[1][0]
  98 + if trk_init_robot:
  99 + robot.StartRobotThreadNavigation(tracker, navigation.coord_queue)
  100 +
88 101 def IsTrackerInitialized(self):
89 102 return self.trk_init and self.tracker_id and self.tracker_connected
90 103  
... ... @@ -126,6 +139,9 @@ class Tracker():
126 139 self.tracker_fiducials_raw[2 * fiducial_index, :] = coord_raw[0, :]
127 140 self.tracker_fiducials_raw[2 * fiducial_index + 1, :] = coord_raw[1, :]
128 141  
  142 + self.m_tracker_fiducials_raw[2 * fiducial_index, :] = dcr.compute_marker_transformation(coord_raw, 0)
  143 + self.m_tracker_fiducials_raw[2 * fiducial_index + 1, :] = dcr.compute_marker_transformation(coord_raw, 1)
  144 +
129 145 print("Set tracker fiducial {} to coordinates {}.".format(fiducial_index, coord[0:3]))
130 146  
131 147 def ResetTrackerFiducials(self):
... ... @@ -135,6 +151,13 @@ class Tracker():
135 151 def GetTrackerFiducials(self):
136 152 return self.tracker_fiducials, self.tracker_fiducials_raw
137 153  
  154 + def GetMatrixTrackerFiducials(self):
  155 + m_probe_ref_left = np.linalg.inv(self.m_tracker_fiducials_raw[1]) @ self.m_tracker_fiducials_raw[0]
  156 + m_probe_ref_right = np.linalg.inv(self.m_tracker_fiducials_raw[3]) @ self.m_tracker_fiducials_raw[2]
  157 + m_probe_ref_nasion = np.linalg.inv(self.m_tracker_fiducials_raw[5]) @ self.m_tracker_fiducials_raw[4]
  158 +
  159 + return [m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion]
  160 +
138 161 def GetTrackerInfo(self):
139 162 return self.trk_init, self.tracker_id
140 163  
... ...
navigation/mtc_files/Markers/2Ref
  Diff comments   View file @3185fc2
  1 +
1 2 [Marker]
2 3 FacetsCount=1
3 4 SliderControlledXpointsCount=0
... ... @@ -7,20 +8,20 @@ Name=2Ref
7 8 VectorsCount=2
8 9  
9 10 [Facet1V1]
10   -EndPos(0,0)=-8.9120676333490003
11   -EndPos(0,1)=2.3980817331903383e-016
12   -EndPos(0,2)=-1.2505552149377763e-015
13   -EndPos(1,0)=8.9120676333490003
14   -EndPos(1,1)=2.6645352591003756e-017
15   -EndPos(1,2)=0.
  11 +EndPos(0,0)=-32.862195626831138
  12 +EndPos(0,1)=2.2319079841241168e-003
  13 +EndPos(0,2)=7.6730702825049029e-003
  14 +EndPos(1,0)=32.860817892941867
  15 +EndPos(1,1)=1.862130420993991e-003
  16 +EndPos(1,2)=5.6832171433645459e-003
16 17  
17 18 [Facet1V2]
18   -EndPos(0,0)=-8.9951734202602971
19   -EndPos(0,1)=-12.045839883319603
20   -EndPos(0,2)=-4.5474735088646413e-016
21   -EndPos(1,0)=-8.9120676333490003
22   -EndPos(1,1)=2.3980817331903383e-016
23   -EndPos(1,2)=-1.2505552149377763e-015
  19 +EndPos(0,0)=-4.6454520609455985
  20 +EndPos(0,1)=24.559311056585091
  21 +EndPos(0,2)=-1.6538883706334069
  22 +EndPos(1,0)=-4.8071411059089124
  23 +EndPos(1,1)=-22.341630540181473
  24 +EndPos(1,2)=-1.654808075526687
24 25  
25 26 [Tooltip2MarkerXf]
26 27 Scale=1.
... ...
navigation/mtc_files/Markers/3Coil_big
View file @52a9c20
... ... @@ -1,39 +0,0 @@
1   -
2   -[Marker]
3   -FacetsCount=1
4   -SliderControlledXpointsCount=0
5   -Name=3Coil_big
6   -
7   -[Facet1]
8   -VectorsCount=2
9   -
10   -[Facet1V1]
11   -EndPos(0,0)=-12.130921665071337
12   -EndPos(0,1)=4.771099944171531e-005
13   -EndPos(0,2)=-1.6777868612989378e-016
14   -EndPos(1,0)=12.131083622124752
15   -EndPos(1,1)=1.0800131569805988e-004
16   -EndPos(1,2)=-2.4831245547224282e-015
17   -
18   -[Facet1V2]
19   -EndPos(0,0)=-12.143021882072084
20   -EndPos(0,1)=-16.190735690907974
21   -EndPos(0,2)=-1.3422294890391503e-015
22   -EndPos(1,0)=-12.130921665071337
23   -EndPos(1,1)=4.771099944171531e-005
24   -EndPos(1,2)=-1.6777868612989378e-016
25   -
26   -[Tooltip2MarkerXf]
27   -Scale=1.
28   -S0=0.
29   -R0,0=1.
30   -R1,0=0.
31   -R2,0=0.
32   -S1=0.
33   -R0,1=0.
34   -R1,1=1.
35   -R2,1=0.
36   -S2=0.
37   -R0,2=0.
38   -R1,2=0.
39   -R2,2=1.
navigation/ndi_files/Markers/objP4.rom 0 → 100644
View file @3185fc2
No preview for this file type
navigation/ndi_files/Markers/refP4.rom
View file @3185fc2
No preview for this file type
optional-requirements.txt
  Diff comments   View file @3185fc2
... ... @@ -5,3 +5,4 @@ python-rtmidi==1.4.9
5 5 python-socketio[client]==5.3.0
6 6 requests==2.26.0
7 7 uvicorn[standard]==0.15.0
  8 +opencv-python==4.5.3
... ...