Download as
Browse Code »

Commit d450b91bc07b33a82a472b62c0d465630106090c

Authored by Victor Hugo Souza
Committed by GitHub
2 parents 0acb6ee2 99884224
Exists in master

Merge pull request #377 from rmatsuda/multimodal_tracking 

Add robot control for TMS applications
Showing 19 changed files with 1461 additions and 217 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @d450b91
... ... @@ -684,18 +684,19 @@ CAMERA = 5
684 684 POLARIS = 6
685 685 POLARISP4 = 7
686 686 OPTITRACK = 8
687   -DEBUGTRACKRANDOM = 9
688   -DEBUGTRACKAPPROACH = 10
  687 +ROBOT = 9
  688 +DEBUGTRACKRANDOM = 10
  689 +DEBUGTRACKAPPROACH = 11
689 690 DEFAULT_TRACKER = SELECT
690 691  
691 692 NDICOMPORT = b'COM1'
692 693  
693 694 TRACKERS = [_("Claron MicronTracker"),
694   - _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
695   - _("Polhemus PATRIOT"), _("Camera tracker"),
696   - _("NDI Polaris"), _("NDI Polaris P4"),
697   - _("Optitrack"), _("Debug tracker (random)"),
698   - _("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)")]
699 700  
700 701 STATIC_REF = 0
701 702 DYNAMIC_REF = 1
... ... @@ -847,3 +848,11 @@ WILDCARD_MARKER_FILES = _("Marker scanner coord files (*.mkss)|*.mkss")
847 848 BAUD_RATES = [300, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200]
848 849 BAUD_RATE_DEFAULT_SELECTION = 4
849 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
  857 +ROBOT_ARC_THRESHOLD_DISTANCE = 100
  858 +ROBOT_VERSOR_SCALE_FACTOR = 70
... ...
invesalius/data/bases.py
  Diff comments   View file @d450b91
... ... @@ -244,3 +244,48 @@ 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 + _, _, angles, translate, _ = tr.decompose_matrix(M_tracker_in_robot)
  288 + tracker_in_robot = [translate[0], translate[1], translate[2], \
  289 + np.degrees(angles[2]), np.degrees(angles[1]), np.degrees(angles[0])]
  290 +
  291 + return tracker_in_robot
... ...
invesalius/data/coordinates.py
  Diff comments   View file @d450b91
... ... @@ -65,15 +65,57 @@ def GetCoordinatesForThread(trck_init, trck_id, ref_mode):
65 65 const.POLARIS: PolarisCoord,
66 66 const.POLARISP4: PolarisP4Coord,
67 67 const.OPTITRACK: OptitrackCoord,
  68 + const.ROBOT: RobotCoord,
68 69 const.DEBUGTRACKRANDOM: DebugCoordRandom,
69 70 const.DEBUGTRACKAPPROACH: DebugCoordRandom}
70   -
71 71 coord, markers_flag = getcoord[trck_id](trck_init, trck_id, ref_mode)
72 72 else:
73 73 print("Select Tracker")
74 74  
75 75 return coord, markers_flag
76 76  
  77 +def PolarisP4Coord(trck_init, trck_id, ref_mode):
  78 + trck = trck_init[0]
  79 + trck.Run()
  80 +
  81 + probe = trck.probe.decode(const.FS_ENCODE)
  82 + ref = trck.ref.decode(const.FS_ENCODE)
  83 + obj = trck.obj.decode(const.FS_ENCODE)
  84 +
  85 + probe = probe[2:]
  86 + ref = ref[2:]
  87 + obj = obj[2:]
  88 +
  89 + if probe[:7] == "MISSING":
  90 + coord1 = np.hstack(([0, 0, 0], [0, 0, 0]))
  91 + else:
  92 + q = [int(probe[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
  93 + t = [int(probe[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
  94 + angles_probe = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
  95 + trans_probe = np.array(t).astype(float)
  96 + coord1 = np.hstack((trans_probe, angles_probe))
  97 +
  98 + if ref[:7] == "MISSING":
  99 + coord2 = np.hstack(([0, 0, 0], [0, 0, 0]))
  100 + else:
  101 + q = [int(ref[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
  102 + t = [int(ref[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
  103 + angles_ref = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
  104 + trans_ref = np.array(t).astype(float)
  105 + coord2 = np.hstack((trans_ref, angles_ref))
  106 +
  107 + if obj[:7] == "MISSING":
  108 + coord3 = np.hstack(([0, 0, 0], [0, 0, 0]))
  109 + else:
  110 + q = [int(obj[i:i + 6]) * 0.0001 for i in range(0, 24, 6)]
  111 + t = [int(obj[i:i + 7]) * 0.01 for i in range(24, 45, 7)]
  112 + angles_obj = np.degrees(tr.euler_from_quaternion(q, axes='rzyx'))
  113 + trans_obj = np.array(t).astype(float)
  114 + coord3 = np.hstack((trans_obj, angles_obj))
  115 +
  116 + coord = np.vstack([coord1, coord2, coord3])
  117 +
  118 + return coord, [trck.probeID, trck.refID, trck.objID]
77 119  
78 120 def OptitrackCoord(trck_init, trck_id, ref_mode):
79 121 """
... ... @@ -139,49 +181,16 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
139 181  
140 182 return coord, [trck.probeID, trck.refID, trck.objID]
141 183  
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]))
  184 +def ElfinCoord(trck_init):
  185 + if len(trck_init) > 2:
  186 + robotcoordinates = trck_init[-1]
  187 + coord = robotcoordinates.GetRobotCoordinates()
  188 + if coord is None:
  189 + coord = np.array([0, 0, 0, 0, 0, 0])
157 190 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]
  191 + coord = np.array([0, 0, 0, 0, 0, 0])
184 192  
  193 + return coord
185 194  
186 195 def CameraCoord(trck_init, trck_id, ref_mode):
187 196 trck = trck_init[0]
... ... @@ -227,7 +236,6 @@ def PolhemusCoord(trck, trck_id, ref_mode):
227 236  
228 237  
229 238 def PolhemusWrapperCoord(trck, trck_id, ref_mode):
230   -
231 239 trck.Run()
232 240 scale = 10.0 * np.array([1., 1., 1.])
233 241  
... ... @@ -475,6 +483,22 @@ def dynamic_reference_m(probe, reference):
475 483 return coord_rot
476 484  
477 485  
  486 +def RobotCoord(trk_init, trck_id, ref_mode):
  487 + coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], trk_init[2], ref_mode)
  488 + coord_robot = ElfinCoord(trk_init[1:])
  489 +
  490 + probe_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[0])
  491 + ref_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[1])
  492 + obj_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[2])
  493 +
  494 + if probe_tracker_in_robot is None:
  495 + probe_tracker_in_robot = coord_tracker[0]
  496 + ref_tracker_in_robot = coord_tracker[1]
  497 + obj_tracker_in_robot = coord_tracker[2]
  498 +
  499 + return np.vstack([probe_tracker_in_robot, ref_tracker_in_robot, coord_robot, obj_tracker_in_robot]), markers_flag
  500 +
  501 +
478 502 def dynamic_reference_m2(probe, reference):
479 503 """
480 504 Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
... ...
invesalius/data/coregistration.py
  Diff comments   View file @d450b91
... ... @@ -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 @d450b91
... ... @@ -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 @d450b91
... ... @@ -0,0 +1,207 @@
  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.transformations as tr
  24 +import invesalius.data.coregistration as dcr
  25 +import invesalius.data.coordinates as dco
  26 +import invesalius.constants as const
  27 +
  28 +
  29 +class KalmanTracker:
  30 + """
  31 + Kalman filter to avoid sudden fluctuation from tracker device.
  32 + The filter strength can be set by the cov_process, and cov_measure parameter
  33 + It is required to create one instance for each variable (x, y, z, a, b, g)
  34 + """
  35 + def __init__(self,
  36 + state_num=2,
  37 + covariance_process=0.001,
  38 + covariance_measure=0.1):
  39 +
  40 + self.state_num = state_num
  41 + measure_num = 1
  42 +
  43 + # The filter itself.
  44 + self.filter = cv2.KalmanFilter(state_num, measure_num, 0)
  45 +
  46 + self.state = np.zeros((state_num, 1), dtype=np.float32)
  47 + self.measurement = np.array((measure_num, 1), np.float32)
  48 + self.prediction = np.zeros((state_num, 1), np.float32)
  49 +
  50 +
  51 + self.filter.transitionMatrix = np.array([[1, 1],
  52 + [0, 1]], np.float32)
  53 + self.filter.measurementMatrix = np.array([[1, 1]], np.float32)
  54 + self.filter.processNoiseCov = np.array([[1, 0],
  55 + [0, 1]], np.float32) * covariance_process
  56 + self.filter.measurementNoiseCov = np.array([[1]], np.float32) * covariance_measure
  57 +
  58 + def update_kalman(self, measurement):
  59 + self.prediction = self.filter.predict()
  60 + self.measurement = np.array([[np.float32(measurement[0])]])
  61 +
  62 + self.filter.correct(self.measurement)
  63 + self.state = self.filter.statePost
  64 +
  65 +
  66 +class TrackerProcessing:
  67 + def __init__(self):
  68 + self.coord_vel = []
  69 + self.timestamp = []
  70 + self.velocity_vector = []
  71 + self.kalman_coord_vector = []
  72 + self.velocity_std = 0
  73 +
  74 + self.tracker_stabilizers = [KalmanTracker(
  75 + state_num=2,
  76 + covariance_process=0.001,
  77 + covariance_measure=0.1) for _ in range(6)]
  78 +
  79 + def kalman_filter(self, coord_tracker):
  80 + kalman_array = []
  81 + pose_np = np.array((coord_tracker[:3], coord_tracker[3:])).flatten()
  82 + for value, ps_stb in zip(pose_np, self.tracker_stabilizers):
  83 + ps_stb.update_kalman([value])
  84 + kalman_array.append(ps_stb.state[0])
  85 + coord_kalman = np.hstack(kalman_array)
  86 +
  87 + self.kalman_coord_vector.append(coord_kalman[:3])
  88 + if len(self.kalman_coord_vector) < 20: #avoid initial fluctuations
  89 + coord_kalman = coord_tracker
  90 + print('initializing filter')
  91 + else:
  92 + del self.kalman_coord_vector[0]
  93 +
  94 + return coord_kalman
  95 +
  96 + def estimate_head_velocity(self, coord_vel, timestamp):
  97 + coord_vel = np.vstack(np.array(coord_vel))
  98 + coord_init = coord_vel[:int(len(coord_vel) / 2)].mean(axis=0)
  99 + coord_final = coord_vel[int(len(coord_vel) / 2):].mean(axis=0)
  100 + velocity = (coord_final - coord_init)/(timestamp[-1] - timestamp[0])
  101 + distance = (coord_final - coord_init)
  102 +
  103 + return velocity, distance
  104 +
  105 + def compute_versors(self, init_point, final_point):
  106 + init_point = np.array(init_point)
  107 + final_point = np.array(final_point)
  108 + norm = (sum((final_point - init_point) ** 2)) ** 0.5
  109 + versor_factor = (((final_point-init_point) / norm) * const.ROBOT_VERSOR_SCALE_FACTOR).tolist()
  110 +
  111 + return versor_factor
  112 +
  113 +
  114 + def compute_arc_motion(self, current_robot_coordinates, head_center_coordinates, new_robot_coordinates):
  115 + head_center = head_center_coordinates[0], head_center_coordinates[1], head_center_coordinates[2], \
  116 + new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5]
  117 +
  118 + versor_factor_move_out = self.compute_versors(head_center, current_robot_coordinates)
  119 + init_move_out_point = current_robot_coordinates[0] + versor_factor_move_out[0], \
  120 + current_robot_coordinates[1] + versor_factor_move_out[1], \
  121 + current_robot_coordinates[2] + versor_factor_move_out[2], \
  122 + current_robot_coordinates[3], current_robot_coordinates[4], current_robot_coordinates[5]
  123 +
  124 + middle_point = ((new_robot_coordinates[0] + current_robot_coordinates[0]) / 2,
  125 + (new_robot_coordinates[1] + current_robot_coordinates[1]) / 2,
  126 + (new_robot_coordinates[2] + current_robot_coordinates[2]) / 2,
  127 + 0, 0, 0)
  128 + versor_factor_middle_arc = (np.array(self.compute_versors(head_center, middle_point))) * 2
  129 + middle_arc_point = middle_point[0] + versor_factor_middle_arc[0], \
  130 + middle_point[1] + versor_factor_middle_arc[1], \
  131 + middle_point[2] + versor_factor_middle_arc[2]
  132 +
  133 + versor_factor_arc = self.compute_versors(head_center, new_robot_coordinates)
  134 + final_ext_arc_point = new_robot_coordinates[0] + versor_factor_arc[0], \
  135 + new_robot_coordinates[1] + versor_factor_arc[1], \
  136 + new_robot_coordinates[2] + versor_factor_arc[2], \
  137 + new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5], 0
  138 +
  139 + target_arc = middle_arc_point + final_ext_arc_point
  140 +
  141 + return init_move_out_point, target_arc
  142 +
  143 + def compute_head_move_threshold(self, current_ref):
  144 + """
  145 + Checks if the head velocity is bellow the threshold
  146 + """
  147 + self.coord_vel.append(current_ref)
  148 + self.timestamp.append(time())
  149 + if len(self.coord_vel) >= 10:
  150 + head_velocity, head_distance = self.estimate_head_velocity(self.coord_vel, self.timestamp)
  151 + self.velocity_vector.append(head_velocity)
  152 +
  153 + del self.coord_vel[0]
  154 + del self.timestamp[0]
  155 +
  156 + if len(self.velocity_vector) >= 30:
  157 + self.velocity_std = np.std(self.velocity_vector)
  158 + del self.velocity_vector[0]
  159 +
  160 + if self.velocity_std > const.ROBOT_HEAD_VELOCITY_THRESHOLD:
  161 + print('Velocity threshold activated')
  162 + return False
  163 + else:
  164 + return True
  165 +
  166 + return False
  167 +
  168 + def compute_head_move_compensation(self, current_head, m_change_robot_to_head):
  169 + """
  170 + Estimates the new robot position to reach the target
  171 + """
  172 + M_current_head = dco.coordinates_to_transformation_matrix(
  173 + position=current_head[:3],
  174 + orientation=current_head[3:6],
  175 + axes='rzyx',
  176 + )
  177 + m_robot_new = M_current_head @ m_change_robot_to_head
  178 + _, _, angles, translate, _ = tr.decompose_matrix(m_robot_new)
  179 + angles = np.degrees(angles)
  180 +
  181 + return m_robot_new[0, -1], m_robot_new[1, -1], m_robot_new[2, -1], angles[0], angles[1], \
  182 + angles[2]
  183 +
  184 + def estimate_head_center(self, tracker, current_head):
  185 + """
  186 + Estimates the actual head center position using fiducials
  187 + """
  188 + m_probe_head_left, m_probe_head_right, m_probe_head_nasion = tracker.GetMatrixTrackerFiducials()
  189 + m_current_head = dcr.compute_marker_transformation(np.array([current_head]), 0)
  190 +
  191 + m_ear_left_new = m_current_head @ m_probe_head_left
  192 + m_ear_right_new = m_current_head @ m_probe_head_right
  193 +
  194 + return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2
  195 +
  196 + def correction_distance_calculation_target(self, coord_inv, actual_point):
  197 + """
  198 + Estimates the Euclidean distance between the actual position and the target
  199 + """
  200 + square_sum = (coord_inv[0]-actual_point[0]) ** 2 +\
  201 + (coord_inv[1]-actual_point[1]) ** 2 +\
  202 + (coord_inv[2]-actual_point[2]) ** 2
  203 + correction_distance_compensation = pow(square_sum, 0.5)
  204 +
  205 + return correction_distance_compensation
  206 +
  207 +
... ...
invesalius/data/trackers.py
  Diff comments   View file @d450b91
... ... @@ -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 @d450b91
... ... @@ -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 @d450b91
... ... @@ -956,10 +956,10 @@ class Viewer(wx.Panel):
956 956  
957 957 if thrdist and thrcoordx and thrcoordy and thrcoordz:
958 958 self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('Green'))
959   - Publisher.sendMessage('Coil at target', state=True)
  959 + wx.CallAfter(Publisher.sendMessage, 'Coil at target', state=True)
960 960 else:
961 961 self.dummy_coil_actor.GetProperty().SetDiffuseColor(vtk_colors.GetColor3d('DarkOrange'))
962   - Publisher.sendMessage('Coil at target', state=False)
  962 + wx.CallAfter(Publisher.sendMessage, 'Coil at target', state=False)
963 963  
964 964 self.arrow_actor_list = arrow_roll_x1, arrow_roll_x2, arrow_yaw_z1, arrow_yaw_z2, \
965 965 arrow_pitch_y1, arrow_pitch_y2
... ...
invesalius/gui/dialogs.py
  Diff comments   View file @d450b91
... ... @@ -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,8 @@ 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 + btn_ok.Enable(False)
  4695 + self.btn_ok = btn_ok
4377 4696  
4378 4697 btn_cancel = wx.Button(self, wx.ID_CANCEL)
4379 4698 btn_cancel.SetHelpText("")
... ... @@ -4402,6 +4721,9 @@ class SetNDIconfigs(wx.Dialog):
4402 4721  
4403 4722 self.CenterOnParent()
4404 4723  
  4724 + def OnChoicePort(self, evt, ctrl):
  4725 + self.btn_ok.Enable(True)
  4726 +
4405 4727 def GetValue(self):
4406 4728 fn_probe = self.dir_probe.GetPath().encode(const.FS_ENCODE)
4407 4729 fn_ref = self.dir_ref.GetPath().encode(const.FS_ENCODE)
... ...
invesalius/gui/task_navigator.py
  Diff comments   View file @d450b91
... ... @@ -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,14 +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
56 67 import invesalius.session as ses
  68 +
57 69 from invesalius import utils
58 70 from invesalius.gui import utils as gui_utils
59 71 from invesalius.navigation.icp import ICP
60 72 from invesalius.navigation.navigation import Navigation
61 73 from invesalius.navigation.tracker import Tracker
  74 +from invesalius.navigation.robot import Robot
62 75  
63 76 HAS_PEDAL_CONNECTION = True
64 77 try:
... ... @@ -184,7 +197,7 @@ class InnerFoldPanel(wx.Panel):
184 197  
185 198 # Fold 3 - Markers panel
186 199 item = fold_panel.AddFoldPanel(_("Markers"), collapsed=True)
187   - mtw = MarkersPanel(item)
  200 + mtw = MarkersPanel(item, tracker)
188 201  
189 202 fold_panel.ApplyCaptionStyle(item, style)
190 203 fold_panel.AddFoldPanelWindow(item, mtw, spacing= 0,
... ... @@ -342,6 +355,7 @@ class NeuronavigationPanel(wx.Panel):
342 355 )
343 356 self.icp = ICP()
344 357 self.tracker = tracker
  358 + self.robot = Robot(tracker)
345 359  
346 360 self.nav_status = False
347 361 self.tracker_fiducial_being_set = None
... ... @@ -641,6 +655,8 @@ class NeuronavigationPanel(wx.Panel):
641 655 self.checkbox_icp.SetValue(False)
642 656  
643 657 def OnDisconnectTracker(self):
  658 + if self.tracker.tracker_id == const.ROBOT:
  659 + self.robot.StopRobotThreadNavigation()
644 660 self.tracker.DisconnectTracker()
645 661 self.ResetICP()
646 662 self.tracker.UpdateUI(self.select_tracker_elem, self.numctrls_fiducial[3:6], self.txtctrl_fre)
... ... @@ -658,6 +674,9 @@ class NeuronavigationPanel(wx.Panel):
658 674 choice = None
659 675  
660 676 self.tracker.SetTracker(choice)
  677 + if self.tracker.tracker_id == const.ROBOT:
  678 + self.tracker.ConnectToRobot(self.navigation, self.tracker, self.robot)
  679 +
661 680 self.ResetICP()
662 681 self.tracker.UpdateUI(ctrl, self.numctrls_fiducial[3:6], self.txtctrl_fre)
663 682  
... ... @@ -740,6 +759,9 @@ class NeuronavigationPanel(wx.Panel):
740 759 choice_ref = self.choice_ref
741 760  
742 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)
743 765  
744 766 # Enable all navigation buttons
745 767 choice_ref.Enable(True)
... ... @@ -842,6 +864,7 @@ class NeuronavigationPanel(wx.Panel):
842 864 )
843 865 self.tracker.__init__()
844 866 self.icp.__init__()
  867 + self.robot.__init__(self.tracker)
845 868  
846 869  
847 870 class ObjectRegistrationPanel(wx.Panel):
... ... @@ -900,16 +923,16 @@ class ObjectRegistrationPanel(wx.Panel):
900 923  
901 924 # Change angles threshold
902 925 text_angles = wx.StaticText(self, -1, _("Angle threshold [degrees]:"))
903   - spin_size_angles = wx.SpinCtrl(self, -1, "", size=wx.Size(50, 23))
904   - 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)
905 928 spin_size_angles.SetValue(const.COIL_ANGLES_THRESHOLD)
906 929 spin_size_angles.Bind(wx.EVT_TEXT, partial(self.OnSelectAngleThreshold, ctrl=spin_size_angles))
907 930 spin_size_angles.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectAngleThreshold, ctrl=spin_size_angles))
908 931  
909 932 # Change dist threshold
910 933 text_dist = wx.StaticText(self, -1, _("Distance threshold [mm]:"))
911   - spin_size_dist = wx.SpinCtrl(self, -1, "", size=wx.Size(50, 23))
912   - 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)
913 936 spin_size_dist.SetValue(const.COIL_ANGLES_THRESHOLD)
914 937 spin_size_dist.Bind(wx.EVT_TEXT, partial(self.OnSelectDistThreshold, ctrl=spin_size_dist))
915 938 spin_size_dist.Bind(wx.EVT_SPINCTRL, partial(self.OnSelectDistThreshold, ctrl=spin_size_dist))
... ... @@ -1201,7 +1224,20 @@ class MarkersPanel(wx.Panel):
1201 1224 if field.type is bool:
1202 1225 setattr(self, field.name, str_val=='True')
1203 1226  
1204   - 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):
1205 1241 wx.Panel.__init__(self, parent)
1206 1242 try:
1207 1243 default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
... ... @@ -1211,6 +1247,8 @@ class MarkersPanel(wx.Panel):
1211 1247  
1212 1248 self.SetAutoLayout(1)
1213 1249  
  1250 + self.tracker = tracker
  1251 +
1214 1252 self.__bind_events()
1215 1253  
1216 1254 self.session = ses.Session()
... ... @@ -1218,8 +1256,11 @@ class MarkersPanel(wx.Panel):
1218 1256 self.current_coord = 0, 0, 0, 0, 0, 0
1219 1257 self.current_angle = 0, 0, 0
1220 1258 self.current_seed = 0, 0, 0
  1259 + self.current_robot_target_matrix = [None] * 9
1221 1260 self.markers = []
  1261 + self.robot_markers = []
1222 1262 self.nav_status = False
  1263 + self.raw_target_robot = None, None
1223 1264  
1224 1265 self.marker_colour = const.MARKER_COLOUR
1225 1266 self.marker_size = const.MARKER_SIZE
... ... @@ -1317,6 +1358,7 @@ class MarkersPanel(wx.Panel):
1317 1358 Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status')
1318 1359 Publisher.subscribe(self.UpdateSeedCoordinates, 'Update tracts')
1319 1360 Publisher.subscribe(self.OnChangeCurrentSession, 'Current session changed')
  1361 + Publisher.subscribe(self.UpdateRobotCoordinates, 'Update raw coordinates')
1320 1362  
1321 1363 def __find_target_marker(self):
1322 1364 """
... ... @@ -1350,6 +1392,7 @@ class MarkersPanel(wx.Panel):
1350 1392 """
1351 1393 for i in reversed(index):
1352 1394 del self.markers[i]
  1395 + del self.robot_markers[i]
1353 1396 self.lc.DeleteItem(i)
1354 1397 for n in range(0, self.lc.GetItemCount()):
1355 1398 self.lc.SetItem(n, 0, str(n + 1))
... ... @@ -1402,6 +1445,9 @@ class MarkersPanel(wx.Panel):
1402 1445 def UpdateSeedCoordinates(self, root=None, affine_vtk=None, coord_offset=(0, 0, 0)):
1403 1446 self.current_seed = coord_offset
1404 1447  
  1448 + def UpdateRobotCoordinates(self, coordinates_raw, markers_flag):
  1449 + self.raw_target_robot = coordinates_raw[1], coordinates_raw[2]
  1450 +
1405 1451 def OnMouseRightDown(self, evt):
1406 1452 # TODO: Enable the "Set as target" only when target is created with registered object
1407 1453 menu_id = wx.Menu()
... ... @@ -1412,6 +1458,16 @@ class MarkersPanel(wx.Panel):
1412 1458 menu_id.AppendSeparator()
1413 1459 target_menu = menu_id.Append(1, _('Set as target'))
1414 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)
1415 1471 # TODO: Create the remove target option so the user can disable the target without removing the marker
1416 1472 # target_menu_rem = menu_id.Append(3, _('Remove target'))
1417 1473 # menu_id.Bind(wx.EVT_MENU, self.OnMenuRemoveTarget, target_menu_rem)
... ... @@ -1462,6 +1518,15 @@ class MarkersPanel(wx.Panel):
1462 1518  
1463 1519 Publisher.sendMessage('Set new color', index=index, color=color_new)
1464 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 +
1465 1530 def OnDeleteAllMarkers(self, evt=None):
1466 1531 if evt is not None:
1467 1532 result = dlg.ShowConfirmationDialog(msg=_("Remove all markers? Cannot be undone."))
... ... @@ -1474,6 +1539,7 @@ class MarkersPanel(wx.Panel):
1474 1539 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
1475 1540  
1476 1541 self.markers = []
  1542 + self.robot_markers = []
1477 1543 Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount())
1478 1544 self.lc.DeleteAllItems()
1479 1545 Publisher.sendMessage('Stop Blink Marker', index='DeleteAll')
... ... @@ -1500,6 +1566,8 @@ class MarkersPanel(wx.Panel):
1500 1566 if index:
1501 1567 if self.__find_target_marker() in index:
1502 1568 Publisher.sendMessage('Disable or enable coil tracker', status=False)
  1569 + Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
  1570 + m_change_robot_to_head=None)
1503 1571 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
1504 1572  
1505 1573 self.__delete_multiple_markers(index)
... ... @@ -1603,12 +1671,21 @@ class MarkersPanel(wx.Panel):
1603 1671 new_marker.seed = seed or self.current_seed
1604 1672 new_marker.session_id = session_id or self.current_session
1605 1673  
  1674 + if self.tracker.tracker_id == const.ROBOT and self.nav_status:
  1675 + self.current_robot_target_matrix = db.compute_robot_to_head_matrix(self.raw_target_robot)
  1676 + else:
  1677 + self.current_robot_target_matrix = [None] * 9
  1678 +
  1679 + new_robot_marker = self.Robot_Marker()
  1680 + new_robot_marker.robot_target_matrix = self.current_robot_target_matrix
  1681 +
1606 1682 # Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added
1607 1683 Publisher.sendMessage('Add marker', ball_id=len(self.markers),
1608 1684 size=new_marker.size,
1609 1685 colour=new_marker.colour,
1610 1686 coord=new_marker.coord[:3])
1611 1687 self.markers.append(new_marker)
  1688 + self.robot_markers.append(new_robot_marker)
1612 1689  
1613 1690 # Add item to list control in panel
1614 1691 num_items = self.lc.GetItemCount()
... ...
invesalius/navigation/navigation.py
  Diff comments   View file @d450b91
... ... @@ -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 @d450b91
... ... @@ -0,0 +1,281 @@
  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 + if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag:
  198 + self.trck_init_robot.SendCoordinates(new_robot_coordinates, const.ROBOT_MOTIONS["normal"])
  199 +
  200 + elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag:
  201 + actual_point = current_robot_coordinates
  202 + if not self.arc_motion_flag:
  203 + head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
  204 + current_head_filtered).tolist()
  205 +
  206 + self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
  207 + new_robot_coordinates)
  208 + self.arc_motion_flag = True
  209 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"]
  210 +
  211 + if self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["linear out"]:
  212 + coord = self.target_linear_out
  213 + if np.allclose(np.array(actual_point), np.array(self.target_linear_out), 0, 1):
  214 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["arc"]
  215 + coord = self.target_arc
  216 +
  217 + elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]:
  218 + head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
  219 + current_head_filtered).tolist()
  220 +
  221 + _, new_target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
  222 + new_robot_coordinates)
  223 + if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1):
  224 + None
  225 + else:
  226 + if self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) >= \
  227 + const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8:
  228 + self.target_arc = new_target_arc
  229 +
  230 + coord = self.target_arc
  231 +
  232 + if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10):
  233 + self.arc_motion_flag = False
  234 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
  235 + coord = new_robot_coordinates
  236 +
  237 + self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag)
  238 +
  239 + def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag):
  240 + coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1]
  241 + marker_head_flag = markers_flag[1]
  242 + coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2]
  243 + marker_obj_flag = markers_flag[2]
  244 +
  245 + if self.robot_tracker_flag:
  246 + current_head = coord_head_tracker_in_robot
  247 + if current_head is not None and marker_head_flag:
  248 + current_head_filtered = self.process_tracker.kalman_filter(current_head)
  249 + if self.process_tracker.compute_head_move_threshold(current_head_filtered):
  250 + new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered,
  251 + self.m_change_robot_to_head)
  252 + if self.coord_inv_old is None:
  253 + self.coord_inv_old = new_robot_coordinates
  254 +
  255 + if np.allclose(np.array(new_robot_coordinates), np.array(current_robot_coordinates), 0, 0.01):
  256 + #avoid small movements (0.01 mm)
  257 + pass
  258 + elif not np.allclose(np.array(new_robot_coordinates), np.array(self.coord_inv_old), 0, 5):
  259 + #if the head moves (>5mm) before the robot reach the target
  260 + self.trck_init_robot.StopRobot()
  261 + self.coord_inv_old = new_robot_coordinates
  262 + else:
  263 + distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates)
  264 + self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered)
  265 + self.coord_inv_old = new_robot_coordinates
  266 + else:
  267 + self.trck_init_robot.StopRobot()
  268 +
  269 + def run(self):
  270 + while not self.event_robot.is_set():
  271 + current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices()
  272 +
  273 + if not self.robot_target_queue.empty():
  274 + self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait()
  275 + self.robot_target_queue.task_done()
  276 +
  277 + if not self.object_at_target_queue.empty():
  278 + self.target_flag = self.object_at_target_queue.get_nowait()
  279 + self.object_at_target_queue.task_done()
  280 +
  281 + self.robot_control(current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag)
... ...
invesalius/navigation/tracker.py
  Diff comments   View file @d450b91
... ... @@ -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 @d450b91
  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 @0acb6ee
... ... @@ -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 @d450b91
No preview for this file type
navigation/ndi_files/Markers/refP4.rom
View file @d450b91
No preview for this file type
optional-requirements.txt
  Diff comments   View file @d450b91
... ... @@ -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
... ...