Commit dfd7fc1e0e4f3535d9cb3ebc9ba26af9ae1ce13c

Authored by sotodela
Committed by GitHub
2 parents 1b4040d7 2d8f59df
Exists in master

Merge branch 'invesalius:master' into 278_add_visualization_for_the_direction_of_the_marker

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
navigation/objects/robot.stl 0 → 100644
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