Download as
Browse Code »

Commit 3a9ab9cd0df30d332aba58994620b8b4b83e280a

Authored by Renan
Committed by GitHub
1 parent f9bfc475
Exists in master

Decouple robot control (#404) 

* INIT testing..

* REM: Decouple robot

* FIX: trck_init gets the first value

* INIT testing..

* REM: Decouple robot

* FIX: trck_init gets the first value

* FIX: append tracker init robot

* FIX: transform robot coordinates to tracker coordinates system

* ENH: robot matrix estimation outside invesalius

* ENH: remove robot coordinates and target matrix from invesalius

* FIX: disconnected

* FIX: tracker connection

* ENH: Cleaning

* CLN: remove unused imports

* ENH: checks with target angle is zero/none to enable send target to robot

* more cleaning

* FIX: destroys robot dialog if not possible to connect to the robot

* ENH: deal with robot resetting

* ADD: function to estimate transformation matrix

-navigation starts after ICP box

* FIX: deals with tracker devices not connected

* FIX: transformation matrix to list

* ADD: send target to robot from image

-need tests

* FIX: image_to_tracker transformation

* ENH: apply icp matrix to target

* ENH: Show robot menu item only with tracker is robot

* FIX: typo and comments

* FIX: exception for BadNamespaceError

* ADD: set metaclass to navigation and icp

-allows access for plugins

* CLEAN: remove unused imports
Showing 14 changed files with 197 additions and 1041 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @3a9ab9c
... ... @@ -832,7 +832,6 @@ SEED_RADIUS = 1.5
832 832 # Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation.
833 833 SLEEP_NAVIGATION = 0.2
834 834 SLEEP_COORDINATES = 0.05
835   -SLEEP_ROBOT = 0.01
836 835  
837 836 BRAIN_OPACITY = 0.6
838 837 N_CPU = psutil.cpu_count()
... ... @@ -864,14 +863,3 @@ PULSE_DURATION_IN_MILLISECONDS = 0.2
864 863  
865 864 #Robot
866 865 ROBOT_ElFIN_IP = ['143.107.220.251', '169.254.153.251', '127.0.0.1']
867   -ROBOT_ElFIN_PORT = 10003
868   -ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2}
869   -ROBOT_HEAD_VELOCITY_THRESHOLD = 10 #mm/s
870   -ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm
871   -ROBOT_VERSOR_SCALE_FACTOR = 70
872   -#Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%.
873   -ROBOT_WORKING_SPACE = 760 #mm
874   -ROBOT_MOVE_STATE = {"free to move": 0,
875   - "in motion": 1009,
876   - "waiting for execution": 1013,
877   - "error": 1025}
... ...
invesalius/data/bases.py
  Diff comments   View file @3a9ab9c
... ... @@ -163,6 +163,12 @@ def transform_icp(m_img, m_icp):
163 163  
164 164 return m_img
165 165  
  166 +def inverse_transform_icp(m_img, m_icp):
  167 + coord_img = [m_img[0, -1], -m_img[1, -1], m_img[2, -1], 1]
  168 + m_img[0, -1], m_img[1, -1], m_img[2, -1], _ = np.linalg.inv(m_icp) @ coord_img
  169 + m_img[0, -1], m_img[1, -1], m_img[2, -1] = m_img[0, -1], -m_img[1, -1], m_img[2, -1]
  170 +
  171 + return m_img
166 172  
167 173 def object_registration(fiducials, orients, coord_raw, m_change):
168 174 """
... ... @@ -244,47 +250,3 @@ def object_registration(fiducials, orients, coord_raw, m_change):
244 250 )
245 251  
246 252 return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
247   -
248   -def compute_robot_to_head_matrix(raw_target_robot):
249   - """
250   - :param head: nx6 array of head coordinates from tracking device in robot space
251   - :param robot: nx6 array of robot coordinates
252   -
253   - :return: target_robot_matrix: 3x3 array representing change of basis from robot to head in robot system
254   - """
255   - head, robot = raw_target_robot
256   - # compute head target matrix
257   - m_head_target = dco.coordinates_to_transformation_matrix(
258   - position=head[:3],
259   - orientation=head[3:],
260   - axes='rzyx',
261   - )
262   -
263   - # compute robot target matrix
264   - m_robot_target = dco.coordinates_to_transformation_matrix(
265   - position=robot[:3],
266   - orientation=robot[3:],
267   - axes='rzyx',
268   - )
269   - robot_to_head_matrix = np.linalg.inv(m_head_target) @ m_robot_target
270   -
271   - return robot_to_head_matrix
272   -
273   -
274   -class transform_tracker_to_robot(object):
275   - M_tracker_to_robot = np.array([])
276   - def transformation_tracker_to_robot(self, tracker_coord):
277   - if not transform_tracker_to_robot.M_tracker_to_robot.any():
278   - return None
279   -
280   - M_tracker = dco.coordinates_to_transformation_matrix(
281   - position=tracker_coord[:3],
282   - orientation=tracker_coord[3:6],
283   - axes='rzyx',
284   - )
285   - M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker
286   -
287   - translation, angles_as_deg = dco.transformation_matrix_to_coordinates(M_tracker_in_robot, axes='rzyx')
288   - tracker_in_robot = list(translation) + list(angles_as_deg)
289   -
290   - return tracker_in_robot
... ...
invesalius/data/coordinates.py
  Diff comments   View file @3a9ab9c
... ... @@ -22,7 +22,6 @@ import numpy as np
22 22 import threading
23 23 import wx
24 24  
25   -import invesalius.data.bases as db
26 25 import invesalius.data.transformations as tr
27 26 import invesalius.constants as const
28 27  
... ... @@ -47,14 +46,20 @@ class TrackerCoordinates():
47 46 def SetCoordinates(self, coord, markers_flag):
48 47 self.coord = coord
49 48 self.markers_flag = markers_flag
50   - if self.previous_markers_flag != self.markers_flag and not self.nav_status:
51   - wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
52   - self.previous_markers_flag = self.markers_flag
  49 + if not self.nav_status:
  50 + wx.CallAfter(Publisher.sendMessage, 'Update tracker coordinates',
  51 + coord=self.coord.tolist(), markers_flag=self.markers_flag)
  52 + if self.previous_markers_flag != self.markers_flag:
  53 + wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
  54 + self.previous_markers_flag = self.markers_flag
53 55  
54 56 def GetCoordinates(self):
55   - if self.previous_markers_flag != self.markers_flag and self.nav_status:
56   - wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
57   - self.previous_markers_flag = self.markers_flag
  57 + if self.nav_status:
  58 + wx.CallAfter(Publisher.sendMessage, 'Update tracker coordinates',
  59 + coord=self.coord.tolist(), markers_flag=self.markers_flag)
  60 + if self.previous_markers_flag != self.markers_flag:
  61 + wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag)
  62 + self.previous_markers_flag = self.markers_flag
58 63  
59 64 return self.coord, self.markers_flag
60 65  
... ... @@ -195,16 +200,6 @@ def PolarisCoord(trck_init, trck_id, ref_mode):
195 200  
196 201 return coord, [trck.probeID, trck.refID, trck.objID]
197 202  
198   -def ElfinCoord(trck_init):
199   - if len(trck_init) > 2:
200   - robotcoordinates = trck_init[-1]
201   - coord = robotcoordinates.GetRobotCoordinates()
202   - if coord is None:
203   - coord = np.array([0, 0, 0, 0, 0, 0])
204   - else:
205   - coord = np.array([0, 0, 0, 0, 0, 0])
206   -
207   - return coord
208 203  
209 204 def CameraCoord(trck_init, trck_id, ref_mode):
210 205 trck = trck_init[0]
... ... @@ -346,6 +341,13 @@ def PolhemusSerialCoord(trck_init, trck_id, ref_mode):
346 341  
347 342 return coord
348 343  
  344 +def RobotCoord(trk_init, trck_id, ref_mode):
  345 + #trck_id is the tracker related to the robot ID. To get the tracker ID, combined with the robot,
  346 + # it is required to get trk_init[1]
  347 + tracker_id = trk_init[1]
  348 + coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], tracker_id, ref_mode)
  349 +
  350 + return np.vstack([coord_tracker[0], coord_tracker[1], coord_tracker[2]]), markers_flag
349 351  
350 352 def DebugCoordRandom(trk_init, trck_id, ref_mode):
351 353 """
... ... @@ -497,22 +499,6 @@ def dynamic_reference_m(probe, reference):
497 499 return coord_rot
498 500  
499 501  
500   -def RobotCoord(trk_init, trck_id, ref_mode):
501   - coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0][0], trk_init[1], ref_mode)
502   - coord_robot = ElfinCoord([trk_init[0][1]]+trk_init[1:])
503   -
504   - probe_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[0])
505   - ref_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[1])
506   - obj_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[2])
507   -
508   - if probe_tracker_in_robot is None:
509   - probe_tracker_in_robot = coord_tracker[0]
510   - ref_tracker_in_robot = coord_tracker[1]
511   - obj_tracker_in_robot = coord_tracker[2]
512   -
513   - return np.vstack([probe_tracker_in_robot, ref_tracker_in_robot, coord_robot, obj_tracker_in_robot]), markers_flag
514   -
515   -
516 502 def dynamic_reference_m2(probe, reference):
517 503 """
518 504 Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama
... ... @@ -590,4 +576,4 @@ class ReceiveCoordinates(threading.Thread):
590 576 while not self.event.is_set():
591 577 coord_raw, markers_flag = GetCoordinatesForThread(self.trck_init, self.trck_id, const.DEFAULT_REF_MODE)
592 578 self.TrackerCoordinates.SetCoordinates(coord_raw, markers_flag)
593   - sleep(const.SLEEP_COORDINATES)
594 579 \ No newline at end of file
  580 + sleep(const.SLEEP_COORDINATES)
... ...
invesalius/data/coregistration.py
  Diff comments   View file @3a9ab9c
... ... @@ -101,6 +101,32 @@ def tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn):
101 101 m_img[:3, :3] = r_obj[:3, :3]
102 102 return m_img
103 103  
  104 +def image_to_tracker(m_change, target, icp):
  105 + """Compute the transformation matrix to the tracker coordinate system
  106 +
  107 + :param m_change: Corregistration transformation obtained from fiducials
  108 + :type m_change: numpy.ndarray
  109 + :param target: Target in invesalius coordinate system
  110 + :type target: numpy.ndarray
  111 + :param icp: ICP transformation matrix
  112 + :type icp: numpy.ndarray
  113 +
  114 + :return: 4 x 4 numpy double array
  115 + :rtype: numpy.ndarray
  116 + """
  117 + m_target_in_image = dco.coordinates_to_transformation_matrix(
  118 + position=target[:3],
  119 + orientation=[0, 0, 0],
  120 + axes='sxyz',
  121 + )
  122 + if icp.use_icp:
  123 + m_target_in_image = bases.inverse_transform_icp(m_target_in_image, icp.m_icp)
  124 + m_target_in_tracker = np.linalg.inv(m_change) @ m_target_in_image
  125 +
  126 + # invert y coordinate
  127 + m_target_in_tracker[2, -1] = -m_target_in_tracker[2, -1]
  128 +
  129 + return m_target_in_tracker
104 130  
105 131 def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp):
106 132  
... ... @@ -193,7 +219,6 @@ class CoordinateCorregistrate(threading.Thread):
193 219 self.object_at_target_queue = queues[3]
194 220 self.icp = None
195 221 self.m_icp = None
196   - self.robot_tracker_flag = None
197 222 self.last_coord = None
198 223 self.tracker_id = tracker_id
199 224 self.target = target
... ... @@ -254,7 +279,7 @@ class CoordinateCorregistrate(threading.Thread):
254 279 if self.icp:
255 280 m_img = bases.transform_icp(m_img, self.m_icp)
256 281  
257   - self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
  282 + self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj])
258 283 # print('CoordCoreg: put {}'.format(count))
259 284 # count += 1
260 285  
... ... @@ -307,7 +332,7 @@ class CoordinateCorregistrateNoObject(threading.Thread):
307 332 if self.icp:
308 333 m_img = bases.transform_icp(m_img, self.m_icp)
309 334  
310   - self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj])
  335 + self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj])
311 336  
312 337 if self.view_tracts:
313 338 self.coord_tracts_queue.put_nowait(m_img_flip)
... ...
invesalius/data/elfin.py
View file @f9bfc47
... ... @@ -1,252 +0,0 @@
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, motion_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 == const.ROBOT_MOVE_STATE["free to move"]:
34   - if motion_type == const.ROBOT_MOTIONS["normal"] or motion_type == const.ROBOT_MOTIONS["linear out"]:
35   - self.cobot.MoveL(target)
36   - elif motion_type == const.ROBOT_MOTIONS["arc"]:
37   - self.cobot.MoveC(target)
38   - elif status == const.ROBOT_MOVE_STATE["error"]:
39   - self.StopRobot()
40   -
41   - def StopRobot(self):
42   - # Takes some microseconds to the robot actual stops after the command.
43   - # The sleep time is required to guarantee the stop
44   - self.cobot.GrpStop()
45   - sleep(0.1)
46   -
47   - def Close(self):
48   - self.StopRobot()
49   - #TODO: robot function to close? self.cobot.close()
50   -
51   -class Elfin:
52   - def __init__(self):
53   - """
54   - Class to communicate with elfin robot. This class follows "HansRobot Communication Protocol Interface".
55   - """
56   - self.end_msg = ",;"
57   -
58   - def connect(self, server_ip, port_number, message_size, robot_id):
59   - mySocket = socket(AF_INET, SOCK_STREAM)
60   - mySocket.connect((server_ip, port_number))
61   -
62   - self.message_size = message_size
63   - self.robot_id = str(robot_id)
64   - self.mySocket = mySocket
65   -
66   - def send(self, message):
67   - self.mySocket.sendall(message.encode('utf-8'))
68   - data = self.mySocket.recv(self.message_size).decode('utf-8').split(',')
69   - status = self.check_status(data)
70   - if status and type(data) != bool:
71   - if len(data) > 3:
72   - return data[2:-1]
73   - return status
74   -
75   - def check_status(self, recv_message):
76   - status = recv_message[1]
77   - if status == 'OK':
78   - return True
79   -
80   - elif status == 'Fail':
81   - print("Error code: ", recv_message[2])
82   - return False
83   -
84   - def Electrify(self):
85   - """
86   - Function: Power the robot
87   - Notes: successful completion of power up before returning, power up time is
88   - about 44s.
89   - :return:
90   - if Error Return False
91   - if not Error Return True
92   - """
93   - message = "Electrify" + self.end_msg
94   - status = self.send(message)
95   - return status
96   -
97   - def BlackOut(self):
98   - """
99   - Function: Robot blackout
100   - Notes: successful power outage will only return, power failure time is 3s.
101   - :return:
102   - if Error Return False
103   - if not Error Return True
104   - """
105   - message = "BlackOut" + self.end_msg
106   - status = self.send(message)
107   - return status
108   -
109   - def StartMaster(self):
110   - """
111   - Function: Start master station
112   - Notes: the master station will not be returned until successfully started, startup
113   - master time is about 4s.
114   - :return:
115   - if Error Return False
116   - if not Error Return True
117   - """
118   - message = "StartMaster" + self.end_msg
119   - status = self.send(message)
120   - return status
121   -
122   - def CloseMaster(self):
123   - """
124   - Function: Close master station
125   - Notes: the master station will not be returned until successfully closed, shut
126   - down the master station time is about 2s.
127   - :return:
128   - if Error Return False
129   - if not Error Return True
130   - """
131   - message = "CloseMaster" + self.end_msg
132   - status = self.send(message)
133   - return status
134   -
135   - def GrpPowerOn(self):
136   - """
137   - Function: Robot servo on
138   - :return:
139   - if Error Return False
140   - if not Error Return True
141   - """
142   - message = "GrpPowerOn," + self.robot_id + self.end_msg
143   - status = self.send(message)
144   - return status
145   -
146   - def GrpPowerOff(self):
147   - """
148   - Function: Robot servo off
149   - :return:
150   - if Error Return False
151   - if not Error Return True
152   - """
153   - message = "GrpPowerOff," + self.robot_id + self.end_msg
154   - status = self.send(message)
155   - return status
156   -
157   - def GrpStop(self):
158   - """
159   - Function: Stop robot
160   - :return:
161   - if Error Return False
162   - if not Error Return True
163   - """
164   - message = "GrpStop," + self.robot_id + self.end_msg
165   - status = self.send(message)
166   - return status
167   -
168   - def SetOverride(self, override):
169   - """
170   - function: Set speed ratio
171   - :param override:
172   - double: set speed ratio, range of 0.01~1
173   - :return:
174   - if Error Return False
175   - if not Error Return True
176   - """
177   -
178   - message = "SetOverride," + self.robot_id + ',' + str(override) + self.end_msg
179   - status = self.send(message)
180   - return status
181   -
182   - def ReadPcsActualPos(self):
183   - """Function: Get the actual position of the space coordinate
184   - :return:
185   - if True Return x,y,z,a,b,c
186   - if Error Return False
187   - """
188   - message = "ReadPcsActualPos," + self.robot_id + self.end_msg
189   - coord = self.send(message)
190   - if coord:
191   - return [float(s) for s in coord]
192   -
193   - return coord
194   -
195   - def MoveL(self, target):
196   - """
197   - function: Robot moves straight to the specified space coordinates
198   - :param: target:[X,Y,Z,RX,RY,RZ]
199   - :return:
200   - """
201   - target = [str(s) for s in target]
202   - target = (",".join(target))
203   - message = "MoveL," + self.robot_id + ',' + target + self.end_msg
204   - return self.send(message)
205   -
206   - def SetToolCoordinateMotion(self, status):
207   - """
208   - function: Function: Set tool coordinate motion
209   - :param: int Switch 0=close 1=open
210   - :return:
211   - if Error Return False
212   - if not Error Return True
213   - """
214   - message = "SetToolCoordinateMotion," + self.robot_id + ',' + str(status) + self.end_msg
215   - status = self.send(message)
216   - return status
217   -
218   - def ReadMoveState(self):
219   - """
220   - Function: Get the motion state of the robot
221   - :return:
222   - Current state of motion of robot:
223   - 0=motion completion;
224   - 1009=in motion;
225   - 1013=waiting for execution;
226   - 1025 =Error reporting
227   - """
228   - message = "ReadMoveState," + self.robot_id + self.end_msg
229   - status = int(self.send(message)[0])
230   - return status
231   -
232   - def MoveHoming(self):
233   - """
234   - Function: Robot returns to the origin
235   - :return:
236   - if Error Return False
237   - if not Error Return True
238   - """
239   - message = "MoveHoming," + self.robot_id + self.end_msg
240   - status = self.send(message)
241   - return status
242   -
243   - def MoveC(self, target):
244   - """
245   - function: Arc motion
246   - :param: Through position[X,Y,Z],GoalCoord[X,Y,Z,RX,RY,RZ],Type[0 or 1],;
247   - :return:
248   - """
249   - target = [str(s) for s in target]
250   - target = (",".join(target))
251   - message = "MoveC," + self.robot_id + ',' + target + self.end_msg
252   - return self.send(message)
invesalius/data/elfin_processing.py
View file @f9bfc47
... ... @@ -1,212 +0,0 @@
1   -#--------------------------------------------------------------------------
2   -# Software: InVesalius - Software de Reconstrucao 3D de Imagens Medicas
3   -# Copyright: (C) 2001 Centro de Pesquisas Renato Archer
4   -# Homepage: http://www.softwarepublico.gov.br
5   -# Contact: invesalius@cti.gov.br
6   -# License: GNU - GPL 2 (LICENSE.txt/LICENCA.txt)
7   -#--------------------------------------------------------------------------
8   -# Este programa e software livre; voce pode redistribui-lo e/ou
9   -# modifica-lo sob os termos da Licenca Publica Geral GNU, conforme
10   -# publicada pela Free Software Foundation; de acordo com a versao 2
11   -# da Licenca.
12   -#
13   -# Este programa eh distribuido na expectativa de ser util, mas SEM
14   -# QUALQUER GARANTIA; sem mesmo a garantia implicita de
15   -# COMERCIALIZACAO ou de ADEQUACAO A QUALQUER PROPOSITO EM
16   -# PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais
17   -# detalhes.
18   -#--------------------------------------------------------------------------
19   -import numpy as np
20   -import cv2
21   -from time import time
22   -
23   -import invesalius.data.coregistration as dcr
24   -import invesalius.data.coordinates as dco
25   -import invesalius.constants as const
26   -
27   -
28   -class KalmanTracker:
29   - """
30   - Kalman filter to avoid sudden fluctuation from tracker device.
31   - The filter strength can be set by the cov_process, and cov_measure parameter
32   - It is required to create one instance for each variable (x, y, z, a, b, g)
33   - """
34   - def __init__(self,
35   - state_num=2,
36   - covariance_process=0.001,
37   - covariance_measure=0.1):
38   -
39   - self.state_num = state_num
40   - measure_num = 1
41   -
42   - # The filter itself.
43   - self.filter = cv2.KalmanFilter(state_num, measure_num, 0)
44   -
45   - self.state = np.zeros((state_num, 1), dtype=np.float32)
46   - self.measurement = np.array((measure_num, 1), np.float32)
47   - self.prediction = np.zeros((state_num, 1), np.float32)
48   -
49   -
50   - self.filter.transitionMatrix = np.array([[1, 1],
51   - [0, 1]], np.float32)
52   - self.filter.measurementMatrix = np.array([[1, 1]], np.float32)
53   - self.filter.processNoiseCov = np.array([[1, 0],
54   - [0, 1]], np.float32) * covariance_process
55   - self.filter.measurementNoiseCov = np.array([[1]], np.float32) * covariance_measure
56   -
57   - def update_kalman(self, measurement):
58   - self.prediction = self.filter.predict()
59   - self.measurement = np.array([[np.float32(measurement[0])]])
60   -
61   - self.filter.correct(self.measurement)
62   - self.state = self.filter.statePost
63   -
64   -
65   -class TrackerProcessing:
66   - def __init__(self):
67   - self.coord_vel = []
68   - self.timestamp = []
69   - self.velocity_vector = []
70   - self.kalman_coord_vector = []
71   - self.velocity_std = 0
72   -
73   - self.tracker_stabilizers = [KalmanTracker(
74   - state_num=2,
75   - covariance_process=0.001,
76   - covariance_measure=0.1) for _ in range(6)]
77   -
78   - def kalman_filter(self, coord_tracker):
79   - kalman_array = []
80   - pose_np = np.array((coord_tracker[:3], coord_tracker[3:])).flatten()
81   - for value, ps_stb in zip(pose_np, self.tracker_stabilizers):
82   - ps_stb.update_kalman([value])
83   - kalman_array.append(ps_stb.state[0])
84   - coord_kalman = np.hstack(kalman_array)
85   -
86   - self.kalman_coord_vector.append(coord_kalman[:3])
87   - if len(self.kalman_coord_vector) < 20: #avoid initial fluctuations
88   - coord_kalman = coord_tracker
89   - print('initializing filter')
90   - else:
91   - del self.kalman_coord_vector[0]
92   -
93   - return coord_kalman
94   -
95   - def estimate_head_velocity(self, coord_vel, timestamp):
96   - coord_vel = np.vstack(np.array(coord_vel))
97   - coord_init = coord_vel[:int(len(coord_vel) / 2)].mean(axis=0)
98   - coord_final = coord_vel[int(len(coord_vel) / 2):].mean(axis=0)
99   - velocity = (coord_final - coord_init)/(timestamp[-1] - timestamp[0])
100   - distance = (coord_final - coord_init)
101   -
102   - return velocity, distance
103   -
104   - def compute_versors(self, init_point, final_point):
105   - init_point = np.array(init_point)
106   - final_point = np.array(final_point)
107   - norm = (sum((final_point - init_point) ** 2)) ** 0.5
108   - versor_factor = (((final_point-init_point) / norm) * const.ROBOT_VERSOR_SCALE_FACTOR).tolist()
109   -
110   - return versor_factor
111   -
112   -
113   - def compute_arc_motion(self, current_robot_coordinates, head_center_coordinates, new_robot_coordinates):
114   - head_center = head_center_coordinates[0], head_center_coordinates[1], head_center_coordinates[2], \
115   - new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5]
116   -
117   - versor_factor_move_out = self.compute_versors(head_center, current_robot_coordinates)
118   - init_move_out_point = current_robot_coordinates[0] + versor_factor_move_out[0], \
119   - current_robot_coordinates[1] + versor_factor_move_out[1], \
120   - current_robot_coordinates[2] + versor_factor_move_out[2], \
121   - current_robot_coordinates[3], current_robot_coordinates[4], current_robot_coordinates[5]
122   -
123   - middle_point = ((new_robot_coordinates[0] + current_robot_coordinates[0]) / 2,
124   - (new_robot_coordinates[1] + current_robot_coordinates[1]) / 2,
125   - (new_robot_coordinates[2] + current_robot_coordinates[2]) / 2,
126   - 0, 0, 0)
127   - versor_factor_middle_arc = (np.array(self.compute_versors(head_center, middle_point))) * 2
128   - middle_arc_point = middle_point[0] + versor_factor_middle_arc[0], \
129   - middle_point[1] + versor_factor_middle_arc[1], \
130   - middle_point[2] + versor_factor_middle_arc[2]
131   -
132   - versor_factor_arc = self.compute_versors(head_center, new_robot_coordinates)
133   - final_ext_arc_point = new_robot_coordinates[0] + versor_factor_arc[0], \
134   - new_robot_coordinates[1] + versor_factor_arc[1], \
135   - new_robot_coordinates[2] + versor_factor_arc[2], \
136   - new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5], 0
137   -
138   - target_arc = middle_arc_point + final_ext_arc_point
139   -
140   - return init_move_out_point, target_arc
141   -
142   - def compute_head_move_threshold(self, current_ref):
143   - """
144   - Checks if the head velocity is bellow the threshold
145   - """
146   - self.coord_vel.append(current_ref)
147   - self.timestamp.append(time())
148   - if len(self.coord_vel) >= 10:
149   - head_velocity, head_distance = self.estimate_head_velocity(self.coord_vel, self.timestamp)
150   - self.velocity_vector.append(head_velocity)
151   -
152   - del self.coord_vel[0]
153   - del self.timestamp[0]
154   -
155   - if len(self.velocity_vector) >= 30:
156   - self.velocity_std = np.std(self.velocity_vector)
157   - del self.velocity_vector[0]
158   -
159   - if self.velocity_std > const.ROBOT_HEAD_VELOCITY_THRESHOLD:
160   - print('Velocity threshold activated')
161   - return False
162   - else:
163   - return True
164   -
165   - return False
166   -
167   - def compute_head_move_compensation(self, current_head, m_change_robot_to_head):
168   - """
169   - Estimates the new robot position to reach the target
170   - """
171   - M_current_head = dco.coordinates_to_transformation_matrix(
172   - position=current_head[:3],
173   - orientation=current_head[3:6],
174   - axes='rzyx',
175   - )
176   - m_robot_new = M_current_head @ m_change_robot_to_head
177   -
178   - translation, angles_as_deg = dco.transformation_matrix_to_coordinates(m_robot_new, axes='sxyz')
179   - new_robot_position = list(translation) + list(angles_as_deg)
180   -
181   - return new_robot_position
182   -
183   - def estimate_head_center(self, tracker, current_head):
184   - """
185   - Estimates the actual head center position using fiducials
186   - """
187   - m_probe_head_left, m_probe_head_right, m_probe_head_nasion = tracker.GetMatrixTrackerFiducials()
188   - m_current_head = dcr.compute_marker_transformation(np.array([current_head]), 0)
189   -
190   - m_ear_left_new = m_current_head @ m_probe_head_left
191   - m_ear_right_new = m_current_head @ m_probe_head_right
192   -
193   - return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2
194   -
195   - def correction_distance_calculation_target(self, coord_inv, actual_point):
196   - """
197   - Estimates the Euclidean distance between the actual position and the target
198   - """
199   - correction_distance_compensation = np.sqrt((coord_inv[0]-actual_point[0]) ** 2 +
200   - (coord_inv[1]-actual_point[1]) ** 2 +
201   - (coord_inv[2]-actual_point[2]) ** 2)
202   -
203   - return correction_distance_compensation
204   -
205   - def estimate_robot_target_length(self, robot_target):
206   - """
207   - Estimates the length of the 3D vector of the robot target
208   - """
209   - robot_target_length = np.sqrt(robot_target[0] ** 2 + robot_target[1] ** 2 + robot_target[2] ** 2)
210   -
211   - return robot_target_length
212   -
invesalius/data/trackers.py
  Diff comments   View file @3a9ab9c
... ... @@ -18,6 +18,7 @@
18 18 #--------------------------------------------------------------------------
19 19 import invesalius.constants as const
20 20 import invesalius.gui.dialogs as dlg
  21 +from invesalius.pubsub import pub as Publisher
21 22 # TODO: Disconnect tracker when a new one is connected
22 23 # TODO: Test if there are too many prints when connection fails
23 24 # TODO: Redesign error messages. No point in having "Could not connect to default tracker" in all trackers
... ... @@ -223,42 +224,23 @@ def OptitrackTracker(tracker_id):
223 224 print('#####')
224 225 return trck_init, lib_mode
225 226  
226   -def ElfinRobot(robot_IP):
227   - trck_init = None
228   - try:
229   - import invesalius.data.elfin as elfin
230   - print("Trying to connect Robot via: ", robot_IP)
231   - trck_init = elfin.Elfin_Server(robot_IP, const.ROBOT_ElFIN_PORT)
232   - trck_init.Initialize()
233   - lib_mode = 'wrapper'
234   - print('Connect to elfin robot tracking device.')
235   -
236   - except:
237   - lib_mode = 'error'
238   - trck_init = None
239   - print('Could not connect to elfin robot tracker.')
240   -
241   - # return tracker initialization variable and type of connection
242   - return trck_init, lib_mode
243   -
244 227 def RobotTracker(tracker_id):
245 228 from wx import ID_OK
246   - trck_init = None
247   - trck_init_robot = None
248   - tracker_id = None
  229 +
249 230 dlg_device = dlg.SetTrackerDeviceToRobot()
250 231 if dlg_device.ShowModal() == ID_OK:
251 232 tracker_id = dlg_device.GetValue()
252 233 if tracker_id:
253   - trck_connection = TrackerConnection(tracker_id, None, 'connect')
254   - if trck_connection[0]:
  234 + trck_init = TrackerConnection(tracker_id, None, 'connect')
  235 + if trck_init[0]:
255 236 dlg_ip = dlg.SetRobotIP()
256 237 if dlg_ip.ShowModal() == ID_OK:
257 238 robot_IP = dlg_ip.GetValue()
258   - trck_init = trck_connection
259   - trck_init_robot = ElfinRobot(robot_IP)
  239 + Publisher.sendMessage('Connect to robot', robot_IP=robot_IP)
  240 +
  241 + return trck_init, tracker_id
260 242  
261   - return [(trck_init, trck_init_robot), tracker_id]
  243 + return None, None
262 244  
263 245 def DebugTrackerRandom(tracker_id):
264 246 trck_init = True
... ... @@ -392,8 +374,9 @@ def DisconnectTracker(tracker_id, trck_init):
392 374 lib_mode = 'serial'
393 375 print('Tracker disconnected.')
394 376 elif tracker_id == const.ROBOT:
395   - trck_init[0][0].Close()
396   - trck_init[1][0].Close()
  377 + Publisher.sendMessage('Reset robot', data=None)
  378 + if trck_init[1] == const.DEBUGTRACKRANDOM or trck_init[1] == const.DEBUGTRACKAPPROACH:
  379 + trck_init[0].Close()
397 380 trck_init = False
398 381 lib_mode = 'wrapper'
399 382 print('Tracker disconnected.')
... ...
invesalius/gui/dialogs.py
  Diff comments   View file @3a9ab9c
... ... @@ -57,8 +57,6 @@ except ImportError:
57 57 from wx import AboutDialogInfo, AboutBox
58 58  
59 59 import invesalius.constants as const
60   -import invesalius.data.coordinates as dco
61   -import invesalius.data.transformations as tr
62 60 import invesalius.gui.widgets.gradient as grad
63 61 import invesalius.session as ses
64 62 import invesalius.utils as utils
... ... @@ -3597,11 +3595,7 @@ class ObjectCalibrationDialog(wx.Dialog):
3597 3595 # and not change the function to the point of potentially breaking it.)
3598 3596 #
3599 3597 if self.obj_ref_id and fiducial_index == 4:
3600   - if self.tracker_id == const.ROBOT:
3601   - trck_init_robot = self.trk_init[1][0]
3602   - coord = trck_init_robot.Run()
3603   - else:
3604   - coord = coord_raw[self.obj_ref_id, :]
  3598 + coord = coord_raw[self.obj_ref_id, :]
3605 3599 else:
3606 3600 coord = coord_raw[0, :]
3607 3601  
... ... @@ -4323,7 +4317,7 @@ class SetTrackerDeviceToRobot(wx.Dialog):
4323 4317 def _init_gui(self):
4324 4318 # ComboBox for spatial tracker device selection
4325 4319 tooltip = wx.ToolTip(_("Choose the tracking device"))
4326   - trackers = const.TRACKERS
  4320 + trackers = const.TRACKERS.copy()
4327 4321 if not ses.Session().debug:
4328 4322 del trackers[-3:]
4329 4323 tracker_options = [_("Select tracker:")] + trackers
... ... @@ -4425,10 +4419,8 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4425 4419 M_robot_2_tracker is created by an affine transformation. Robot TCP should be calibrated to the center of the tracker marker
4426 4420 '''
4427 4421 #TODO: make aboutbox
4428   - self.tracker_coord = []
4429   - self.tracker_angles = []
4430   - self.robot_coord = []
4431   - self.robot_angles = []
  4422 + self.matrix_tracker_to_robot = []
  4423 + self.robot_status = False
4432 4424  
4433 4425 self.tracker = tracker
4434 4426  
... ... @@ -4470,6 +4462,8 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4470 4462  
4471 4463 btn_load = wx.Button(self, -1, label=_('Load'), size=wx.Size(65, 23))
4472 4464 btn_load.Bind(wx.EVT_BUTTON, self.OnLoadReg)
  4465 + btn_load.Enable(False)
  4466 + self.btn_load = btn_load
4473 4467  
4474 4468 # Create a horizontal sizers
4475 4469 border = 1
... ... @@ -4524,16 +4518,17 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4524 4518 main_sizer.Fit(self)
4525 4519  
4526 4520 self.CenterOnParent()
  4521 + self.__bind_events()
4527 4522  
4528   - def affine_correg(self, tracker, robot):
4529   - m_change = tr.affine_matrix_from_points(robot[:].T, tracker[:].T,
4530   - shear=False, scale=False, usesvd=False)
4531   - return m_change
  4523 + def __bind_events(self):
  4524 + Publisher.subscribe(self.OnUpdateTransformationMatrix, 'Update robot transformation matrix')
  4525 + Publisher.subscribe(self.OnCoordinatesAdquired, 'Coordinates for the robot transformation matrix collected')
  4526 + Publisher.subscribe(self.OnRobotConnectionStatus, 'Robot connection status')
4532 4527  
4533 4528 def OnContinuousAcquisition(self, evt=None, btn=None):
4534 4529 value = btn.GetValue()
4535 4530 if value:
4536   - self.timer.Start(500)
  4531 + self.timer.Start(100)
4537 4532 else:
4538 4533 self.timer.Stop()
4539 4534  
... ... @@ -4541,56 +4536,50 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4541 4536 self.OnCreatePoint(evt=None)
4542 4537  
4543 4538 def OnCreatePoint(self, evt):
4544   - coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
4545   - #robot thread is not initialized yet
4546   - coord_raw_robot = self.tracker.trk_init[0][1][0].Run()
4547   - coord_raw_tracker_obj = coord_raw[3]
4548   -
4549   - if markers_flag[2]:
4550   - self.tracker_coord.append(coord_raw_tracker_obj[:3])
4551   - self.tracker_angles.append(coord_raw_tracker_obj[3:])
4552   - self.robot_coord.append(coord_raw_robot[:3])
4553   - self.robot_angles.append(coord_raw_robot[3:])
4554   - self.txt_number.SetLabel(str(int(self.txt_number.GetLabel())+1))
4555   - else:
4556   - print('Cannot detect the coil markers, pls try again')
  4539 + Publisher.sendMessage('Collect coordinates for the robot transformation matrix', data=None)
  4540 +
  4541 + def OnCoordinatesAdquired(self):
  4542 + self.txt_number.SetLabel(str(int(self.txt_number.GetLabel())+1))
4557 4543  
4558   - if len(self.tracker_coord) >= 3:
  4544 + if self.robot_status and int(self.txt_number.GetLabel()) >= 3:
4559 4545 self.btn_apply_reg.Enable(True)
4560 4546  
  4547 + def OnRobotConnectionStatus(self, data):
  4548 + self.robot_status = data
  4549 + if self.robot_status:
  4550 + self.btn_load.Enable(True)
  4551 + if int(self.txt_number.GetLabel()) >= 3:
  4552 + self.btn_apply_reg.Enable(True)
  4553 +
4561 4554 def OnReset(self, evt):
  4555 + Publisher.sendMessage('Reset coordinates collection for the robot transformation matrix', data=None)
4562 4556 if self.btn_cont_point:
4563 4557 self.btn_cont_point.SetValue(False)
4564 4558 self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point)
4565 4559  
4566   - self.tracker_coord = []
4567   - self.tracker_angles = []
4568   - self.robot_coord = []
4569   - self.robot_angles = []
4570   - self.M_tracker_2_robot = []
4571 4560 self.txt_number.SetLabel('0')
4572 4561  
4573 4562 self.btn_apply_reg.Enable(False)
4574 4563 self.btn_save.Enable(False)
4575 4564 self.btn_ok.Enable(False)
4576 4565  
  4566 + self.matrix_tracker_to_robot = []
  4567 +
4577 4568 def OnApply(self, evt):
4578 4569 if self.btn_cont_point:
4579 4570 self.btn_cont_point.SetValue(False)
4580 4571 self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point)
4581 4572  
4582   - tracker_coord = np.array(self.tracker_coord)
4583   - robot_coord = np.array(self.robot_coord)
4584   -
4585   - M_robot_2_tracker = self.affine_correg(tracker_coord, robot_coord)
4586   - M_tracker_2_robot = tr.inverse_matrix(M_robot_2_tracker)
4587   - self.M_tracker_2_robot = M_tracker_2_robot
  4573 + Publisher.sendMessage('Robot transformation matrix estimation', data=None)
4588 4574  
4589 4575 self.btn_save.Enable(True)
4590 4576 self.btn_ok.Enable(True)
4591 4577  
4592 4578 #TODO: make a colored circle to sinalize that the transformation was made (green) (red if not)
4593 4579  
  4580 + def OnUpdateTransformationMatrix(self, data):
  4581 + self.matrix_tracker_to_robot = np.array(data)
  4582 +
4594 4583 def OnSaveReg(self, evt):
4595 4584 filename = ShowLoadSaveDialog(message=_(u"Save robot transformation file as..."),
4596 4585 wildcard=_("Robot transformation files (*.rbtf)|*.rbtf"),
... ... @@ -4598,10 +4587,10 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4598 4587 default_filename="robottransform.rbtf", save_ext="rbtf")
4599 4588  
4600 4589 if filename:
4601   - if self.M_tracker_2_robot is not None:
  4590 + if self.matrix_tracker_to_robot is not None:
4602 4591 with open(filename, 'w', newline='') as file:
4603 4592 writer = csv.writer(file, delimiter='\t')
4604   - writer.writerows(self.M_tracker_2_robot)
  4593 + writer.writerows(np.vstack(self.matrix_tracker_to_robot).tolist())
4605 4594  
4606 4595 def OnLoadReg(self, evt):
4607 4596 filename = ShowLoadSaveDialog(message=_(u"Load robot transformation"),
... ... @@ -4611,12 +4600,12 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4611 4600 reader = csv.reader(file, delimiter='\t')
4612 4601 content = [row for row in reader]
4613 4602  
4614   - self.M_tracker_2_robot = np.vstack(list(np.float_(content)))
4615   - print("Matrix tracker to robot:", self.M_tracker_2_robot)
4616   - self.btn_ok.Enable(True)
  4603 + self.matrix_tracker_to_robot = np.vstack(list(np.float_(content)))
  4604 + print("Matrix tracker to robot:", self.matrix_tracker_to_robot)
  4605 + Publisher.sendMessage('Load robot transformation matrix', data=self.matrix_tracker_to_robot.tolist())
  4606 + if self.robot_status:
  4607 + self.btn_ok.Enable(True)
4617 4608  
4618   - def GetValue(self):
4619   - return self.M_tracker_2_robot
4620 4609  
4621 4610 class SetNDIconfigs(wx.Dialog):
4622 4611 def __init__(self, title=_("Setting NDI polaris configs:")):
... ...
invesalius/gui/task_navigator.py
  Diff comments   View file @3a9ab9c
... ... @@ -20,10 +20,7 @@
20 20 import dataclasses
21 21 from functools import partial
22 22 import itertools
23   -import csv
24   -import queue
25 23 import time
26   -import threading
27 24  
28 25 import nibabel as nb
29 26 import numpy as np
... ... @@ -41,7 +38,6 @@ except ImportError:
41 38 has_robot = False
42 39  
43 40 import wx
44   -import vtk
45 41  
46 42 try:
47 43 import wx.lib.agw.foldpanelbar as fpb
... ... @@ -51,7 +47,6 @@ except ImportError:
51 47 import wx.lib.colourselect as csel
52 48 import wx.lib.masked.numctrl
53 49 from invesalius.pubsub import pub as Publisher
54   -from time import sleep
55 50  
56 51 import invesalius.constants as const
57 52  
... ... @@ -64,6 +59,7 @@ import invesalius.data.tractography as dti
64 59 import invesalius.data.record_coords as rec
65 60 import invesalius.data.vtk_utils as vtk_utils
66 61 import invesalius.data.bases as db
  62 +import invesalius.data.coregistration as dcr
67 63 import invesalius.gui.dialogs as dlg
68 64 import invesalius.project as prj
69 65 import invesalius.session as ses
... ... @@ -73,7 +69,6 @@ from invesalius.gui import utils as gui_utils
73 69 from invesalius.navigation.icp import ICP
74 70 from invesalius.navigation.navigation import Navigation
75 71 from invesalius.navigation.tracker import Tracker
76   -from invesalius.navigation.robot import Robot
77 72 from invesalius.data.converters import to_vtk
78 73  
79 74 from invesalius.net.neuronavigation_api import NeuronavigationApi
... ... @@ -176,7 +171,7 @@ class InnerFoldPanel(wx.Panel):
176 171 #
177 172 tracker = Tracker()
178 173 pedal_connection = PedalConnection() if HAS_PEDAL_CONNECTION else None
179   -
  174 + icp = ICP()
180 175 neuronavigation_api = NeuronavigationApi()
181 176 navigation = Navigation(
182 177 pedal_connection=pedal_connection,
... ... @@ -194,6 +189,7 @@ class InnerFoldPanel(wx.Panel):
194 189 parent=item,
195 190 navigation=navigation,
196 191 tracker=tracker,
  192 + icp=icp,
197 193 pedal_connection=pedal_connection,
198 194 neuronavigation_api=neuronavigation_api,
199 195 )
... ... @@ -213,7 +209,7 @@ class InnerFoldPanel(wx.Panel):
213 209  
214 210 # Fold 3 - Markers panel
215 211 item = fold_panel.AddFoldPanel(_("Markers"), collapsed=True)
216   - mtw = MarkersPanel(item, navigation, tracker)
  212 + mtw = MarkersPanel(item, navigation, tracker, icp)
217 213  
218 214 fold_panel.ApplyCaptionStyle(item, style)
219 215 fold_panel.AddFoldPanelWindow(item, mtw, spacing= 0,
... ... @@ -352,7 +348,7 @@ class InnerFoldPanel(wx.Panel):
352 348  
353 349  
354 350 class NeuronavigationPanel(wx.Panel):
355   - def __init__(self, parent, navigation, tracker, pedal_connection, neuronavigation_api):
  351 + def __init__(self, parent, navigation, tracker, icp, pedal_connection, neuronavigation_api):
356 352 wx.Panel.__init__(self, parent)
357 353 try:
358 354 default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
... ... @@ -369,9 +365,8 @@ class NeuronavigationPanel(wx.Panel):
369 365 self.neuronavigation_api = neuronavigation_api
370 366  
371 367 self.navigation = navigation
372   - self.icp = ICP()
  368 + self.icp = icp
373 369 self.tracker = tracker
374   - self.robot = Robot(tracker)
375 370  
376 371 self.nav_status = False
377 372 self.tracker_fiducial_being_set = None
... ... @@ -558,6 +553,7 @@ class NeuronavigationPanel(wx.Panel):
558 553 Publisher.subscribe(self.UpdateTarget, 'Update target')
559 554 Publisher.subscribe(self.OnStartNavigation, 'Start navigation')
560 555 Publisher.subscribe(self.OnStopNavigation, 'Stop navigation')
  556 + Publisher.subscribe(self.OnDialogRobotDestroy, 'Dialog robot destroy')
561 557  
562 558 def LoadImageFiducials(self, label, coord):
563 559 fiducial = self.GetFiducialByAttribute(const.IMAGE_FIDUCIALS, 'label', label)
... ... @@ -671,8 +667,6 @@ class NeuronavigationPanel(wx.Panel):
671 667 self.checkbox_icp.SetValue(False)
672 668  
673 669 def OnDisconnectTracker(self):
674   - if self.tracker.tracker_id == const.ROBOT:
675   - self.robot.StopRobotThreadNavigation()
676 670 self.tracker.DisconnectTracker()
677 671 self.ResetICP()
678 672 self.tracker.UpdateUI(self.select_tracker_elem, self.numctrls_fiducial[3:6], self.txtctrl_fre)
... ... @@ -691,7 +685,12 @@ class NeuronavigationPanel(wx.Panel):
691 685  
692 686 self.tracker.SetTracker(choice)
693 687 if self.tracker.tracker_id == const.ROBOT:
694   - self.tracker.ConnectToRobot(self.navigation, self.tracker, self.robot)
  688 + self.dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker)
  689 + if self.dlg_correg_robot.ShowModal() == wx.ID_OK:
  690 + Publisher.sendMessage('Robot navigation mode', robot_mode=True)
  691 + else:
  692 + Publisher.sendMessage('Disconnect tracker')
  693 + wx.MessageBox(_("Not possible to connect to the robot."), _("InVesalius 3"))
695 694  
696 695 self.ResetICP()
697 696 self.tracker.UpdateUI(ctrl, self.numctrls_fiducial[3:6], self.txtctrl_fre)
... ... @@ -776,8 +775,8 @@ class NeuronavigationPanel(wx.Panel):
776 775  
777 776 self.navigation.StopNavigation()
778 777 if self.tracker.tracker_id == const.ROBOT:
779   - Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
780   - m_change_robot_to_head=None)
  778 + Publisher.sendMessage('Update robot target', robot_tracker_flag=False,
  779 + target_index=None, target=None)
781 780  
782 781 # Enable all navigation buttons
783 782 choice_ref.Enable(True)
... ... @@ -786,6 +785,10 @@ class NeuronavigationPanel(wx.Panel):
786 785 for btn_c in self.btns_set_fiducial:
787 786 btn_c.Enable(True)
788 787  
  788 + def OnDialogRobotDestroy(self):
  789 + if self.dlg_correg_robot:
  790 + self.dlg_correg_robot.Destroy()
  791 +
789 792 def CheckFiducialRegistrationError(self):
790 793 self.navigation.UpdateFiducialRegistrationError(self.tracker)
791 794 fre, fre_ok = self.navigation.GetFiducialRegistrationError(self.icp)
... ... @@ -820,7 +823,7 @@ class NeuronavigationPanel(wx.Panel):
820 823 for btn_c in self.btns_set_fiducial:
821 824 btn_c.Enable(False)
822 825  
823   - self.navigation.StartNavigation(self.tracker)
  826 + self.navigation.EstimateTrackerToInVTransformationMatrix(self.tracker)
824 827  
825 828 if not self.CheckFiducialRegistrationError():
826 829 # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok
... ... @@ -830,9 +833,11 @@ class NeuronavigationPanel(wx.Panel):
830 833 if self.icp.use_icp:
831 834 self.checkbox_icp.Enable(True)
832 835 self.checkbox_icp.SetValue(True)
833   - # Update FRE once more after starting the navigation, due to the optional use of ICP,
834   - # which improves FRE.
835   - self.CheckFiducialRegistrationError()
  836 + # Update FRE once more after starting the navigation, due to the optional use of ICP,
  837 + # which improves FRE.
  838 + self.CheckFiducialRegistrationError()
  839 +
  840 + self.navigation.StartNavigation(self.tracker)
836 841  
837 842 def OnNavigate(self, evt, btn_nav):
838 843 select_tracker_elem = self.select_tracker_elem
... ... @@ -881,7 +886,6 @@ class NeuronavigationPanel(wx.Panel):
881 886 )
882 887 self.tracker.__init__()
883 888 self.icp.__init__()
884   - self.robot.__init__(self.tracker)
885 889  
886 890  
887 891 class ObjectRegistrationPanel(wx.Panel):
... ... @@ -1248,20 +1252,7 @@ class MarkersPanel(wx.Panel):
1248 1252 if field.type is bool:
1249 1253 setattr(self, field.name, str_val=='True')
1250 1254  
1251   - @dataclasses.dataclass
1252   - class Robot_Marker:
1253   - """Class for storing robot target."""
1254   - m_robot_target : list = None
1255   -
1256   - @property
1257   - def robot_target_matrix(self):
1258   - return self.m_robot_target
1259   -
1260   - @robot_target_matrix.setter
1261   - def robot_target_matrix(self, new_m_robot_target):
1262   - self.m_robot_target = new_m_robot_target
1263   -
1264   - def __init__(self, parent, navigation, tracker):
  1255 + def __init__(self, parent, navigation, tracker, icp):
1265 1256 wx.Panel.__init__(self, parent)
1266 1257 try:
1267 1258 default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR)
... ... @@ -1273,6 +1264,7 @@ class MarkersPanel(wx.Panel):
1273 1264  
1274 1265 self.navigation = navigation
1275 1266 self.tracker = tracker
  1267 + self.icp = icp
1276 1268  
1277 1269 self.__bind_events()
1278 1270  
... ... @@ -1280,11 +1272,9 @@ class MarkersPanel(wx.Panel):
1280 1272  
1281 1273 self.current_coord = [0, 0, 0, None, None, None]
1282 1274 self.current_seed = 0, 0, 0
1283   - self.current_robot_target_matrix = [None] * 9
  1275 +
1284 1276 self.markers = []
1285   - self.robot_markers = []
1286 1277 self.nav_status = False
1287   - self.raw_target_robot = None, None
1288 1278  
1289 1279 self.marker_colour = const.MARKER_COLOUR
1290 1280 self.marker_size = const.MARKER_SIZE
... ... @@ -1383,7 +1373,6 @@ class MarkersPanel(wx.Panel):
1383 1373 Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status')
1384 1374 Publisher.subscribe(self.UpdateSeedCoordinates, 'Update tracts')
1385 1375 Publisher.subscribe(self.OnChangeCurrentSession, 'Current session changed')
1386   - Publisher.subscribe(self.UpdateRobotCoordinates, 'Update raw coordinates')
1387 1376  
1388 1377 def __find_target_marker(self):
1389 1378 """
... ... @@ -1417,7 +1406,6 @@ class MarkersPanel(wx.Panel):
1417 1406 """
1418 1407 for i in reversed(index):
1419 1408 del self.markers[i]
1420   - del self.robot_markers[i]
1421 1409 self.lc.DeleteItem(i)
1422 1410 for n in range(0, self.lc.GetItemCount()):
1423 1411 self.lc.SetItem(n, 0, str(n + 1))
... ... @@ -1470,34 +1458,37 @@ class MarkersPanel(wx.Panel):
1470 1458 def UpdateSeedCoordinates(self, root=None, affine_vtk=None, coord_offset=(0, 0, 0), coord_offset_w=(0, 0, 0)):
1471 1459 self.current_seed = coord_offset_w
1472 1460  
1473   - def UpdateRobotCoordinates(self, coordinates_raw, markers_flag):
1474   - self.raw_target_robot = coordinates_raw[1], coordinates_raw[2]
1475   -
1476 1461 def OnMouseRightDown(self, evt):
1477 1462 # TODO: Enable the "Set as target" only when target is created with registered object
1478 1463 menu_id = wx.Menu()
1479 1464 edit_id = menu_id.Append(0, _('Edit label'))
1480 1465 menu_id.Bind(wx.EVT_MENU, self.OnMenuEditMarkerLabel, edit_id)
1481   - color_id = menu_id.Append(2, _('Edit color'))
  1466 + color_id = menu_id.Append(1, _('Edit color'))
1482 1467 menu_id.Bind(wx.EVT_MENU, self.OnMenuSetColor, color_id)
1483 1468 menu_id.AppendSeparator()
1484   - target_menu = menu_id.Append(1, _('Set as target'))
  1469 + target_menu = menu_id.Append(2, _('Set as target'))
1485 1470 menu_id.Bind(wx.EVT_MENU, self.OnMenuSetTarget, target_menu)
1486 1471 menu_id.AppendSeparator()
1487   - send_target_to_robot = menu_id.Append(3, _('Send target to robot'))
1488   - menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot)
1489 1472  
1490   - if all([elem is not None for elem in self.markers[self.lc.GetFocusedItem()].coord[3:]]):
  1473 + check_target_angles = all([elem is not None for elem in self.markers[self.lc.GetFocusedItem()].coord[3:]])
  1474 + # Enable "Send target to robot" button only if tracker is robot, if navigation is on and if target is not none
  1475 + if self.tracker.tracker_id == const.ROBOT:
  1476 + send_target_to_robot_compensation = menu_id.Append(3, _('Sets target to robot head move compensation'))
  1477 + menu_id.Bind(wx.EVT_MENU, self.OnMenuSetRobotCompensation, send_target_to_robot_compensation)
  1478 + send_target_to_robot = menu_id.Append(4, _('Send target from InVesalius to robot'))
  1479 + menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot)
  1480 + if self.nav_status and check_target_angles:
  1481 + send_target_to_robot_compensation.Enable(True)
  1482 + send_target_to_robot.Enable(True)
  1483 + else:
  1484 + send_target_to_robot_compensation.Enable(False)
  1485 + send_target_to_robot.Enable(False)
  1486 +
  1487 + if check_target_angles:
1491 1488 target_menu.Enable(True)
1492 1489 else:
1493 1490 target_menu.Enable(False)
1494 1491  
1495   - # Enable "Send target to robot" button only if tracker is robot, if navigation is on and if target is not none
1496   - m_target_robot = np.array([self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix])
1497   - if self.tracker.tracker_id == const.ROBOT and self.nav_status and m_target_robot.any():
1498   - send_target_to_robot.Enable(True)
1499   - else:
1500   - send_target_to_robot.Enable(False)
1501 1492 # TODO: Create the remove target option so the user can disable the target without removing the marker
1502 1493 # target_menu_rem = menu_id.Append(3, _('Remove target'))
1503 1494 # menu_id.Bind(wx.EVT_MENU, self.OnMenuRemoveTarget, target_menu_rem)
... ... @@ -1547,14 +1538,33 @@ class MarkersPanel(wx.Panel):
1547 1538  
1548 1539 Publisher.sendMessage('Set new color', index=index, color=color_new)
1549 1540  
  1541 + def OnMenuSetRobotCompensation(self, evt):
  1542 + if isinstance(evt, int):
  1543 + self.lc.Focus(evt)
  1544 +
  1545 + Publisher.sendMessage('Reset robot process', data=None)
  1546 + matrix_tracker_fiducials = self.tracker.GetMatrixTrackerFiducials()
  1547 + Publisher.sendMessage('Update tracker fiducials matrix',
  1548 + matrix_tracker_fiducials=matrix_tracker_fiducials)
  1549 + Publisher.sendMessage('Update robot target', robot_tracker_flag=True, target_index=self.lc.GetFocusedItem(), target=None)
  1550 +
1550 1551 def OnMenuSendTargetToRobot(self, evt):
1551 1552 if isinstance(evt, int):
1552 1553 self.lc.Focus(evt)
  1554 + index = self.lc.GetFocusedItem()
  1555 + if index == -1:
  1556 + wx.MessageBox(_("No data selected."), _("InVesalius 3"))
  1557 + return
  1558 +
  1559 + Publisher.sendMessage('Reset robot process', data=None)
  1560 + matrix_tracker_fiducials = self.tracker.GetMatrixTrackerFiducials()
  1561 + Publisher.sendMessage('Update tracker fiducials matrix',
  1562 + matrix_tracker_fiducials=matrix_tracker_fiducials)
1553 1563  
1554   - m_target_robot = self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix
  1564 + target_coord = self.markers[index].coord[:3]
  1565 + target = dcr.image_to_tracker(self.navigation.m_change, target_coord, self.icp)
1555 1566  
1556   - Publisher.sendMessage('Reset robot process')
1557   - Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot_to_head=m_target_robot)
  1567 + Publisher.sendMessage('Update robot target', robot_tracker_flag=True, target_index=self.lc.GetFocusedItem(), target=target.tolist())
1558 1568  
1559 1569 def OnDeleteAllMarkers(self, evt=None):
1560 1570 if evt is not None:
... ... @@ -1566,9 +1576,11 @@ class MarkersPanel(wx.Panel):
1566 1576 Publisher.sendMessage('Disable or enable coil tracker', status=False)
1567 1577 if evt is not None:
1568 1578 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
  1579 + if self.tracker.tracker_id == const.ROBOT:
  1580 + Publisher.sendMessage('Update robot target', robot_tracker_flag=False,
  1581 + target_index=None, target=None)
1569 1582  
1570 1583 self.markers = []
1571   - self.robot_markers = []
1572 1584 Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount())
1573 1585 self.lc.DeleteAllItems()
1574 1586 Publisher.sendMessage('Stop Blink Marker', index='DeleteAll')
... ... @@ -1596,8 +1608,8 @@ class MarkersPanel(wx.Panel):
1596 1608 if self.__find_target_marker() in index:
1597 1609 Publisher.sendMessage('Disable or enable coil tracker', status=False)
1598 1610 if self.tracker.tracker_id == const.ROBOT:
1599   - Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
1600   - m_change_robot_to_head=[])
  1611 + Publisher.sendMessage('Update robot target', robot_tracker_flag=False,
  1612 + target_index=None, target=None)
1601 1613 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
1602 1614  
1603 1615 self.__delete_multiple_markers(index)
... ... @@ -1702,12 +1714,11 @@ class MarkersPanel(wx.Panel):
1702 1714 new_marker.session_id = session_id or self.current_session
1703 1715  
1704 1716 if self.tracker.tracker_id == const.ROBOT and self.nav_status:
1705   - self.current_robot_target_matrix = db.compute_robot_to_head_matrix(self.raw_target_robot)
  1717 + current_head_robot_target_status = True
1706 1718 else:
1707   - self.current_robot_target_matrix = [None] * 9
  1719 + current_head_robot_target_status = False
1708 1720  
1709   - new_robot_marker = self.Robot_Marker()
1710   - new_robot_marker.robot_target_matrix = self.current_robot_target_matrix
  1721 + Publisher.sendMessage('Add marker to robot control', data=current_head_robot_target_status)
1711 1722  
1712 1723 # Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added
1713 1724 if all([elem is not None for elem in new_marker.coord[3:]]):
... ... @@ -1723,7 +1734,6 @@ class MarkersPanel(wx.Panel):
1723 1734  
1724 1735  
1725 1736 self.markers.append(new_marker)
1726   - self.robot_markers.append(new_robot_marker)
1727 1737  
1728 1738 # Add item to list control in panel
1729 1739 num_items = self.lc.GetItemCount()
... ...
invesalius/navigation/icp.py
  Diff comments   View file @3a9ab9c
... ... @@ -21,10 +21,10 @@ import wx
21 21  
22 22 import invesalius.data.bases as db
23 23 import invesalius.gui.dialogs as dlg
24   -from invesalius.pubsub import pub as Publisher
25 24  
  25 +from invesalius.utils import Singleton
26 26  
27   -class ICP():
  27 +class ICP(metaclass=Singleton):
28 28 def __init__(self):
29 29 self.use_icp = False
30 30 self.m_icp = None
... ... @@ -35,7 +35,6 @@ class ICP():
35 35  
36 36 if not self.use_icp:
37 37 if dlg.ICPcorregistration(navigation.fre):
38   - Publisher.sendMessage('Stop navigation')
39 38 use_icp, self.m_icp = self.OnICP(navigation, tracker, navigation.m_change)
40 39 if use_icp:
41 40 self.icp_fre = db.calculate_fre(tracker.tracker_fiducials_raw, navigation.all_fiducials,
... ... @@ -43,7 +42,6 @@ class ICP():
43 42 self.SetICP(navigation, use_icp)
44 43 else:
45 44 print("ICP canceled")
46   - Publisher.sendMessage('Start navigation')
47 45  
48 46 def OnICP(self, navigation, tracker, m_change):
49 47 ref_mode_id = navigation.GetReferenceMode()
... ...
invesalius/navigation/navigation.py
  Diff comments   View file @3a9ab9c
... ... @@ -34,6 +34,7 @@ import invesalius.data.tractography as dti
34 34 import invesalius.data.transformations as tr
35 35 import invesalius.data.vtk_utils as vtk_utils
36 36 from invesalius.pubsub import pub as Publisher
  37 +from invesalius.utils import Singleton
37 38  
38 39  
39 40 class QueueCustom(queue.Queue):
... ... @@ -83,7 +84,7 @@ class UpdateNavigationScene(threading.Thread):
83 84  
84 85 threading.Thread.__init__(self, name='UpdateScene')
85 86 self.serial_port_enabled, self.view_tracts, self.peel_loaded = vis_components
86   - self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue = vis_queues
  87 + self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue = vis_queues
87 88 self.sle = sle
88 89 self.event = event
89 90 self.neuronavigation_api = neuronavigation_api
... ... @@ -93,7 +94,7 @@ class UpdateNavigationScene(threading.Thread):
93 94 while not self.event.is_set():
94 95 got_coords = False
95 96 try:
96   - coord, [coordinates_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait()
  97 + coord, markers_flag, m_img, view_obj = self.coord_queue.get_nowait()
97 98 got_coords = True
98 99  
99 100 # print('UpdateScene: get {}'.format(count))
... ... @@ -117,7 +118,6 @@ class UpdateNavigationScene(threading.Thread):
117 118 # see the red cross in the position of the offset marker
118 119 wx.CallAfter(Publisher.sendMessage, 'Update slices position', position=coord[:3])
119 120 wx.CallAfter(Publisher.sendMessage, 'Set cross focal point', position=coord)
120   - wx.CallAfter(Publisher.sendMessage, 'Update raw coordinates', coordinates_raw=coordinates_raw, markers_flag=markers_flag)
121 121 wx.CallAfter(Publisher.sendMessage, 'Update slice viewer')
122 122 wx.CallAfter(Publisher.sendMessage, 'Sensor ID', markers_flag=markers_flag)
123 123  
... ... @@ -140,7 +140,7 @@ class UpdateNavigationScene(threading.Thread):
140 140 self.coord_queue.task_done()
141 141  
142 142  
143   -class Navigation():
  143 +class Navigation(metaclass=Singleton):
144 144 def __init__(self, pedal_connection, neuronavigation_api):
145 145 self.pedal_connection = pedal_connection
146 146 self.neuronavigation_api = neuronavigation_api
... ... @@ -157,7 +157,6 @@ class Navigation():
157 157 self.coord_queue = QueueCustom(maxsize=1)
158 158 self.icp_queue = QueueCustom(maxsize=1)
159 159 self.object_at_target_queue = QueueCustom(maxsize=1)
160   - self.robot_target_queue = QueueCustom(maxsize=1)
161 160 # self.visualization_queue = QueueCustom(maxsize=1)
162 161 self.serial_port_queue = QueueCustom(maxsize=1)
163 162 self.coord_tracts_queue = QueueCustom(maxsize=1)
... ... @@ -245,9 +244,14 @@ class Navigation():
245 244 if state and permission_to_stimulate:
246 245 self.serial_port_connection.SendPulse()
247 246  
248   - def StartNavigation(self, tracker):
  247 + def EstimateTrackerToInVTransformationMatrix(self, tracker):
249 248 tracker_fiducials, tracker_fiducials_raw = tracker.GetTrackerFiducials()
  249 + self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
250 250  
  251 + self.m_change = tr.affine_matrix_from_points(self.all_fiducials[3:, :].T, self.all_fiducials[:3, :].T,
  252 + shear=False, scale=False)
  253 +
  254 + def StartNavigation(self, tracker):
251 255 # initialize jobs list
252 256 jobs_list = []
253 257  
... ... @@ -255,17 +259,9 @@ class Navigation():
255 259 self.event.clear()
256 260  
257 261 vis_components = [self.serial_port_in_use, self.view_tracts, self.peel_loaded]
258   - vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue]
  262 + vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue]
259 263  
260 264 Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components)
261   -
262   - self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials])
263   -
264   - # fiducials matrix
265   - m_change = tr.affine_matrix_from_points(self.all_fiducials[3:, :].T, self.all_fiducials[:3, :].T,
266   - shear=False, scale=False)
267   - self.m_change = m_change
268   -
269 265 errors = False
270 266  
271 267 if self.track_obj:
... ... @@ -279,14 +275,14 @@ class Navigation():
279 275 # obj_reg[0] is object 3x3 fiducial matrix and obj_reg[1] is 3x3 orientation matrix
280 276 obj_fiducials, obj_orients, obj_ref_mode, obj_name = self.obj_reg
281 277  
282   - coreg_data = [m_change, obj_ref_mode]
  278 + coreg_data = [self.m_change, obj_ref_mode]
283 279  
284 280 if self.ref_mode_id:
285 281 coord_raw, markers_flag = tracker.TrackerCoordinates.GetCoordinates()
286 282 else:
287 283 coord_raw = np.array([None])
288 284  
289   - obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
  285 + obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, self.m_change)
290 286 coreg_data.extend(obj_data)
291 287  
292 288 queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.object_at_target_queue]
... ... @@ -295,7 +291,7 @@ class Navigation():
295 291 self.event, self.sleep_nav, tracker.tracker_id,
296 292 self.target))
297 293 else:
298   - coreg_data = (m_change, 0)
  294 + coreg_data = (self.m_change, 0)
299 295 queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue]
300 296 jobs_list.append(dcr.CoordinateCorregistrateNoObject(self.ref_mode_id, tracker, coreg_data,
301 297 self.view_tracts, queues,
... ...
invesalius/navigation/robot.py
View file @f9bfc47
... ... @@ -1,308 +0,0 @@
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   -from time import sleep
24   -
25   -import invesalius.data.bases as db
26   -import invesalius.gui.dialogs as dlg
27   -import invesalius.constants as const
28   -from invesalius.pubsub import pub as Publisher
29   -
30   -try:
31   - import invesalius.data.elfin as elfin
32   - import invesalius.data.elfin_processing as elfin_process
33   - has_robot = True
34   -except ImportError:
35   - has_robot = False
36   -
37   -class Robot():
38   - def __init__(self, tracker):
39   - """
40   - Class to establish the connection between the robot and InVesalius
41   - :param tracker: tracker.py instance
42   - """
43   - self.tracker = tracker
44   - self.trk_init = None
45   - self.robot_target_queue = None
46   - self.object_at_target_queue = None
47   - self.process_tracker = None
48   -
49   - self.thread_robot = None
50   -
51   - self.robotcoordinates = RobotCoordinates()
52   -
53   - self.__bind_events()
54   -
55   - def __bind_events(self):
56   - Publisher.subscribe(self.OnSendCoordinates, 'Send coord to robot')
57   - Publisher.subscribe(self.OnUpdateRobotTargetMatrix, 'Robot target matrix')
58   - Publisher.subscribe(self.OnObjectTarget, 'Coil at target')
59   - Publisher.subscribe(self.OnResetProcessTracker, 'Reset robot process')
60   -
61   - def OnRobotConnection(self):
62   - if not self.tracker.trk_init[0][0][0] or not self.tracker.trk_init[0][1][0]:
63   - dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, self.tracker.trk_init[1])
64   - self.tracker.tracker_id = 0
65   - self.tracker.tracker_connected = False
66   - else:
67   - self.tracker.trk_init.append(self.robotcoordinates)
68   - self.process_tracker = elfin_process.TrackerProcessing()
69   - dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker)
70   - if dlg_correg_robot.ShowModal() == wx.ID_OK:
71   - M_tracker_to_robot = dlg_correg_robot.GetValue()
72   - db.transform_tracker_to_robot.M_tracker_to_robot = M_tracker_to_robot
73   - self.robot_server = self.tracker.trk_init[0][1][0]
74   - self.trk_init = self.tracker.trk_init
75   - else:
76   - dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, 'disconnect')
77   - self.tracker.trk_init = None
78   - self.tracker.tracker_id = 0
79   - self.tracker.tracker_connected = False
80   -
81   - def StartRobotThreadNavigation(self, tracker, coord_queue):
82   - if tracker.event_robot.is_set():
83   - tracker.event_robot.clear()
84   - self.thread_robot = ControlRobot(self.trk_init, tracker, self.robotcoordinates,
85   - [coord_queue, self.robot_target_queue, self.object_at_target_queue],
86   - self.process_tracker, tracker.event_robot)
87   - self.thread_robot.start()
88   -
89   - def StopRobotThreadNavigation(self):
90   - self.thread_robot.join()
91   - self.OnResetProcessTracker()
92   -
93   - def OnResetProcessTracker(self):
94   - self.process_tracker.__init__()
95   -
96   - def OnSendCoordinates(self, coord):
97   - self.robot_server.SendCoordinates(coord)
98   -
99   - def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot_to_head):
100   - try:
101   - self.robot_target_queue.put_nowait([robot_tracker_flag, m_change_robot_to_head])
102   - except queue.Full:
103   - pass
104   -
105   - def OnObjectTarget(self, state):
106   - try:
107   - if self.object_at_target_queue:
108   - self.object_at_target_queue.put_nowait(state)
109   - except queue.Full:
110   - pass
111   -
112   - def SetRobotQueues(self, queues):
113   - self.robot_target_queue, self.object_at_target_queue = queues
114   -
115   -
116   -class RobotCoordinates():
117   - """
118   - Class to set/get robot coordinates. Robot coordinates are acquired in ControlRobot thread.
119   - The class is required to avoid acquisition conflict with different threads (coordinates and navigation)
120   - """
121   - def __init__(self):
122   - self.coord = None
123   -
124   - def SetRobotCoordinates(self, coord):
125   - self.coord = coord
126   -
127   - def GetRobotCoordinates(self):
128   - return self.coord
129   -
130   -
131   -class ControlRobot(threading.Thread):
132   - def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event_robot):
133   - """Class (threading) to perform the robot control.
134   - A distinguish thread is required to increase perform and separate robot control from navigation thread
135   - (safetly layer for robot positining).
136   -
137   - There is no GUI involved, them no Sleep is required
138   -
139   - :param trck_init: Initialization variable of tracking device and connection type. See tracker.py.
140   - :param tracker: tracker.py instance
141   - :param robotcoordinates: RobotCoordinates() instance
142   - :param queues: Queue instance that manage coordinates and robot target
143   - :param process_tracker: TrackerProcessing() instance from elfin_process
144   - :param event_robot: Threading event to ControlRobot when tasks is done
145   - """
146   - threading.Thread.__init__(self, name='ControlRobot')
147   -
148   - self.trck_init_robot = trck_init[0][1][0]
149   - self.trck_init_tracker = trck_init[0][0][0]
150   - self.trk_id = trck_init[1]
151   - self.tracker = tracker
152   - self.robotcoordinates = robotcoordinates
153   - self.robot_tracker_flag = False
154   - self.objattarget_flag = False
155   - self.target_flag = False
156   - self.m_change_robot_to_head = None
157   - self.coord_inv_old = None
158   - self.coord_queue = queues[0]
159   - self.robot_target_queue = queues[1]
160   - self.object_at_target_queue = queues[2]
161   - self.process_tracker = process_tracker
162   - self.event_robot = event_robot
163   - self.arc_motion_flag = False
164   - self.arc_motion_step_flag = None
165   - self.target_linear_out = None
166   - self.target_linear_in = None
167   - self.target_arc = None
168   - self.previous_robot_status = False
169   -
170   - def get_coordinates_from_tracker_devices(self):
171   - coord_robot_raw = self.trck_init_robot.Run()
172   - coord_robot = np.array(coord_robot_raw)
173   - coord_robot[3], coord_robot[5] = coord_robot[5], coord_robot[3]
174   - self.robotcoordinates.SetRobotCoordinates(coord_robot)
175   -
176   - coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
177   -
178   - return coord_raw, coord_robot_raw, markers_flag
179   -
180   - def robot_motion_reset(self):
181   - self.trck_init_robot.StopRobot()
182   - self.arc_motion_flag = False
183   - self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
184   -
185   - def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered):
186   - """
187   - There are two types of robot movements.
188   - We can imagine in two concentric spheres of different sizes. The inside sphere is to compensate for small head movements.
189   - It was named "normal" moves.
190   - The outside sphere is for the arc motion. The arc motion is a safety feature for long robot movements.
191   - Even for a new target or a sudden huge head movement.
192   - 1) normal:
193   - A linear move from the actual position until the target position.
194   - This movement just happens when move distance is below a threshold (const.ROBOT_ARC_THRESHOLD_DISTANCE)
195   - 2) arc motion:
196   - It can be divided into three parts.
197   - The first one represents the movement from the inner sphere to the outer sphere.
198   - The robot moves back using a radial move (it use the center of the head as a reference).
199   - The second step is the actual arc motion (along the outer sphere).
200   - A middle point, between the actual position and the target, is required.
201   - The last step is to make a linear move until the target (goes to the inner sphere)
202   -
203   - """
204   - #Check if the target is inside the working space
205   - if self.process_tracker.estimate_robot_target_length(new_robot_coordinates) < const.ROBOT_WORKING_SPACE:
206   - #Check the target distance to define the motion mode
207   - if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag:
208   - self.trck_init_robot.SendCoordinates(new_robot_coordinates, const.ROBOT_MOTIONS["normal"])
209   -
210   - elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag:
211   - actual_point = current_robot_coordinates
212   - if not self.arc_motion_flag:
213   - head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
214   - current_head_filtered).tolist()
215   -
216   - self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
217   - new_robot_coordinates)
218   - self.arc_motion_flag = True
219   - self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"]
220   -
221   - if self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["linear out"]:
222   - coord = self.target_linear_out
223   - if np.allclose(np.array(actual_point), np.array(self.target_linear_out), 0, 1):
224   - self.arc_motion_step_flag = const.ROBOT_MOTIONS["arc"]
225   - coord = self.target_arc
226   -
227   - elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]:
228   - head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker,
229   - current_head_filtered).tolist()
230   -
231   - _, new_target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates,
232   - new_robot_coordinates)
233   - if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1):
234   - None
235   - else:
236   - if self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) >= \
237   - const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8:
238   - self.target_arc = new_target_arc
239   -
240   - coord = self.target_arc
241   -
242   - if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10):
243   - self.arc_motion_flag = False
244   - self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
245   - coord = new_robot_coordinates
246   -
247   - self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag)
248   - robot_status = True
249   - else:
250   - print("Head is too far from the robot basis")
251   - robot_status = False
252   -
253   - return robot_status
254   -
255   - def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag):
256   - coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1]
257   - marker_head_flag = markers_flag[1]
258   - coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2]
259   - marker_obj_flag = markers_flag[2]
260   - robot_status = False
261   -
262   - if self.robot_tracker_flag:
263   - current_head = coord_head_tracker_in_robot
264   - if current_head is not None and marker_head_flag:
265   - current_head_filtered = self.process_tracker.kalman_filter(current_head)
266   - if self.process_tracker.compute_head_move_threshold(current_head_filtered):
267   - new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered,
268   - self.m_change_robot_to_head)
269   - robot_status = True
270   - if self.coord_inv_old is None:
271   - self.coord_inv_old = new_robot_coordinates
272   -
273   - if np.allclose(np.array(new_robot_coordinates), np.array(current_robot_coordinates), 0, 0.01):
274   - #avoid small movements (0.01 mm)
275   - pass
276   - elif not np.allclose(np.array(new_robot_coordinates), np.array(self.coord_inv_old), 0, 5):
277   - #if the head moves (>5mm) before the robot reach the target
278   - self.trck_init_robot.StopRobot()
279   - self.coord_inv_old = new_robot_coordinates
280   - else:
281   - distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates)
282   - robot_status = self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered)
283   - self.coord_inv_old = new_robot_coordinates
284   - else:
285   - self.trck_init_robot.StopRobot()
286   -
287   - return robot_status
288   -
289   - def run(self):
290   - while not self.event_robot.is_set():
291   - current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices()
292   -
293   - if not self.robot_target_queue.empty():
294   - self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait()
295   - self.robot_motion_reset()
296   - self.robot_target_queue.task_done()
297   -
298   - if not self.object_at_target_queue.empty():
299   - self.target_flag = self.object_at_target_queue.get_nowait()
300   - self.object_at_target_queue.task_done()
301   -
302   - robot_status = self.robot_control(current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag)
303   -
304   - if self.previous_robot_status != robot_status:
305   - wx.CallAfter(Publisher.sendMessage, 'Update robot status', robot_status=robot_status)
306   - self.previous_robot_status = robot_status
307   -
308   - sleep(const.SLEEP_ROBOT)
invesalius/navigation/tracker.py
  Diff comments   View file @3a9ab9c
... ... @@ -42,7 +42,6 @@ class Tracker():
42 42 self.thread_coord = None
43 43  
44 44 self.event_coord = threading.Event()
45   - self.event_robot = threading.Event()
46 45  
47 46 self.TrackerCoordinates = dco.TrackerCoordinates()
48 47  
... ... @@ -78,11 +77,8 @@ class Tracker():
78 77  
79 78 if self.thread_coord:
80 79 self.event_coord.set()
81   - self.event_robot.set()
82 80 self.thread_coord.join()
83 81 self.event_coord.clear()
84   - self.event_robot.clear()
85   -
86 82  
87 83 Publisher.sendMessage('Update status text in GUI',
88 84 label=_("Tracker disconnected"))
... ... @@ -92,13 +88,6 @@ class Tracker():
92 88 label=_("Tracker still connected"))
93 89 print("Tracker still connected!")
94 90  
95   - def ConnectToRobot(self, navigation, tracker, robot):
96   - robot.SetRobotQueues([navigation.robot_target_queue, navigation.object_at_target_queue])
97   - robot.OnRobotConnection()
98   - trk_init_robot = self.trk_init[0][1][0]
99   - if trk_init_robot:
100   - robot.StartRobotThreadNavigation(tracker, navigation.coord_queue)
101   - Publisher.sendMessage('Robot navigation mode', robot_mode=True)
102 91  
103 92 def IsTrackerInitialized(self):
104 93 return self.trk_init and self.tracker_id and self.tracker_connected
... ... @@ -158,7 +147,7 @@ class Tracker():
158 147 m_probe_ref_right = np.linalg.inv(self.m_tracker_fiducials_raw[3]) @ self.m_tracker_fiducials_raw[2]
159 148 m_probe_ref_nasion = np.linalg.inv(self.m_tracker_fiducials_raw[5]) @ self.m_tracker_fiducials_raw[4]
160 149  
161   - return [m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion]
  150 + return [m_probe_ref_left.tolist(), m_probe_ref_right.tolist(), m_probe_ref_nasion.tolist()]
162 151  
163 152 def GetTrackerInfo(self):
164 153 return self.trk_init, self.tracker_id
... ...
invesalius/net/remote_control.py
  Diff comments   View file @3a9ab9c
... ... @@ -79,5 +79,7 @@ class RemoteControl:
79 79 })
80 80 except TypeError:
81 81 pass
  82 + except socketio.exceptions.BadNamespaceError:
  83 + pass
82 84  
83 85 Publisher.add_sendMessage_hook(_emit)
... ...