Download as
Browse Code »

Commit 0065134e7d4f94d65ea18f6213cab9b25ea4b83d

Authored by Renan
1 parent 0ff58d76
Exists in master

ADD: stop robot when navigation is off 

-cleaning
Showing 4 changed files with 38 additions and 38 deletions   Show diff stats
invesalius/gui/task_navigator.py
  Diff comments   View file @0065134
@@ -340,8 +340,7 @@ class NeuronavigationPanel(wx.Panel): @@ -340,8 +340,7 @@ class NeuronavigationPanel(wx.Panel):
340 ) 340 )
341 self.icp = ICP() 341 self.icp = ICP()
342 self.tracker = tracker 342 self.tracker = tracker
343 - self.robot = Robot()  
344 - self.robotcoordinates = elfin_process.RobotCoordinates() 343 + self.robot = Robot(tracker)
345 344
346 self.nav_status = False 345 self.nav_status = False
347 self.tracker_fiducial_being_set = None 346 self.tracker_fiducial_being_set = None
@@ -644,14 +643,7 @@ class NeuronavigationPanel(wx.Panel): @@ -644,14 +643,7 @@ class NeuronavigationPanel(wx.Panel):
644 643
645 self.tracker.SetTracker(choice) 644 self.tracker.SetTracker(choice)
646 if self.tracker.tracker_id == const.ROBOT: 645 if self.tracker.tracker_id == const.ROBOT:
647 - self.robot.SetRobotQueues([self.navigation.robottarget_queue,  
648 - self.navigation.objattarget_queue])  
649 - self.robot.OnRobotConnection(self.tracker, self.robotcoordinates)  
650 - trk_init_robot = self.tracker.trk_init[1][0]  
651 - if trk_init_robot:  
652 - #todo: create a variable to stop thread  
653 - self.robot.StartRobotNavigation(self.tracker, self.robotcoordinates,  
654 - self.navigation.coord_queue) 646 + self.tracker.ConnectToRobot(self.navigation, self.tracker, self.robot)
655 647
656 self.ResetICP() 648 self.ResetICP()
657 self.tracker.UpdateUI(ctrl, self.numctrls_fiducial[3:6], self.txtctrl_fre) 649 self.tracker.UpdateUI(ctrl, self.numctrls_fiducial[3:6], self.txtctrl_fre)
@@ -1499,13 +1491,6 @@ class MarkersPanel(wx.Panel): @@ -1499,13 +1491,6 @@ class MarkersPanel(wx.Panel):
1499 robot = self.markers[self.lc.GetFocusedItem()].robot 1491 robot = self.markers[self.lc.GetFocusedItem()].robot
1500 head = self.markers[self.lc.GetFocusedItem()].head 1492 head = self.markers[self.lc.GetFocusedItem()].head
1501 1493
1502 - # coord_target = self.list_coord[3]  
1503 - # coord_home = self.list_coord[4]  
1504 - # if self.flag_target:  
1505 - # coord = coord_home  
1506 - # else:  
1507 - # coord = coord_target  
1508 -  
1509 trans = tr.translation_matrix(robot[:3]) 1494 trans = tr.translation_matrix(robot[:3])
1510 a, b, g = np.radians(robot[3:]) 1495 a, b, g = np.radians(robot[3:])
1511 rot = tr.euler_matrix(a, b, g, 'rzyx') 1496 rot = tr.euler_matrix(a, b, g, 'rzyx')
@@ -1520,7 +1505,6 @@ class MarkersPanel(wx.Panel): @@ -1520,7 +1505,6 @@ class MarkersPanel(wx.Panel):
1520 1505
1521 Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot2ref=m_change_robot2ref) 1506 Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot2ref=m_change_robot2ref)
1522 1507
1523 -  
1524 def OnDeleteAllMarkers(self, evt=None): 1508 def OnDeleteAllMarkers(self, evt=None):
1525 if evt is None: 1509 if evt is None:
1526 result = wx.ID_OK 1510 result = wx.ID_OK
@@ -1613,7 +1597,6 @@ class MarkersPanel(wx.Panel): @@ -1613,7 +1597,6 @@ class MarkersPanel(wx.Panel):
1613 wx.MessageBox(_("Invalid markers file."), _("InVesalius 3")) 1597 wx.MessageBox(_("Invalid markers file."), _("InVesalius 3"))
1614 1598
1615 def OnMarkersVisibility(self, evt, ctrl): 1599 def OnMarkersVisibility(self, evt, ctrl):
1616 -  
1617 if ctrl.GetValue(): 1600 if ctrl.GetValue():
1618 Publisher.sendMessage('Hide all markers', indexes=self.lc.GetItemCount()) 1601 Publisher.sendMessage('Hide all markers', indexes=self.lc.GetItemCount())
1619 ctrl.SetLabel('Show') 1602 ctrl.SetLabel('Show')
invesalius/navigation/navigation.py
  Diff comments   View file @0065134
@@ -342,3 +342,5 @@ class Navigation(): @@ -342,3 +342,5 @@ class Navigation():
342 342
343 vis_components = [self.SerialPortEnabled(), self.view_tracts, self.peel_loaded] 343 vis_components = [self.SerialPortEnabled(), self.view_tracts, self.peel_loaded]
344 Publisher.sendMessage("Navigation status", nav_status=False, vis_status=vis_components) 344 Publisher.sendMessage("Navigation status", nav_status=False, vis_status=vis_components)
  345 + Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
  346 + m_change_robot2ref=None)
invesalius/navigation/robot.py
  Diff comments   View file @0065134
@@ -31,12 +31,17 @@ except ImportError: @@ -31,12 +31,17 @@ except ImportError:
31 has_robot = False 31 has_robot = False
32 32
33 class Robot(): 33 class Robot():
34 - def __init__(self): 34 + def __init__(self, tracker):
  35 + self.tracker = tracker
35 self.trk_init = None 36 self.trk_init = None
36 self.robottarget_queue = None 37 self.robottarget_queue = None
37 self.objattarget_queue = None 38 self.objattarget_queue = None
38 self.process_tracker = None 39 self.process_tracker = None
39 40
  41 + self.thread_robot = None
  42 +
  43 + self.robotcoordinates = elfin_process.RobotCoordinates()
  44 +
40 self.__bind_events() 45 self.__bind_events()
41 46
42 def __bind_events(self): 47 def __bind_events(self):
@@ -44,36 +49,39 @@ class Robot(): @@ -44,36 +49,39 @@ class Robot():
44 Publisher.subscribe(self.OnUpdateRobotTargetMatrix, 'Robot target matrix') 49 Publisher.subscribe(self.OnUpdateRobotTargetMatrix, 'Robot target matrix')
45 Publisher.subscribe(self.OnObjectTarget, 'Coil at target') 50 Publisher.subscribe(self.OnObjectTarget, 'Coil at target')
46 51
47 - def OnRobotConnection(self, tracker, robotcoordinates):  
48 - if not tracker.trk_init[0][0] or not tracker.trk_init[1][0]:  
49 - dlg.ShowNavigationTrackerWarning(tracker.tracker_id, tracker.trk_init[1])  
50 - tracker.tracker_id = 0  
51 - tracker.tracker_connected = False 52 + def OnRobotConnection(self):
  53 + if not self.tracker.trk_init[0][0] or not self.tracker.trk_init[1][0]:
  54 + dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, self.tracker.trk_init[1])
  55 + self.tracker.tracker_id = 0
  56 + self.tracker.tracker_connected = False
52 else: 57 else:
53 - tracker.trk_init.append(robotcoordinates) 58 + self.tracker.trk_init.append(self.robotcoordinates)
54 self.process_tracker = elfin_process.TrackerProcessing() 59 self.process_tracker = elfin_process.TrackerProcessing()
55 - dlg_correg_robot = dlg.CreateTransformationMatrixRobot(tracker) 60 + dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker)
56 if dlg_correg_robot.ShowModal() == wx.ID_OK: 61 if dlg_correg_robot.ShowModal() == wx.ID_OK:
57 M_tracker_2_robot = dlg_correg_robot.GetValue() 62 M_tracker_2_robot = dlg_correg_robot.GetValue()
58 db.transform_tracker_2_robot.M_tracker_2_robot = M_tracker_2_robot 63 db.transform_tracker_2_robot.M_tracker_2_robot = M_tracker_2_robot
59 - self.robot_server = tracker.trk_init[1][0]  
60 - self.trk_init = tracker.trk_init 64 + self.robot_server = self.tracker.trk_init[1][0]
  65 + self.trk_init = self.tracker.trk_init
61 else: 66 else:
62 - dlg.ShowNavigationTrackerWarning(tracker.tracker_id, 'disconnect')  
63 - tracker.trk_init = None  
64 - tracker.tracker_id = 0  
65 - tracker.tracker_connected = False 67 + dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, 'disconnect')
  68 + self.tracker.trk_init = None
  69 + self.tracker.tracker_id = 0
  70 + self.tracker.tracker_connected = False
66 71
67 # Publisher.sendMessage('Update tracker initializer', 72 # Publisher.sendMessage('Update tracker initializer',
68 # nav_prop=(tracker.tracker_id, tracker.trk_init, tracker.TrackerCoordinates, tracker.GetReferenceMode())) 73 # nav_prop=(tracker.tracker_id, tracker.trk_init, tracker.TrackerCoordinates, tracker.GetReferenceMode()))
69 74
70 - def StartRobotNavigation(self, tracker, robotcoordinates, coord_queue): 75 + def StartRobotThreadNavigation(self, tracker, coord_queue):
71 if tracker.event_robot.is_set(): 76 if tracker.event_robot.is_set():
72 tracker.event_robot.clear() 77 tracker.event_robot.clear()
73 - elfin_process.ControlRobot(self.trk_init, tracker, robotcoordinates,  
74 - [coord_queue, self.robottarget_queue,  
75 - self.objattarget_queue],  
76 - self.process_tracker, tracker.event_robot).start() 78 + self.thread_robot = elfin_process.ControlRobot(self.trk_init, tracker, self.robotcoordinates,
  79 + [coord_queue, self.robottarget_queue, self.objattarget_queue],
  80 + self.process_tracker, tracker.event_robot)
  81 + self.thread_robot.start()
  82 +
  83 + def StopRobotThreadNavigation(self):
  84 + self.thread_robot.join()
77 85
78 def OnSendCoordinates(self, coord): 86 def OnSendCoordinates(self, coord):
79 self.robot_server.SendCoordinates(coord) 87 self.robot_server.SendCoordinates(coord)
invesalius/navigation/tracker.py
  Diff comments   View file @0065134
@@ -92,6 +92,13 @@ class Tracker(): @@ -92,6 +92,13 @@ class Tracker():
92 label=_("Tracker still connected")) 92 label=_("Tracker still connected"))
93 print("Tracker still connected!") 93 print("Tracker still connected!")
94 94
  95 + def ConnectToRobot(self, navigation, tracker, robot):
  96 + robot.SetRobotQueues([navigation.robottarget_queue, navigation.objattarget_queue])
  97 + robot.OnRobotConnection()
  98 + trk_init_robot = self.trk_init[1][0]
  99 + if trk_init_robot:
  100 + robot.StartRobotThreadNavigation(tracker, navigation.coord_queue)
  101 +
95 def IsTrackerInitialized(self): 102 def IsTrackerInitialized(self):
96 return self.trk_init and self.tracker_id and self.tracker_connected 103 return self.trk_init and self.tracker_id and self.tracker_connected
97 104