diff --git a/invesalius/data/coregistration.py b/invesalius/data/coregistration.py index 27b2268..784bb2a 100644 --- a/invesalius/data/coregistration.py +++ b/invesalius/data/coregistration.py @@ -20,11 +20,12 @@ import numpy as np import queue import threading -from time import sleep +from time import sleep import invesalius.constants as const import invesalius.data.transformations as tr import invesalius.data.bases as bases +import invesalius.data.coordinates as dco # TODO: Replace the use of degrees by radians in every part of the navigation pipeline diff --git a/invesalius/gui/dialogs.py b/invesalius/gui/dialogs.py index 7e3d2a9..8192057 100644 --- a/invesalius/gui/dialogs.py +++ b/invesalius/gui/dialogs.py @@ -2920,11 +2920,6 @@ class FFillSegmentationOptionsDialog(wx.Dialog): sizer.Add(0, 0, (14, 0)) except TypeError: sizer.AddStretchSpacer((14, 0)) - sizer.Add(self.close_btn, (15, 0), (1, 6), flag=wx.ALIGN_RIGHT|wx.RIGHT, border=5) - try: - sizer.Add(0, 0, (16, 0)) - except TypeError: - sizer.AddStretchSpacer((16, 0)) self.SetSizer(sizer) sizer.Fit(self) @@ -4329,7 +4324,7 @@ class SetTrackerDevice2Robot(wx.Dialog): def _init_gui(self): # ComboBox for spatial tracker device selection tooltip = wx.ToolTip(_("Choose the tracking device")) - tracker_options = [_("Select tracker:")] + const.TRACKERS[:-3] + tracker_options = [_("Select tracker:")] + const.TRACKERS[:-1] choice_trck = wx.ComboBox(self, -1, "", choices=tracker_options, style=wx.CB_DROPDOWN | wx.CB_READONLY) choice_trck.SetToolTip(tooltip) diff --git a/invesalius/gui/task_navigator.py b/invesalius/gui/task_navigator.py index 9637566..fa27158 100644 --- a/invesalius/gui/task_navigator.py +++ b/invesalius/gui/task_navigator.py @@ -68,6 +68,8 @@ from invesalius.gui import utils as gui_utils from invesalius.navigation.icp import ICP from invesalius.navigation.navigation import Navigation from invesalius.navigation.tracker import Tracker +from invesalius.navigation.robot import Robot +import invesalius.data.transformations as tr HAS_PEDAL_CONNECTION = True try: @@ -647,6 +649,7 @@ class NeuronavigationPanel(wx.Panel): 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) @@ -684,9 +687,11 @@ class NeuronavigationPanel(wx.Panel): colour = (0., 1., 0.) size = 2 seed = 3 * [0.] + head = 6 * [0.] + robot = 6 * [0.] Publisher.sendMessage('Create marker', coord=coord, colour=colour, size=size, - label=label, seed=seed) + label=label, seed=seed, head=head, robot=robot) else: for m in [0, 1, 2]: self.numctrls_fiducial[n][m].SetValue(float(self.current_coord[m])) @@ -1119,6 +1124,18 @@ class MarkersPanel(wx.Panel): x_seed : float = 0 y_seed : float = 0 z_seed : float = 0 + x_head : float = 0 + y_head : float = 0 + z_head : float = 0 + alpha_head : float = 0 + beta_head : float = 0 + gamma_head : float = 0 + x_robot : float = 0 + y_robot : float = 0 + z_robot : float = 0 + alpha_robot : float = 0 + beta_robot: float = 0 + gamma_robot : float = 0 is_target : int = 0 # is_target is int instead of boolean to avoid # problems with CSV export @@ -1149,6 +1166,24 @@ class MarkersPanel(wx.Panel): def seed(self, new_seed): self.x_seed, self.y_seed, self.z_seed = new_seed + # x_head, y_head, z_head, alpha_head, beta_head, gamma_head can be jointly accessed as robot + @property + def head(self): + return list((self.x_head, self.y_head, self.z_head, self.alpha_head, self.beta_head, self.gamma_head),) + + @head.setter + def head(self, new_head): + self.x_head, self.y_head, self.z_head, self.alpha_head, self.beta_head, self.gamma_head = new_head + + # x_robot, y_robot, z_robot, alpha_robot, beta_robot, gamma_robot can be jointly accessed as robot + @property + def robot(self): + return list((self.x_robot, self.y_robot, self.z_robot, self.alpha_robot, self.beta_robot, self.gamma_robot),) + + @robot.setter + def robot(self, new_robot): + self.x_robot, self.y_robot, self.z_robot, self.alpha_robot, self.beta_robot, self.gamma_robot = new_robot + @classmethod def get_headers(cls): """Return the list of field names (headers) for exporting to csv.""" @@ -1288,7 +1323,7 @@ class MarkersPanel(wx.Panel): Publisher.subscribe(self.UpdateSeedCoordinates, 'Update tracts') Publisher.subscribe(self.UpdateMchange, 'Update matrix change') Publisher.subscribe(self.UpdateMRef, 'Update ref matrix') - Publisher.subscribe(self.UpdateRawCoord, 'Update raw coord') + Publisher.subscribe(self.UpdateRobotCoord, 'Update raw coord') Publisher.subscribe(self.UpdateObjectMarker2Center, 'Update object marker to center') Publisher.subscribe(self.OnObjectTarget, 'Coil at target') @@ -1377,8 +1412,8 @@ class MarkersPanel(wx.Panel): def UpdateMchange(self, mchange): self.mchange = mchange - def UpdateRawCoord(self, coord_raw, markers_flag): - self.current_ref = coord_raw[1] + def UpdateRobotCoord(self, coord_raw, markers_flag): + self.current_head = coord_raw[1] self.current_robot = coord_raw[2] def UpdateObjectMarker2Center(self, s0_raw, t_offset): @@ -1452,16 +1487,13 @@ class MarkersPanel(wx.Panel): Publisher.sendMessage('Set new color', index=index, color=color_new) - # def OnContinuousSendCoord(self, evt=None): - # self.timer.Start(30000) - # - # def OnUpdateSendCoord(self, evt): - # self.OnMenuSendCoord(evt=None) - def OnMenuSendCoord(self, evt): if isinstance(evt, int): self.lc.Focus(evt) - coord = self.list_coord[self.lc.GetFocusedItem()] + + robot = self.markers[self.lc.GetFocusedItem()].robot + head = self.markers[self.lc.GetFocusedItem()].head + print(robot) # coord_target = self.list_coord[3] # coord_home = self.list_coord[4] # if self.flag_target: @@ -1469,13 +1501,13 @@ class MarkersPanel(wx.Panel): # else: # coord = coord_target - trans = tr.translation_matrix(coord[14:17]) - a, b, g = np.radians(coord[17:20]) + trans = tr.translation_matrix(robot[:3]) + a, b, g = np.radians(robot[3:]) rot = tr.euler_matrix(a, b, g, 'rzyx') m_robot_target = tr.concatenate_matrices(trans, rot) - trans = tr.translation_matrix(coord[20:23]) - a, b, g = np.radians(coord[23:26]) + trans = tr.translation_matrix(head[:3]) + a, b, g = np.radians(head[3:]) rot = tr.euler_matrix(a, b, g, 'rzyx') m_ref_target = tr.concatenate_matrices(trans, rot) @@ -1618,7 +1650,7 @@ class MarkersPanel(wx.Panel): def OnSelectSize(self, evt, ctrl): self.marker_size = ctrl.GetValue() - def CreateMarker(self, coord=None, colour=None, size=None, label='*', is_target=0, seed=None, robot=None, ref=None): + def CreateMarker(self, coord=None, colour=None, size=None, label='*', is_target=0, seed=None, head=None, robot=None): new_marker = self.Marker() new_marker.coord = coord or self.current_coord new_marker.colour = colour or self.marker_colour @@ -1626,8 +1658,8 @@ class MarkersPanel(wx.Panel): new_marker.label = label new_marker.is_target = is_target new_marker.seed = seed or self.current_seed + new_marker.head = head or self.current_head new_marker.robot = robot or self.current_robot - new_marker.ref = ref or self.current_ref # Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added Publisher.sendMessage('Add marker', ball_id=len(self.markers), diff --git a/invesalius/navigation/navigation.py b/invesalius/navigation/navigation.py index 7e3bdb7..bd1e656 100644 --- a/invesalius/navigation/navigation.py +++ b/invesalius/navigation/navigation.py @@ -82,7 +82,7 @@ class UpdateNavigationScene(threading.Thread): threading.Thread.__init__(self, name='UpdateScene') self.serial_port_enabled, self.view_tracts, self.peel_loaded = vis_components - self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue = vis_queues + self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robottarget_queue = vis_queues self.sle = sle self.event = event @@ -91,7 +91,7 @@ class UpdateNavigationScene(threading.Thread): while not self.event.is_set(): got_coords = False try: - coord, m_img, view_obj = self.coord_queue.get_nowait() + coord, [coord_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait() got_coords = True # print('UpdateScene: get {}'.format(count)) @@ -116,6 +116,7 @@ class UpdateNavigationScene(threading.Thread): # see the red cross in the position of the offset marker wx.CallAfter(Publisher.sendMessage, 'Update slices position', position=coord[:3]) wx.CallAfter(Publisher.sendMessage, 'Set cross focal point', position=coord) + wx.CallAfter(Publisher.sendMessage, 'Update raw coord', coord_raw=coord_raw, markers_flag=markers_flag) wx.CallAfter(Publisher.sendMessage, 'Update slice viewer') if view_obj: @@ -146,6 +147,8 @@ class Navigation(): self.event = threading.Event() self.coord_queue = QueueCustom(maxsize=1) self.icp_queue = QueueCustom(maxsize=1) + self.objattarget_queue = QueueCustom(maxsize=1) + self.robottarget_queue = QueueCustom(maxsize=1) # self.visualization_queue = QueueCustom(maxsize=1) self.serial_port_queue = QueueCustom(maxsize=1) self.coord_tracts_queue = QueueCustom(maxsize=1) @@ -228,7 +231,7 @@ class Navigation(): self.event.clear() vis_components = [self.SerialPortEnabled(), self.view_tracts, self.peel_loaded] - vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue] + vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robottarget_queue] Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components) @@ -262,7 +265,7 @@ class Navigation(): obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change) coreg_data.extend(obj_data) - queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue] + queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.objattarget_queue] jobs_list.append(dcr.CoordinateCorregistrate(self.ref_mode_id, tracker, coreg_data, self.view_tracts, queues, self.event, self.sleep_nav, tracker.tracker_id, diff --git a/invesalius/navigation/robot.py b/invesalius/navigation/robot.py new file mode 100644 index 0000000..03425b6 --- /dev/null +++ b/invesalius/navigation/robot.py @@ -0,0 +1,97 @@ +#-------------------------------------------------------------------------- +# Software: InVesalius - Software de Reconstrucao 3D de Imagens Medicas +# Copyright: (C) 2001 Centro de Pesquisas Renato Archer +# Homepage: http://www.softwarepublico.gov.br +# Contact: invesalius@cti.gov.br +# License: GNU - GPL 2 (LICENSE.txt/LICENCA.txt) +#-------------------------------------------------------------------------- +# Este programa e software livre; voce pode redistribui-lo e/ou +# modifica-lo sob os termos da Licenca Publica Geral GNU, conforme +# publicada pela Free Software Foundation; de acordo com a versao 2 +# da Licenca. +# +# Este programa eh distribuido na expectativa de ser util, mas SEM +# QUALQUER GARANTIA; sem mesmo a garantia implicita de +# COMERCIALIZACAO ou de ADEQUACAO A QUALQUER PROPOSITO EM +# PARTICULAR. Consulte a Licenca Publica Geral GNU para obter mais +# detalhes. +#-------------------------------------------------------------------------- +import wx +import queue + +import invesalius.data.bases as db +import invesalius.gui.dialogs as dlg +from invesalius.pubsub import pub as Publisher + +try: + import invesalius.data.elfin as elfin + import invesalius.data.elfin_robot as elfin_process + has_robot = True +except ImportError: + has_robot = False + +class Robot(): + def __init__(self): + self.trk_init = None + self.robottarget_queue = None + self.objattarget_queue = None + self.process_tracker = None + + self.__bind_events() + + def __bind_events(self): + Publisher.subscribe(self.OnSendCoordinates, 'Send coord to 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 + else: + tracker.trk_init.append(robotcoordinates) + self.process_tracker = elfin_process.TrackerProcessing() + dlg_correg_robot = dlg.CreateTransformationMatrixRobot(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 + else: + dlg.ShowNavigationTrackerWarning(tracker.tracker_id, 'disconnect') + tracker.trk_init = None + tracker.tracker_id = 0 + 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): + 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() + + def OnSendCoordinates(self, coord): + self.robot_server.SendCoordinates(coord) + + def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot2ref): + try: + self.robottarget_queue.put_nowait([robot_tracker_flag, m_change_robot2ref]) + except queue.Full: + print('full target') + pass + + def OnObjectTarget(self, state): + try: + if self.objattarget_queue: + self.objattarget_queue.put_nowait(state) + except queue.Full: + #print('full flag target') + pass + + def SetRobotQueues(self, queues): + self.robottarget_queue, self.objattarget_queue = queues \ No newline at end of file diff --git a/invesalius/navigation/tracker.py b/invesalius/navigation/tracker.py index b92a491..e40e4ed 100644 --- a/invesalius/navigation/tracker.py +++ b/invesalius/navigation/tracker.py @@ -24,6 +24,7 @@ import invesalius.constants as const import invesalius.data.coordinates as dco import invesalius.data.trackers as dt import invesalius.gui.dialogs as dlg +import invesalius.data.coregistration as dcr from invesalius.pubsub import pub as Publisher @@ -34,12 +35,14 @@ class Tracker(): self.tracker_fiducials = np.full([3, 3], np.nan) self.tracker_fiducials_raw = np.zeros((6, 6)) + self.m_tracker_fiducials_raw = np.zeros((6, 4, 4)) self.tracker_connected = False self.thread_coord = None self.event_coord = threading.Event() + self.event_robot = threading.Event() self.TrackerCoordinates = dco.TrackerCoordinates() @@ -74,8 +77,12 @@ class Tracker(): if self.thread_coord: self.event_coord.set() + self.event_robot.set() self.thread_coord.join() + #get the robot thread?? self.event_coord.clear() + self.event_robot.clear() + Publisher.sendMessage('Update status text in GUI', label=_("Tracker disconnected")) @@ -126,6 +133,9 @@ class Tracker(): self.tracker_fiducials_raw[2 * fiducial_index, :] = coord_raw[0, :] self.tracker_fiducials_raw[2 * fiducial_index + 1, :] = coord_raw[1, :] + self.m_tracker_fiducials_raw[2 * fiducial_index, :] = dcr.compute_marker_transformation(coord_raw, 0) + self.m_tracker_fiducials_raw[2 * fiducial_index + 1, :] = dcr.compute_marker_transformation(coord_raw, 1) + print("Set tracker fiducial {} to coordinates {}.".format(fiducial_index, coord[0:3])) def ResetTrackerFiducials(self): @@ -135,6 +145,13 @@ class Tracker(): def GetTrackerFiducials(self): return self.tracker_fiducials, self.tracker_fiducials_raw + def GetMatrixTrackerFiducials(self): + m_probe_ref_left = np.linalg.inv(self.m_tracker_fiducials_raw[1]) @ self.m_tracker_fiducials_raw[0] + m_probe_ref_right = np.linalg.inv(self.m_tracker_fiducials_raw[3]) @ self.m_tracker_fiducials_raw[2] + m_probe_ref_nasion = np.linalg.inv(self.m_tracker_fiducials_raw[5]) @ self.m_tracker_fiducials_raw[4] + + return [m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion] + def GetTrackerInfo(self): return self.trk_init, self.tracker_id -- libgit2 0.21.2