diff --git a/invesalius/gui/task_navigator.py b/invesalius/gui/task_navigator.py index ab3e795..b98653e 100644 --- a/invesalius/gui/task_navigator.py +++ b/invesalius/gui/task_navigator.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') diff --git a/invesalius/navigation/navigation.py b/invesalius/navigation/navigation.py index bd1e656..8c0f5fb 100644 --- a/invesalius/navigation/navigation.py +++ b/invesalius/navigation/navigation.py @@ -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) diff --git a/invesalius/navigation/robot.py b/invesalius/navigation/robot.py index 03425b6..d762dd7 100644 --- a/invesalius/navigation/robot.py +++ b/invesalius/navigation/robot.py @@ -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) diff --git a/invesalius/navigation/tracker.py b/invesalius/navigation/tracker.py index e40e4ed..766ff58 100644 --- a/invesalius/navigation/tracker.py +++ b/invesalius/navigation/tracker.py @@ -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 -- libgit2 0.21.2