ADD: stop robot when navigation is off

-cleaning

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