Download as
Browse Code »

Commit 2d8f59df639243f63561d7ace929ed117e06c8d9

Authored by Victor Hugo Souza
Committed by GitHub
2 parents c5e45cf4 0f686335
Exists in master

Merge pull request #402 from rmatsuda/robot-status-UI 

Added robot status indicator in volume VTK render
Showing 9 changed files with 92 additions and 18 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @2d8f59d
... ... @@ -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
  Diff comments   View file @2d8f59d
... ... @@ -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
  Diff comments   View file @2d8f59d
... ... @@ -45,7 +45,8 @@ class Elfin_Server():
45 45 sleep(0.1)
46 46  
47 47 def Close(self):
48   - self.cobot.close()
  48 + self.StopRobot()
  49 + #TODO: robot function to close? self.cobot.close()
49 50  
50 51 class Elfin:
51 52 def __init__(self):
... ...
invesalius/data/trackers.py
  Diff comments   View file @2d8f59d
... ... @@ -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
  Diff comments   View file @2d8f59d
... ... @@ -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,47 @@ class Viewer(wx.Panel):
1681 1686 self.actor_tracts = None
1682 1687 self.Refresh()
1683 1688  
  1689 + def ActivateRobotMode(self, robot_mode=None):
  1690 + if robot_mode:
  1691 + self.ren_robot = vtk.vtkRenderer()
  1692 + self.ren_robot.SetLayer(1)
  1693 +
  1694 + self.interactor.GetRenderWindow().AddRenderer(self.ren_robot)
  1695 + self.ren_robot.SetViewport(0.02, 0.82, 0.08, 0.92)
  1696 + filename = os.path.join(inv_paths.OBJ_DIR, "robot.stl")
  1697 +
  1698 + reader = vtk.vtkSTLReader()
  1699 + reader.SetFileName(filename)
  1700 + mapper = vtk.vtkPolyDataMapper()
  1701 + mapper.SetInputConnection(reader.GetOutputPort())
  1702 +
  1703 + dummy_robot_actor = vtk.vtkActor()
  1704 + dummy_robot_actor.SetMapper(mapper)
  1705 + dummy_robot_actor.GetProperty().SetColor(1, 1, 1)
  1706 + dummy_robot_actor.GetProperty().SetOpacity(1.)
  1707 + self.dummy_robot_actor = dummy_robot_actor
  1708 +
  1709 + self.ren_robot.AddActor(dummy_robot_actor)
  1710 + self.ren_robot.InteractiveOff()
  1711 +
  1712 + self.interactor.Render()
  1713 + else:
  1714 + self.DisableRobotMode()
  1715 +
  1716 + def OnUpdateRobotStatus(self, robot_status):
  1717 + if self.dummy_robot_actor:
  1718 + if robot_status:
  1719 + self.dummy_robot_actor.GetProperty().SetColor(0, 1, 0)
  1720 + else:
  1721 + self.dummy_robot_actor.GetProperty().SetColor(1, 0, 0)
  1722 + self.Refresh()
  1723 +
  1724 + def DisableRobotMode(self):
  1725 + if self.dummy_robot_actor:
  1726 + self.ren_robot.RemoveActor(self.dummy_robot_actor)
  1727 + self.interactor.GetRenderWindow().RemoveRenderer(self.ren_robot)
  1728 + self.interactor.Render()
  1729 +
1684 1730 def __bind_events_wx(self):
1685 1731 #self.Bind(wx.EVT_SIZE, self.OnSize)
1686 1732 # self.canvas.subscribe_event('LeftButtonPressEvent', self.on_insert_point)
... ...
invesalius/gui/dialogs.py
  Diff comments   View file @2d8f59d
... ... @@ -4323,7 +4323,10 @@ class SetTrackerDeviceToRobot(wx.Dialog):
4323 4323 def _init_gui(self):
4324 4324 # ComboBox for spatial tracker device selection
4325 4325 tooltip = wx.ToolTip(_("Choose the tracking device"))
4326   - tracker_options = [_("Select tracker:")] + const.TRACKERS[:-3]
  4326 + trackers = const.TRACKERS
  4327 + if not ses.Session().debug:
  4328 + del trackers[-3:]
  4329 + tracker_options = [_("Select tracker:")] + trackers
4327 4330 choice_trck = wx.ComboBox(self, -1, "",
4328 4331 choices=tracker_options, style=wx.CB_DROPDOWN | wx.CB_READONLY)
4329 4332 choice_trck.SetToolTip(tooltip)
... ... @@ -4540,7 +4543,7 @@ class CreateTransformationMatrixRobot(wx.Dialog):
4540 4543 def OnCreatePoint(self, evt):
4541 4544 coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
4542 4545 #robot thread is not initialized yet
4543   - coord_raw_robot = self.tracker.trk_init[1][0].Run()
  4546 + coord_raw_robot = self.tracker.trk_init[0][1][0].Run()
4544 4547 coord_raw_tracker_obj = coord_raw[3]
4545 4548  
4546 4549 if markers_flag[2]:
... ...
invesalius/navigation/robot.py
  Diff comments   View file @2d8f59d
... ... @@ -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
... ... @@ -164,6 +165,7 @@ class ControlRobot(threading.Thread):
164 165 self.target_linear_out = None
165 166 self.target_linear_in = None
166 167 self.target_arc = None
  168 + self.previous_robot_status = False
167 169  
168 170 def get_coordinates_from_tracker_devices(self):
169 171 coord_robot_raw = self.trck_init_robot.Run()
... ... @@ -243,14 +245,19 @@ class ControlRobot(threading.Thread):
243 245 coord = new_robot_coordinates
244 246  
245 247 self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag)
  248 + robot_status = True
246 249 else:
247 250 print("Head is too far from the robot basis")
  251 + robot_status = False
  252 +
  253 + return robot_status
248 254  
249 255 def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag):
250 256 coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1]
251 257 marker_head_flag = markers_flag[1]
252 258 coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2]
253 259 marker_obj_flag = markers_flag[2]
  260 + robot_status = False
254 261  
255 262 if self.robot_tracker_flag:
256 263 current_head = coord_head_tracker_in_robot
... ... @@ -259,6 +266,7 @@ class ControlRobot(threading.Thread):
259 266 if self.process_tracker.compute_head_move_threshold(current_head_filtered):
260 267 new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered,
261 268 self.m_change_robot_to_head)
  269 + robot_status = True
262 270 if self.coord_inv_old is None:
263 271 self.coord_inv_old = new_robot_coordinates
264 272  
... ... @@ -271,11 +279,13 @@ class ControlRobot(threading.Thread):
271 279 self.coord_inv_old = new_robot_coordinates
272 280 else:
273 281 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)
  282 + robot_status = self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered)
275 283 self.coord_inv_old = new_robot_coordinates
276 284 else:
277 285 self.trck_init_robot.StopRobot()
278 286  
  287 + return robot_status
  288 +
279 289 def run(self):
280 290 while not self.event_robot.is_set():
281 291 current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices()
... ... @@ -289,4 +299,10 @@ class ControlRobot(threading.Thread):
289 299 self.target_flag = self.object_at_target_queue.get_nowait()
290 300 self.object_at_target_queue.task_done()
291 301  
292   - self.robot_control(current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag)
  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 @2d8f59d
... ... @@ -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
... ...
navigation/objects/robot.stl 0 → 100644
View file @2d8f59d
No preview for this file type