Commit dfd7fc1e0e4f3535d9cb3ebc9ba26af9ae1ce13c
Committed by
GitHub
Exists in
master
Merge branch 'invesalius:master' into 278_add_visualization_for_the_direction_of_the_marker
Showing
10 changed files
with
110 additions
and
23 deletions
Show diff stats
invesalius/constants.py
@@ -832,6 +832,7 @@ SEED_RADIUS = 1.5 | @@ -832,6 +832,7 @@ SEED_RADIUS = 1.5 | ||
832 | # Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation. | 832 | # Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation. |
833 | SLEEP_NAVIGATION = 0.15 | 833 | SLEEP_NAVIGATION = 0.15 |
834 | SLEEP_COORDINATES = 0.05 | 834 | SLEEP_COORDINATES = 0.05 |
835 | +SLEEP_ROBOT = 0.01 | ||
835 | 836 | ||
836 | BRAIN_OPACITY = 0.5 | 837 | BRAIN_OPACITY = 0.5 |
837 | N_CPU = psutil.cpu_count() | 838 | N_CPU = psutil.cpu_count() |
@@ -858,3 +859,7 @@ ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm | @@ -858,3 +859,7 @@ ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm | ||
858 | ROBOT_VERSOR_SCALE_FACTOR = 70 | 859 | ROBOT_VERSOR_SCALE_FACTOR = 70 |
859 | #Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%. | 860 | #Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%. |
860 | ROBOT_WORKING_SPACE = 760 #mm | 861 | ROBOT_WORKING_SPACE = 760 #mm |
862 | +ROBOT_MOVE_STATE = {"free to move": 0, | ||
863 | + "in motion": 1009, | ||
864 | + "waiting for execution": 1013, | ||
865 | + "error": 1025} |
invesalius/data/coordinates.py
@@ -498,8 +498,8 @@ def dynamic_reference_m(probe, reference): | @@ -498,8 +498,8 @@ def dynamic_reference_m(probe, reference): | ||
498 | 498 | ||
499 | 499 | ||
500 | def RobotCoord(trk_init, trck_id, ref_mode): | 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 | probe_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[0]) | 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]) | 505 | ref_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[1]) |
invesalius/data/elfin.py
@@ -24,17 +24,19 @@ class Elfin_Server(): | @@ -24,17 +24,19 @@ class Elfin_Server(): | ||
24 | def Run(self): | 24 | def Run(self): |
25 | return self.cobot.ReadPcsActualPos() | 25 | return self.cobot.ReadPcsActualPos() |
26 | 26 | ||
27 | - def SendCoordinates(self, target, type=const.ROBOT_MOTIONS["normal"]): | 27 | + def SendCoordinates(self, target, motion_type=const.ROBOT_MOTIONS["normal"]): |
28 | """ | 28 | """ |
29 | It's not possible to send a move command to elfin if the robot is during a move. | 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. | 30 | Status 1009 means robot in motion. |
31 | """ | 31 | """ |
32 | status = self.cobot.ReadMoveState() | 32 | status = self.cobot.ReadMoveState() |
33 | - if status != 1009: | ||
34 | - if type == const.ROBOT_MOTIONS["normal"] or type == const.ROBOT_MOTIONS["linear out"]: | 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) | 35 | self.cobot.MoveL(target) |
36 | - elif type == const.ROBOT_MOTIONS["arc"]: | 36 | + elif motion_type == const.ROBOT_MOTIONS["arc"]: |
37 | self.cobot.MoveC(target) | 37 | self.cobot.MoveC(target) |
38 | + elif status == const.ROBOT_MOVE_STATE["error"]: | ||
39 | + self.StopRobot() | ||
38 | 40 | ||
39 | def StopRobot(self): | 41 | def StopRobot(self): |
40 | # Takes some microseconds to the robot actual stops after the command. | 42 | # Takes some microseconds to the robot actual stops after the command. |
@@ -43,7 +45,8 @@ class Elfin_Server(): | @@ -43,7 +45,8 @@ class Elfin_Server(): | ||
43 | sleep(0.1) | 45 | sleep(0.1) |
44 | 46 | ||
45 | def Close(self): | 47 | def Close(self): |
46 | - self.cobot.close() | 48 | + self.StopRobot() |
49 | + #TODO: robot function to close? self.cobot.close() | ||
47 | 50 | ||
48 | class Elfin: | 51 | class Elfin: |
49 | def __init__(self): | 52 | def __init__(self): |
invesalius/data/trackers.py
@@ -211,13 +211,17 @@ def OptitrackTracker(tracker_id): | @@ -211,13 +211,17 @@ def OptitrackTracker(tracker_id): | ||
211 | 211 | ||
212 | if trck_init.Initialize(Cal_optitrack, User_profile_optitrack)==0: | 212 | if trck_init.Initialize(Cal_optitrack, User_profile_optitrack)==0: |
213 | trck_init.Run() #Runs once Run function, to update cameras. | 213 | trck_init.Run() #Runs once Run function, to update cameras. |
214 | + lib_mode = 'wrapper' | ||
214 | else: | 215 | else: |
215 | trck_init = None | 216 | trck_init = None |
217 | + lib_mode = 'error' | ||
216 | except ImportError: | 218 | except ImportError: |
219 | + lib_mode = 'error' | ||
217 | print('Error') | 220 | print('Error') |
218 | else: | 221 | else: |
222 | + lib_mode = None | ||
219 | print('#####') | 223 | print('#####') |
220 | - return trck_init, 'wrapper' | 224 | + return trck_init, lib_mode |
221 | 225 | ||
222 | def ElfinRobot(robot_IP): | 226 | def ElfinRobot(robot_IP): |
223 | trck_init = None | 227 | trck_init = None |
@@ -230,7 +234,7 @@ def ElfinRobot(robot_IP): | @@ -230,7 +234,7 @@ def ElfinRobot(robot_IP): | ||
230 | print('Connect to elfin robot tracking device.') | 234 | print('Connect to elfin robot tracking device.') |
231 | 235 | ||
232 | except: | 236 | except: |
233 | - lib_mode = 'disconnect' | 237 | + lib_mode = 'error' |
234 | trck_init = None | 238 | trck_init = None |
235 | print('Could not connect to elfin robot tracker.') | 239 | print('Could not connect to elfin robot tracker.') |
236 | 240 | ||
@@ -254,7 +258,7 @@ def RobotTracker(tracker_id): | @@ -254,7 +258,7 @@ def RobotTracker(tracker_id): | ||
254 | trck_init = trck_connection | 258 | trck_init = trck_connection |
255 | trck_init_robot = ElfinRobot(robot_IP) | 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 | def DebugTrackerRandom(tracker_id): | 263 | def DebugTrackerRandom(tracker_id): |
260 | trck_init = True | 264 | trck_init = True |
@@ -388,7 +392,8 @@ def DisconnectTracker(tracker_id, trck_init): | @@ -388,7 +392,8 @@ def DisconnectTracker(tracker_id, trck_init): | ||
388 | lib_mode = 'serial' | 392 | lib_mode = 'serial' |
389 | print('Tracker disconnected.') | 393 | print('Tracker disconnected.') |
390 | elif tracker_id == const.ROBOT: | 394 | elif tracker_id == const.ROBOT: |
391 | - trck_init[0].Close() | 395 | + trck_init[0][0].Close() |
396 | + trck_init[1][0].Close() | ||
392 | trck_init = False | 397 | trck_init = False |
393 | lib_mode = 'wrapper' | 398 | lib_mode = 'wrapper' |
394 | print('Tracker disconnected.') | 399 | print('Tracker disconnected.') |
invesalius/data/viewer_volume.py
@@ -191,6 +191,7 @@ class Viewer(wx.Panel): | @@ -191,6 +191,7 @@ class Viewer(wx.Panel): | ||
191 | self.target_coord = None | 191 | self.target_coord = None |
192 | self.aim_actor = None | 192 | self.aim_actor = None |
193 | self.dummy_coil_actor = None | 193 | self.dummy_coil_actor = None |
194 | + self.dummy_robot_actor = None | ||
194 | self.target_mode = False | 195 | self.target_mode = False |
195 | self.polydata = None | 196 | self.polydata = None |
196 | self.use_default_object = True | 197 | self.use_default_object = True |
@@ -314,6 +315,10 @@ class Viewer(wx.Panel): | @@ -314,6 +315,10 @@ class Viewer(wx.Panel): | ||
314 | Publisher.subscribe(self.load_mask_preview, 'Load mask preview') | 315 | Publisher.subscribe(self.load_mask_preview, 'Load mask preview') |
315 | Publisher.subscribe(self.remove_mask_preview, 'Remove mask preview') | 316 | Publisher.subscribe(self.remove_mask_preview, 'Remove mask preview') |
316 | 317 | ||
318 | + # Related to robot tracking during neuronavigation | ||
319 | + Publisher.subscribe(self.ActivateRobotMode, 'Robot navigation mode') | ||
320 | + Publisher.subscribe(self.OnUpdateRobotStatus, 'Update robot status') | ||
321 | + | ||
317 | def SetStereoMode(self, mode): | 322 | def SetStereoMode(self, mode): |
318 | ren_win = self.interactor.GetRenderWindow() | 323 | ren_win = self.interactor.GetRenderWindow() |
319 | 324 | ||
@@ -1698,6 +1703,47 @@ class Viewer(wx.Panel): | @@ -1698,6 +1703,47 @@ class Viewer(wx.Panel): | ||
1698 | self.actor_tracts = None | 1703 | self.actor_tracts = None |
1699 | self.Refresh() | 1704 | self.Refresh() |
1700 | 1705 | ||
1706 | + def ActivateRobotMode(self, robot_mode=None): | ||
1707 | + if robot_mode: | ||
1708 | + self.ren_robot = vtk.vtkRenderer() | ||
1709 | + self.ren_robot.SetLayer(1) | ||
1710 | + | ||
1711 | + self.interactor.GetRenderWindow().AddRenderer(self.ren_robot) | ||
1712 | + self.ren_robot.SetViewport(0.02, 0.82, 0.08, 0.92) | ||
1713 | + filename = os.path.join(inv_paths.OBJ_DIR, "robot.stl") | ||
1714 | + | ||
1715 | + reader = vtk.vtkSTLReader() | ||
1716 | + reader.SetFileName(filename) | ||
1717 | + mapper = vtk.vtkPolyDataMapper() | ||
1718 | + mapper.SetInputConnection(reader.GetOutputPort()) | ||
1719 | + | ||
1720 | + dummy_robot_actor = vtk.vtkActor() | ||
1721 | + dummy_robot_actor.SetMapper(mapper) | ||
1722 | + dummy_robot_actor.GetProperty().SetColor(1, 1, 1) | ||
1723 | + dummy_robot_actor.GetProperty().SetOpacity(1.) | ||
1724 | + self.dummy_robot_actor = dummy_robot_actor | ||
1725 | + | ||
1726 | + self.ren_robot.AddActor(dummy_robot_actor) | ||
1727 | + self.ren_robot.InteractiveOff() | ||
1728 | + | ||
1729 | + self.interactor.Render() | ||
1730 | + else: | ||
1731 | + self.DisableRobotMode() | ||
1732 | + | ||
1733 | + def OnUpdateRobotStatus(self, robot_status): | ||
1734 | + if self.dummy_robot_actor: | ||
1735 | + if robot_status: | ||
1736 | + self.dummy_robot_actor.GetProperty().SetColor(0, 1, 0) | ||
1737 | + else: | ||
1738 | + self.dummy_robot_actor.GetProperty().SetColor(1, 0, 0) | ||
1739 | + self.Refresh() | ||
1740 | + | ||
1741 | + def DisableRobotMode(self): | ||
1742 | + if self.dummy_robot_actor: | ||
1743 | + self.ren_robot.RemoveActor(self.dummy_robot_actor) | ||
1744 | + self.interactor.GetRenderWindow().RemoveRenderer(self.ren_robot) | ||
1745 | + self.interactor.Render() | ||
1746 | + | ||
1701 | def __bind_events_wx(self): | 1747 | def __bind_events_wx(self): |
1702 | #self.Bind(wx.EVT_SIZE, self.OnSize) | 1748 | #self.Bind(wx.EVT_SIZE, self.OnSize) |
1703 | # self.canvas.subscribe_event('LeftButtonPressEvent', self.on_insert_point) | 1749 | # self.canvas.subscribe_event('LeftButtonPressEvent', self.on_insert_point) |
invesalius/gui/dialogs.py
@@ -4323,7 +4323,10 @@ class SetTrackerDeviceToRobot(wx.Dialog): | @@ -4323,7 +4323,10 @@ class SetTrackerDeviceToRobot(wx.Dialog): | ||
4323 | def _init_gui(self): | 4323 | def _init_gui(self): |
4324 | # ComboBox for spatial tracker device selection | 4324 | # ComboBox for spatial tracker device selection |
4325 | tooltip = wx.ToolTip(_("Choose the tracking device")) | 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 | choice_trck = wx.ComboBox(self, -1, "", | 4330 | choice_trck = wx.ComboBox(self, -1, "", |
4328 | choices=tracker_options, style=wx.CB_DROPDOWN | wx.CB_READONLY) | 4331 | choices=tracker_options, style=wx.CB_DROPDOWN | wx.CB_READONLY) |
4329 | choice_trck.SetToolTip(tooltip) | 4332 | choice_trck.SetToolTip(tooltip) |
@@ -4540,7 +4543,7 @@ class CreateTransformationMatrixRobot(wx.Dialog): | @@ -4540,7 +4543,7 @@ class CreateTransformationMatrixRobot(wx.Dialog): | ||
4540 | def OnCreatePoint(self, evt): | 4543 | def OnCreatePoint(self, evt): |
4541 | coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates() | 4544 | coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates() |
4542 | #robot thread is not initialized yet | 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 | coord_raw_tracker_obj = coord_raw[3] | 4547 | coord_raw_tracker_obj = coord_raw[3] |
4545 | 4548 | ||
4546 | if markers_flag[2]: | 4549 | if markers_flag[2]: |
invesalius/navigation/robot.py
@@ -20,6 +20,7 @@ import numpy as np | @@ -20,6 +20,7 @@ import numpy as np | ||
20 | import wx | 20 | import wx |
21 | import queue | 21 | import queue |
22 | import threading | 22 | import threading |
23 | +from time import sleep | ||
23 | 24 | ||
24 | import invesalius.data.bases as db | 25 | import invesalius.data.bases as db |
25 | import invesalius.gui.dialogs as dlg | 26 | import invesalius.gui.dialogs as dlg |
@@ -58,7 +59,7 @@ class Robot(): | @@ -58,7 +59,7 @@ class Robot(): | ||
58 | Publisher.subscribe(self.OnResetProcessTracker, 'Reset robot process') | 59 | Publisher.subscribe(self.OnResetProcessTracker, 'Reset robot process') |
59 | 60 | ||
60 | def OnRobotConnection(self): | 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 | dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, self.tracker.trk_init[1]) | 63 | dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, self.tracker.trk_init[1]) |
63 | self.tracker.tracker_id = 0 | 64 | self.tracker.tracker_id = 0 |
64 | self.tracker.tracker_connected = False | 65 | self.tracker.tracker_connected = False |
@@ -69,7 +70,7 @@ class Robot(): | @@ -69,7 +70,7 @@ class Robot(): | ||
69 | if dlg_correg_robot.ShowModal() == wx.ID_OK: | 70 | if dlg_correg_robot.ShowModal() == wx.ID_OK: |
70 | M_tracker_to_robot = dlg_correg_robot.GetValue() | 71 | M_tracker_to_robot = dlg_correg_robot.GetValue() |
71 | db.transform_tracker_to_robot.M_tracker_to_robot = M_tracker_to_robot | 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 | self.trk_init = self.tracker.trk_init | 74 | self.trk_init = self.tracker.trk_init |
74 | else: | 75 | else: |
75 | dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, 'disconnect') | 76 | dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, 'disconnect') |
@@ -144,9 +145,9 @@ class ControlRobot(threading.Thread): | @@ -144,9 +145,9 @@ class ControlRobot(threading.Thread): | ||
144 | """ | 145 | """ |
145 | threading.Thread.__init__(self, name='ControlRobot') | 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 | self.tracker = tracker | 151 | self.tracker = tracker |
151 | self.robotcoordinates = robotcoordinates | 152 | self.robotcoordinates = robotcoordinates |
152 | self.robot_tracker_flag = False | 153 | self.robot_tracker_flag = False |
@@ -164,6 +165,7 @@ class ControlRobot(threading.Thread): | @@ -164,6 +165,7 @@ class ControlRobot(threading.Thread): | ||
164 | self.target_linear_out = None | 165 | self.target_linear_out = None |
165 | self.target_linear_in = None | 166 | self.target_linear_in = None |
166 | self.target_arc = None | 167 | self.target_arc = None |
168 | + self.previous_robot_status = False | ||
167 | 169 | ||
168 | def get_coordinates_from_tracker_devices(self): | 170 | def get_coordinates_from_tracker_devices(self): |
169 | coord_robot_raw = self.trck_init_robot.Run() | 171 | coord_robot_raw = self.trck_init_robot.Run() |
@@ -175,6 +177,11 @@ class ControlRobot(threading.Thread): | @@ -175,6 +177,11 @@ class ControlRobot(threading.Thread): | ||
175 | 177 | ||
176 | return coord_raw, coord_robot_raw, markers_flag | 178 | return coord_raw, coord_robot_raw, markers_flag |
177 | 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 | + | ||
178 | def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered): | 185 | def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered): |
179 | """ | 186 | """ |
180 | There are two types of robot movements. | 187 | There are two types of robot movements. |
@@ -238,14 +245,19 @@ class ControlRobot(threading.Thread): | @@ -238,14 +245,19 @@ class ControlRobot(threading.Thread): | ||
238 | coord = new_robot_coordinates | 245 | coord = new_robot_coordinates |
239 | 246 | ||
240 | self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag) | 247 | self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag) |
248 | + robot_status = True | ||
241 | else: | 249 | else: |
242 | print("Head is too far from the robot basis") | 250 | print("Head is too far from the robot basis") |
251 | + robot_status = False | ||
252 | + | ||
253 | + return robot_status | ||
243 | 254 | ||
244 | def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag): | 255 | def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag): |
245 | coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1] | 256 | coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1] |
246 | marker_head_flag = markers_flag[1] | 257 | marker_head_flag = markers_flag[1] |
247 | coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2] | 258 | coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2] |
248 | marker_obj_flag = markers_flag[2] | 259 | marker_obj_flag = markers_flag[2] |
260 | + robot_status = False | ||
249 | 261 | ||
250 | if self.robot_tracker_flag: | 262 | if self.robot_tracker_flag: |
251 | current_head = coord_head_tracker_in_robot | 263 | current_head = coord_head_tracker_in_robot |
@@ -254,6 +266,7 @@ class ControlRobot(threading.Thread): | @@ -254,6 +266,7 @@ class ControlRobot(threading.Thread): | ||
254 | if self.process_tracker.compute_head_move_threshold(current_head_filtered): | 266 | if self.process_tracker.compute_head_move_threshold(current_head_filtered): |
255 | new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered, | 267 | new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered, |
256 | self.m_change_robot_to_head) | 268 | self.m_change_robot_to_head) |
269 | + robot_status = True | ||
257 | if self.coord_inv_old is None: | 270 | if self.coord_inv_old is None: |
258 | self.coord_inv_old = new_robot_coordinates | 271 | self.coord_inv_old = new_robot_coordinates |
259 | 272 | ||
@@ -266,21 +279,30 @@ class ControlRobot(threading.Thread): | @@ -266,21 +279,30 @@ class ControlRobot(threading.Thread): | ||
266 | self.coord_inv_old = new_robot_coordinates | 279 | self.coord_inv_old = new_robot_coordinates |
267 | else: | 280 | else: |
268 | distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) | 281 | distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) |
269 | - self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered) | 282 | + robot_status = self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered) |
270 | self.coord_inv_old = new_robot_coordinates | 283 | self.coord_inv_old = new_robot_coordinates |
271 | else: | 284 | else: |
272 | self.trck_init_robot.StopRobot() | 285 | self.trck_init_robot.StopRobot() |
273 | 286 | ||
287 | + return robot_status | ||
288 | + | ||
274 | def run(self): | 289 | def run(self): |
275 | while not self.event_robot.is_set(): | 290 | while not self.event_robot.is_set(): |
276 | current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices() | 291 | current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices() |
277 | 292 | ||
278 | if not self.robot_target_queue.empty(): | 293 | if not self.robot_target_queue.empty(): |
279 | self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait() | 294 | self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait() |
295 | + self.robot_motion_reset() | ||
280 | self.robot_target_queue.task_done() | 296 | self.robot_target_queue.task_done() |
281 | 297 | ||
282 | if not self.object_at_target_queue.empty(): | 298 | if not self.object_at_target_queue.empty(): |
283 | self.target_flag = self.object_at_target_queue.get_nowait() | 299 | self.target_flag = self.object_at_target_queue.get_nowait() |
284 | self.object_at_target_queue.task_done() | 300 | self.object_at_target_queue.task_done() |
285 | 301 | ||
286 | - 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
@@ -51,7 +51,7 @@ class Tracker(): | @@ -51,7 +51,7 @@ class Tracker(): | ||
51 | self.DisconnectTracker() | 51 | self.DisconnectTracker() |
52 | 52 | ||
53 | self.trk_init = dt.TrackerConnection(new_tracker, None, 'connect') | 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 | dlg.ShowNavigationTrackerWarning(self.tracker_id, self.trk_init[1]) | 55 | dlg.ShowNavigationTrackerWarning(self.tracker_id, self.trk_init[1]) |
56 | 56 | ||
57 | self.tracker_id = 0 | 57 | self.tracker_id = 0 |
@@ -70,6 +70,7 @@ class Tracker(): | @@ -70,6 +70,7 @@ class Tracker(): | ||
70 | label=_("Disconnecting tracker ...")) | 70 | label=_("Disconnecting tracker ...")) |
71 | Publisher.sendMessage('Remove sensors ID') | 71 | Publisher.sendMessage('Remove sensors ID') |
72 | Publisher.sendMessage('Remove object data') | 72 | Publisher.sendMessage('Remove object data') |
73 | + Publisher.sendMessage('Robot navigation mode', robot_mode=False) | ||
73 | self.trk_init = dt.TrackerConnection(self.tracker_id, self.trk_init[0], 'disconnect') | 74 | self.trk_init = dt.TrackerConnection(self.tracker_id, self.trk_init[0], 'disconnect') |
74 | if not self.trk_init[0]: | 75 | if not self.trk_init[0]: |
75 | self.tracker_connected = False | 76 | self.tracker_connected = False |
@@ -94,9 +95,10 @@ class Tracker(): | @@ -94,9 +95,10 @@ class Tracker(): | ||
94 | def ConnectToRobot(self, navigation, tracker, robot): | 95 | def ConnectToRobot(self, navigation, tracker, robot): |
95 | robot.SetRobotQueues([navigation.robot_target_queue, navigation.object_at_target_queue]) | 96 | robot.SetRobotQueues([navigation.robot_target_queue, navigation.object_at_target_queue]) |
96 | robot.OnRobotConnection() | 97 | robot.OnRobotConnection() |
97 | - trk_init_robot = self.trk_init[1][0] | 98 | + trk_init_robot = self.trk_init[0][1][0] |
98 | if trk_init_robot: | 99 | if trk_init_robot: |
99 | robot.StartRobotThreadNavigation(tracker, navigation.coord_queue) | 100 | robot.StartRobotThreadNavigation(tracker, navigation.coord_queue) |
101 | + Publisher.sendMessage('Robot navigation mode', robot_mode=True) | ||
100 | 102 | ||
101 | def IsTrackerInitialized(self): | 103 | def IsTrackerInitialized(self): |
102 | return self.trk_init and self.tracker_id and self.tracker_connected | 104 | return self.trk_init and self.tracker_id and self.tracker_connected |
No preview for this file type
optional-requirements.txt
@@ -5,4 +5,5 @@ python-rtmidi==1.4.9 | @@ -5,4 +5,5 @@ python-rtmidi==1.4.9 | ||
5 | python-socketio[client]==5.3.0 | 5 | python-socketio[client]==5.3.0 |
6 | requests==2.26.0 | 6 | requests==2.26.0 |
7 | uvicorn[standard]==0.15.0 | 7 | uvicorn[standard]==0.15.0 |
8 | -opencv-python==4.5.3 | 8 | +opencv-python==4.5.3.56 |
9 | +pyacvd==0.2.7 |