Commit e9f17e0f0a42074779c66db8a5ce1be68f343d3e
1 parent
aeb03abe
Exists in
master
ADD: robot status for UI
Showing
9 changed files
with
84 additions
and
16 deletions
Show diff stats
invesalius/constants.py
... | ... | @@ -832,6 +832,7 @@ 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.15 |
834 | 834 | SLEEP_COORDINATES = 0.05 |
835 | +SLEEP_ROBOT = 0.01 | |
835 | 836 | |
836 | 837 | BRAIN_OPACITY = 0.5 |
837 | 838 | N_CPU = psutil.cpu_count() | ... | ... |
invesalius/data/coordinates.py
... | ... | @@ -498,8 +498,8 @@ def dynamic_reference_m(probe, reference): |
498 | 498 | |
499 | 499 | |
500 | 500 | def RobotCoord(trk_init, trck_id, ref_mode): |
501 | - coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], trk_init[2], ref_mode) | |
502 | - coord_robot = ElfinCoord(trk_init[1:]) | |
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 | 503 | |
504 | 504 | probe_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[0]) |
505 | 505 | ref_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[1]) | ... | ... |
invesalius/data/elfin.py
invesalius/data/trackers.py
... | ... | @@ -211,13 +211,17 @@ def OptitrackTracker(tracker_id): |
211 | 211 | |
212 | 212 | if trck_init.Initialize(Cal_optitrack, User_profile_optitrack)==0: |
213 | 213 | trck_init.Run() #Runs once Run function, to update cameras. |
214 | + lib_mode = 'wrapper' | |
214 | 215 | else: |
215 | 216 | trck_init = None |
217 | + lib_mode = 'error' | |
216 | 218 | except ImportError: |
219 | + lib_mode = 'error' | |
217 | 220 | print('Error') |
218 | 221 | else: |
222 | + lib_mode = None | |
219 | 223 | print('#####') |
220 | - return trck_init, 'wrapper' | |
224 | + return trck_init, lib_mode | |
221 | 225 | |
222 | 226 | def ElfinRobot(robot_IP): |
223 | 227 | trck_init = None |
... | ... | @@ -230,7 +234,7 @@ def ElfinRobot(robot_IP): |
230 | 234 | print('Connect to elfin robot tracking device.') |
231 | 235 | |
232 | 236 | except: |
233 | - lib_mode = 'disconnect' | |
237 | + lib_mode = 'error' | |
234 | 238 | trck_init = None |
235 | 239 | print('Could not connect to elfin robot tracker.') |
236 | 240 | |
... | ... | @@ -254,7 +258,7 @@ def RobotTracker(tracker_id): |
254 | 258 | trck_init = trck_connection |
255 | 259 | trck_init_robot = ElfinRobot(robot_IP) |
256 | 260 | |
257 | - return [trck_init, trck_init_robot, tracker_id] | |
261 | + return [(trck_init, trck_init_robot), tracker_id] | |
258 | 262 | |
259 | 263 | def DebugTrackerRandom(tracker_id): |
260 | 264 | trck_init = True |
... | ... | @@ -388,7 +392,8 @@ def DisconnectTracker(tracker_id, trck_init): |
388 | 392 | lib_mode = 'serial' |
389 | 393 | print('Tracker disconnected.') |
390 | 394 | elif tracker_id == const.ROBOT: |
391 | - trck_init[0].Close() | |
395 | + trck_init[0][0].Close() | |
396 | + trck_init[1][0].Close() | |
392 | 397 | trck_init = False |
393 | 398 | lib_mode = 'wrapper' |
394 | 399 | print('Tracker disconnected.') | ... | ... |
invesalius/data/viewer_volume.py
... | ... | @@ -191,6 +191,7 @@ class Viewer(wx.Panel): |
191 | 191 | self.target_coord = None |
192 | 192 | self.aim_actor = None |
193 | 193 | self.dummy_coil_actor = None |
194 | + self.dummy_robot_actor = None | |
194 | 195 | self.target_mode = False |
195 | 196 | self.polydata = None |
196 | 197 | self.use_default_object = True |
... | ... | @@ -313,6 +314,10 @@ class Viewer(wx.Panel): |
313 | 314 | Publisher.subscribe(self.load_mask_preview, 'Load mask preview') |
314 | 315 | Publisher.subscribe(self.remove_mask_preview, 'Remove mask preview') |
315 | 316 | |
317 | + # Related to robot tracking during neuronavigation | |
318 | + Publisher.subscribe(self.ActivateRobotMode, 'Robot navigation mode') | |
319 | + Publisher.subscribe(self.OnUpdateRobotStatus, 'Update robot status') | |
320 | + | |
316 | 321 | def SetStereoMode(self, mode): |
317 | 322 | ren_win = self.interactor.GetRenderWindow() |
318 | 323 | |
... | ... | @@ -1681,6 +1686,50 @@ class Viewer(wx.Panel): |
1681 | 1686 | self.actor_tracts = None |
1682 | 1687 | self.Refresh() |
1683 | 1688 | |
1689 | + def ActivateRobotMode(self, robot_mode=None): | |
1690 | + self.robot_mode = robot_mode | |
1691 | + if self.robot_mode: | |
1692 | + self.ren_robot = vtk.vtkRenderer() | |
1693 | + self.ren_robot.SetLayer(1) | |
1694 | + | |
1695 | + self.interactor.GetRenderWindow().AddRenderer(self.ren_robot) | |
1696 | + self.ren_robot.SetViewport(0.02, 0.82, 0.08, 0.92) | |
1697 | + filename = os.path.join(inv_paths.OBJ_DIR, "robot.stl") | |
1698 | + | |
1699 | + reader = vtk.vtkSTLReader() | |
1700 | + reader.SetFileName(filename) | |
1701 | + mapper = vtk.vtkPolyDataMapper() | |
1702 | + mapper.SetInputConnection(reader.GetOutputPort()) | |
1703 | + | |
1704 | + dummy_robot_actor = vtk.vtkActor() | |
1705 | + dummy_robot_actor.SetMapper(mapper) | |
1706 | + dummy_robot_actor.GetProperty().SetColor(1, 1, 1) | |
1707 | + dummy_robot_actor.GetProperty().SetOpacity(1.) | |
1708 | + self.dummy_robot_actor = dummy_robot_actor | |
1709 | + | |
1710 | + self.ren_robot.AddActor(dummy_robot_actor) | |
1711 | + self.ren_robot.InteractiveOff() | |
1712 | + | |
1713 | + self.interactor.Render() | |
1714 | + else: | |
1715 | + self.DisableRobotMode() | |
1716 | + | |
1717 | + def OnUpdateRobotStatus(self, robot_status): | |
1718 | + if self.dummy_robot_actor: | |
1719 | + if robot_status: | |
1720 | + self.dummy_robot_actor.GetProperty().SetColor(0, 1, 0) | |
1721 | + else: | |
1722 | + self.dummy_robot_actor.GetProperty().SetColor(1, 0, 0) | |
1723 | + self.Refresh() | |
1724 | + | |
1725 | + def DisableRobotMode(self): | |
1726 | + try: | |
1727 | + self.interactor.GetRenderWindow().RemoveRenderer(self.ren_robot) | |
1728 | + self.ren.RemoveActor(self.dummy_robot_actor) | |
1729 | + self.interactor.Render() | |
1730 | + except: | |
1731 | + None | |
1732 | + | |
1684 | 1733 | def __bind_events_wx(self): |
1685 | 1734 | #self.Bind(wx.EVT_SIZE, self.OnSize) |
1686 | 1735 | # self.canvas.subscribe_event('LeftButtonPressEvent', self.on_insert_point) | ... | ... |
invesalius/gui/dialogs.py
... | ... | @@ -4540,7 +4540,7 @@ class CreateTransformationMatrixRobot(wx.Dialog): |
4540 | 4540 | def OnCreatePoint(self, evt): |
4541 | 4541 | coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates() |
4542 | 4542 | #robot thread is not initialized yet |
4543 | - coord_raw_robot = self.tracker.trk_init[1][0].Run() | |
4543 | + coord_raw_robot = self.tracker.trk_init[0][1][0].Run() | |
4544 | 4544 | coord_raw_tracker_obj = coord_raw[3] |
4545 | 4545 | |
4546 | 4546 | if markers_flag[2]: | ... | ... |
invesalius/navigation/robot.py
... | ... | @@ -20,6 +20,7 @@ import numpy as np |
20 | 20 | import wx |
21 | 21 | import queue |
22 | 22 | import threading |
23 | +from time import sleep | |
23 | 24 | |
24 | 25 | import invesalius.data.bases as db |
25 | 26 | import invesalius.gui.dialogs as dlg |
... | ... | @@ -58,7 +59,7 @@ class Robot(): |
58 | 59 | Publisher.subscribe(self.OnResetProcessTracker, 'Reset robot process') |
59 | 60 | |
60 | 61 | def OnRobotConnection(self): |
61 | - if not self.tracker.trk_init[0][0] or not self.tracker.trk_init[1][0]: | |
62 | + if not self.tracker.trk_init[0][0][0] or not self.tracker.trk_init[0][1][0]: | |
62 | 63 | dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, self.tracker.trk_init[1]) |
63 | 64 | self.tracker.tracker_id = 0 |
64 | 65 | self.tracker.tracker_connected = False |
... | ... | @@ -69,7 +70,7 @@ class Robot(): |
69 | 70 | if dlg_correg_robot.ShowModal() == wx.ID_OK: |
70 | 71 | M_tracker_to_robot = dlg_correg_robot.GetValue() |
71 | 72 | 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.robot_server = self.tracker.trk_init[0][1][0] | |
73 | 74 | self.trk_init = self.tracker.trk_init |
74 | 75 | else: |
75 | 76 | dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, 'disconnect') |
... | ... | @@ -144,9 +145,9 @@ class ControlRobot(threading.Thread): |
144 | 145 | """ |
145 | 146 | threading.Thread.__init__(self, name='ControlRobot') |
146 | 147 | |
147 | - self.trck_init_robot = trck_init[1][0] | |
148 | - self.trck_init_tracker = trck_init[0] | |
149 | - self.trk_id = trck_init[2] | |
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] | |
150 | 151 | self.tracker = tracker |
151 | 152 | self.robotcoordinates = robotcoordinates |
152 | 153 | self.robot_tracker_flag = False |
... | ... | @@ -243,14 +244,19 @@ class ControlRobot(threading.Thread): |
243 | 244 | coord = new_robot_coordinates |
244 | 245 | |
245 | 246 | self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag) |
247 | + robot_status = True | |
246 | 248 | else: |
247 | 249 | print("Head is too far from the robot basis") |
250 | + robot_status = False | |
251 | + | |
252 | + return robot_status | |
248 | 253 | |
249 | 254 | def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag): |
250 | 255 | coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1] |
251 | 256 | marker_head_flag = markers_flag[1] |
252 | 257 | coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2] |
253 | 258 | marker_obj_flag = markers_flag[2] |
259 | + robot_status = False | |
254 | 260 | |
255 | 261 | if self.robot_tracker_flag: |
256 | 262 | current_head = coord_head_tracker_in_robot |
... | ... | @@ -259,6 +265,7 @@ class ControlRobot(threading.Thread): |
259 | 265 | if self.process_tracker.compute_head_move_threshold(current_head_filtered): |
260 | 266 | new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered, |
261 | 267 | self.m_change_robot_to_head) |
268 | + robot_status = True | |
262 | 269 | if self.coord_inv_old is None: |
263 | 270 | self.coord_inv_old = new_robot_coordinates |
264 | 271 | |
... | ... | @@ -271,11 +278,13 @@ class ControlRobot(threading.Thread): |
271 | 278 | self.coord_inv_old = new_robot_coordinates |
272 | 279 | else: |
273 | 280 | distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) |
274 | - self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered) | |
281 | + robot_status = self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered) | |
275 | 282 | self.coord_inv_old = new_robot_coordinates |
276 | 283 | else: |
277 | 284 | self.trck_init_robot.StopRobot() |
278 | 285 | |
286 | + wx.CallAfter(Publisher.sendMessage, 'Update robot status', robot_status=robot_status) | |
287 | + | |
279 | 288 | def run(self): |
280 | 289 | while not self.event_robot.is_set(): |
281 | 290 | current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices() |
... | ... | @@ -290,3 +299,4 @@ class ControlRobot(threading.Thread): |
290 | 299 | self.object_at_target_queue.task_done() |
291 | 300 | |
292 | 301 | self.robot_control(current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag) |
302 | + sleep(const.SLEEP_ROBOT) | ... | ... |
invesalius/navigation/tracker.py
... | ... | @@ -51,7 +51,7 @@ class Tracker(): |
51 | 51 | self.DisconnectTracker() |
52 | 52 | |
53 | 53 | self.trk_init = dt.TrackerConnection(new_tracker, None, 'connect') |
54 | - if not self.trk_init[0]: | |
54 | + if not all(list(self.trk_init)): | |
55 | 55 | dlg.ShowNavigationTrackerWarning(self.tracker_id, self.trk_init[1]) |
56 | 56 | |
57 | 57 | self.tracker_id = 0 |
... | ... | @@ -70,6 +70,7 @@ class Tracker(): |
70 | 70 | label=_("Disconnecting tracker ...")) |
71 | 71 | Publisher.sendMessage('Remove sensors ID') |
72 | 72 | Publisher.sendMessage('Remove object data') |
73 | + Publisher.sendMessage('Robot navigation mode', robot_mode=False) | |
73 | 74 | self.trk_init = dt.TrackerConnection(self.tracker_id, self.trk_init[0], 'disconnect') |
74 | 75 | if not self.trk_init[0]: |
75 | 76 | self.tracker_connected = False |
... | ... | @@ -94,9 +95,10 @@ class Tracker(): |
94 | 95 | def ConnectToRobot(self, navigation, tracker, robot): |
95 | 96 | robot.SetRobotQueues([navigation.robot_target_queue, navigation.object_at_target_queue]) |
96 | 97 | robot.OnRobotConnection() |
97 | - trk_init_robot = self.trk_init[1][0] | |
98 | + trk_init_robot = self.trk_init[0][1][0] | |
98 | 99 | if trk_init_robot: |
99 | 100 | robot.StartRobotThreadNavigation(tracker, navigation.coord_queue) |
101 | + Publisher.sendMessage('Robot navigation mode', robot_mode=True) | |
100 | 102 | |
101 | 103 | def IsTrackerInitialized(self): |
102 | 104 | return self.trk_init and self.tracker_id and self.tracker_connected | ... | ... |
No preview for this file type