From 839d45c1a425b83bb66e770c07172b5acf43befe Mon Sep 17 00:00:00 2001 From: Renan Date: Mon, 11 Oct 2021 13:22:27 +0300 Subject: [PATCH] ENH: variable names refractored --- invesalius/constants.py | 5 ++--- invesalius/data/bases.py | 12 ++++++------ invesalius/data/coordinates.py | 6 +++--- invesalius/data/coregistration.py | 7 +++---- invesalius/data/elfin.py | 36 ++++++++++++++++++------------------ invesalius/data/elfin_processing.py | 48 ++++++++++++++++++++++++++++-------------------- invesalius/data/trackers.py | 2 +- invesalius/gui/dialogs.py | 2 +- invesalius/gui/task_navigator.py | 26 +++++++++++++------------- invesalius/navigation/navigation.py | 14 +++++++------- invesalius/navigation/robot.py | 96 +++++++++++++++++++++++++++++++++++++++++++++++++----------------------------------------------- invesalius/navigation/tracker.py | 2 +- 12 files changed, 132 insertions(+), 124 deletions(-) diff --git a/invesalius/constants.py b/invesalius/constants.py index eac9543..d519aa6 100644 --- a/invesalius/constants.py +++ b/invesalius/constants.py @@ -830,7 +830,6 @@ TREKKER_CONFIG = {'seed_max': 1, 'step_size': 0.1, 'min_fod': 0.1, 'probe_qualit MARKER_FILE_MAGICK_STRING = "INVESALIUS3_MARKER_FILE_" CURRENT_MARKER_FILE_VERSION = 0 - WILDCARD_MARKER_FILES = _("Marker scanner coord files (*.mkss)|*.mkss") # Serial port @@ -840,8 +839,8 @@ BAUD_RATE_DEFAULT_SELECTION = 4 #Robot ROBOT_ElFIN_IP = ['Select robot IP:', '143.107.220.251', '169.254.153.251', '127.0.0.1'] ROBOT_ElFIN_PORT = 10003 - ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2} - ROBOT_HEAD_VELOCITY_THRESHOLD = 10 ROBOT_ARC_THRESHOLD_DISTANCE = 100 +ROBOT_VERSOR_SCALE_FACTOR = 70 + diff --git a/invesalius/data/bases.py b/invesalius/data/bases.py index c08ccaa..f9b5279 100644 --- a/invesalius/data/bases.py +++ b/invesalius/data/bases.py @@ -245,10 +245,10 @@ def object_registration(fiducials, orients, coord_raw, m_change): return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img -class transform_tracker_2_robot(object): - M_tracker_2_robot = None - def transformation_tracker_2_robot(self, tracker_coord): - if transform_tracker_2_robot.M_tracker_2_robot is None: +class transform_tracker_to_robot(object): + M_tracker_to_robot = np.array([]) + def transformation_tracker_to_robot(self, tracker_coord): + if not transform_tracker_to_robot.M_tracker_to_robot.any(): #print("matrix tracker2robot is not define") return None else: @@ -256,10 +256,10 @@ class transform_tracker_2_robot(object): a, b, g = np.radians(tracker_coord[3:6]) rot = tr.euler_matrix(a, b, g, 'rzyx') M_tracker = tr.concatenate_matrices(trans, rot) - M_tracker_in_robot = transform_tracker_2_robot.M_tracker_2_robot @ M_tracker + M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker _, _, angles, translate, _ = tr.decompose_matrix(M_tracker_in_robot) tracker_in_robot = [translate[0], translate[1], translate[2], \ np.degrees(angles[2]), np.degrees(angles[1]), np.degrees(angles[0])] - return tracker_in_robot \ No newline at end of file + return tracker_in_robot diff --git a/invesalius/data/coordinates.py b/invesalius/data/coordinates.py index 31c8e92..e44b2c6 100644 --- a/invesalius/data/coordinates.py +++ b/invesalius/data/coordinates.py @@ -488,9 +488,9 @@ def RobotCoord(trk_init, trck_id, ref_mode): coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], trk_init[2], ref_mode) coord_robot, _ = ElfinCoord(trk_init[1:]) - probe_tracker_in_robot = db.transform_tracker_2_robot().transformation_tracker_2_robot(coord_tracker[0]) - ref_tracker_in_robot = db.transform_tracker_2_robot().transformation_tracker_2_robot(coord_tracker[1]) - obj_tracker_in_robot = db.transform_tracker_2_robot().transformation_tracker_2_robot(coord_tracker[2]) + probe_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[0]) + ref_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[1]) + obj_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[2]) if probe_tracker_in_robot is None: probe_tracker_in_robot = coord_tracker[0] diff --git a/invesalius/data/coregistration.py b/invesalius/data/coregistration.py index 784bb2a..4dfcbb6 100644 --- a/invesalius/data/coregistration.py +++ b/invesalius/data/coregistration.py @@ -190,11 +190,10 @@ class CoordinateCorregistrate(threading.Thread): self.event = event self.sle = sle self.icp_queue = queues[2] - self.objattarget_queue = queues[3] + self.object_at_target_queue = queues[3] self.icp = None self.m_icp = None self.robot_tracker_flag = None - self.m_change_robot2ref = None self.last_coord = None self.tracker_id = tracker_id self.target = target @@ -223,10 +222,10 @@ class CoordinateCorregistrate(threading.Thread): else: self.icp, self.m_icp = self.icp_queue.get_nowait() - if self.objattarget_queue.empty(): + if self.object_at_target_queue.empty(): None else: - self.target_flag = self.objattarget_queue.get_nowait() + self.target_flag = self.object_at_target_queue.get_nowait() # print(f"Set the coordinate") coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates() diff --git a/invesalius/data/elfin.py b/invesalius/data/elfin.py index 35d603e..ad0f3e5 100644 --- a/invesalius/data/elfin.py +++ b/invesalius/data/elfin.py @@ -11,10 +11,10 @@ class elfin_server(): self.port_number = port_number def Initialize(self): - SIZE = 1024 - rbtID = 0 + message_size = 1024 + robot_id = 0 self.cobot = elfin() - self.cobot.connect(self.server_ip, self.port_number, SIZE, rbtID) + self.cobot.connect(self.server_ip, self.port_number, message_size, robot_id) print("conected!") def Run(self): @@ -40,17 +40,17 @@ class elfin: def __init__(self): self.end_msg = ",;" - def connect(self, SERVER_IP, PORT_NUMBER, SIZE, rbtID): + def connect(self, server_ip, port_number, message_size, robot_id): mySocket = socket(AF_INET, SOCK_STREAM) - mySocket.connect((SERVER_IP, PORT_NUMBER)) + mySocket.connect((server_ip, port_number)) - self.size = SIZE - self.rbtID = str(rbtID) + self.message_size = message_size + self.robot_id = str(robot_id) self.mySocket = mySocket def send(self, message): self.mySocket.sendall(message.encode('utf-8')) - data = self.mySocket.recv(self.size).decode('utf-8').split(',') + data = self.mySocket.recv(self.message_size).decode('utf-8').split(',') status = self.check_status(data) if status and type(data) != bool: if len(data) > 3: @@ -124,7 +124,7 @@ class elfin: if Error Return False if not Error Return True """ - message = "GrpPowerOn," + self.rbtID + self.end_msg + message = "GrpPowerOn," + self.robot_id + self.end_msg status = self.send(message) return status @@ -135,7 +135,7 @@ class elfin: if Error Return False if not Error Return True """ - message = "GrpPowerOff," + self.rbtID + self.end_msg + message = "GrpPowerOff," + self.robot_id + self.end_msg status = self.send(message) return status @@ -146,7 +146,7 @@ class elfin: if Error Return False if not Error Return True """ - message = "GrpStop," + self.rbtID + self.end_msg + message = "GrpStop," + self.robot_id + self.end_msg status = self.send(message) return status @@ -160,7 +160,7 @@ class elfin: if not Error Return True """ - message = "SetOverride," + self.rbtID + ',' + str(override) + self.end_msg + message = "SetOverride," + self.robot_id + ',' + str(override) + self.end_msg status = self.send(message) return status @@ -170,7 +170,7 @@ class elfin: if True Return x,y,z,a,b,c if Error Return False """ - message = "ReadPcsActualPos," + self.rbtID + self.end_msg + message = "ReadPcsActualPos," + self.robot_id + self.end_msg coord = self.send(message) if coord: return [float(s) for s in coord] @@ -185,7 +185,7 @@ class elfin: """ target = [str(s) for s in target] target = (",".join(target)) - message = "MoveL," + self.rbtID + ',' + target + self.end_msg + message = "MoveL," + self.robot_id + ',' + target + self.end_msg return self.send(message) def SetToolCoordinateMotion(self, status): @@ -196,7 +196,7 @@ class elfin: if Error Return False if not Error Return True """ - message = "SetToolCoordinateMotion," + self.rbtID + ',' + str(status) + self.end_msg + message = "SetToolCoordinateMotion," + self.robot_id + ',' + str(status) + self.end_msg status = self.send(message) return status @@ -210,7 +210,7 @@ class elfin: 1013=waiting for execution; 1025 =Error reporting """ - message = "ReadMoveState," + self.rbtID + self.end_msg + message = "ReadMoveState," + self.robot_id + self.end_msg status = int(self.send(message)[0]) return status @@ -221,7 +221,7 @@ class elfin: if Error Return False if not Error Return True """ - message = "MoveHoming," + self.rbtID + self.end_msg + message = "MoveHoming," + self.robot_id + self.end_msg status = self.send(message) return status @@ -233,5 +233,5 @@ class elfin: """ target = [str(s) for s in target] target = (",".join(target)) - message = "MoveC," + self.rbtID + ',' + target + self.end_msg + message = "MoveC," + self.robot_id + ',' + target + self.end_msg return self.send(message) diff --git a/invesalius/data/elfin_processing.py b/invesalius/data/elfin_processing.py index 0d1f2da..426b0dc 100644 --- a/invesalius/data/elfin_processing.py +++ b/invesalius/data/elfin_processing.py @@ -89,9 +89,9 @@ class TrackerProcessing: def estimate_head_velocity(self, coord_vel, timestamp): coord_vel = np.vstack(np.array(coord_vel)) - coord_init = coord_vel[:int(len(coord_vel)/2)].mean(axis=0) - coord_final = coord_vel[int(len(coord_vel)/2):].mean(axis=0) - velocity = (coord_final - coord_init)/(timestamp[-1]-timestamp[0]) + coord_init = coord_vel[:int(len(coord_vel) / 2)].mean(axis=0) + coord_final = coord_vel[int(len(coord_vel) / 2):].mean(axis=0) + velocity = (coord_final - coord_init)/(timestamp[-1] - timestamp[0]) distance = (coord_final - coord_init) return velocity, distance @@ -99,32 +99,40 @@ class TrackerProcessing: def versors(self, init_point, final_point): init_point = np.array(init_point) final_point = np.array(final_point) - norm = (sum((final_point-init_point)**2))**0.5 - versorfactor = (((final_point-init_point)/norm)*70).tolist() + norm = (sum((final_point - init_point) ** 2)) ** 0.5 + versorfactor = (((final_point-init_point) / norm) * const.ROBOT_VERSOR_SCALE_FACTOR).tolist() return versorfactor - def arcmotion(self, actual_point, coord_head, coord_inv): + def compute_arc_motion(self, actual_point, coord_head, coord_inv): p1 = coord_inv pc = coord_head[0], coord_head[1], coord_head[2], coord_inv[3], coord_inv[4], coord_inv[5] versorfactor1 = self.versors(pc, actual_point) - init_ext_point = actual_point[0]+versorfactor1[0],\ - actual_point[1]+versorfactor1[1],\ - actual_point[2]+versorfactor1[2], \ + init_ext_point = actual_point[0] + versorfactor1[0], \ + actual_point[1] + versorfactor1[1], \ + actual_point[2] + versorfactor1[2], \ actual_point[3], actual_point[4], actual_point[5] - middle_point = ((p1[0]+actual_point[0])/2, (p1[1]+actual_point[1])/2, (p1[2]+actual_point[2])/2, 0, 0, 0) + middle_point = ((p1[0] + actual_point[0]) / 2, + (p1[1] + actual_point[1]) / 2, + (p1[2] + actual_point[2]) / 2, + 0, 0, 0) - newarr = (np.array(self.versors(pc, middle_point)))*2 - middle_arc_point = middle_point[0]+newarr[0], middle_point[1]+newarr[1], middle_point[2]+newarr[2] + newarr = (np.array(self.versors(pc, middle_point))) * 2 + middle_arc_point = middle_point[0] + newarr[0], \ + middle_point[1] + newarr[1], \ + middle_point[2] + newarr[2] versorfactor3 = self.versors(pc, p1) - final_ext_arc_point = p1[0]+versorfactor3[0], p1[1]+versorfactor3[1], p1[2]+versorfactor3[2], \ - coord_inv[3], coord_inv[4], coord_inv[5], 0 - target_arc = middle_arc_point+final_ext_arc_point + final_ext_arc_point = p1[0] + versorfactor3[0], \ + p1[1] + versorfactor3[1], \ + p1[2] + versorfactor3[2], \ + coord_inv[3], coord_inv[4], coord_inv[5], 0 + + target_arc = middle_arc_point + final_ext_arc_point return init_ext_point, target_arc @@ -150,13 +158,13 @@ class TrackerProcessing: return False - def head_move_compensation(self, current_ref, m_change_robot2ref): + def head_move_compensation(self, current_ref, m_change_robot_to_head): trans = tr.translation_matrix(current_ref[:3]) a, b, g = np.radians(current_ref[3:6]) rot = tr.euler_matrix(a, b, g, 'rzyx') M_current_ref = tr.concatenate_matrices(trans, rot) - m_robot_new = M_current_ref @ m_change_robot2ref + m_robot_new = M_current_ref @ m_change_robot_to_head _, _, angles, translate, _ = tr.decompose_matrix(m_robot_new) angles = np.degrees(angles) @@ -172,9 +180,9 @@ class TrackerProcessing: return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2 def correction_distance_calculation_target(self, coord_inv, actual_point): - sum = (coord_inv[0]-actual_point[0])**2\ - + (coord_inv[1]-actual_point[1])**2\ - + (coord_inv[2]-actual_point[2])**2 + sum = (coord_inv[0]-actual_point[0]) ** 2\ + + (coord_inv[1]-actual_point[1]) ** 2\ + + (coord_inv[2]-actual_point[2]) ** 2 correction_distance_compensation = pow(sum, 0.5) return correction_distance_compensation diff --git a/invesalius/data/trackers.py b/invesalius/data/trackers.py index c68e86d..86eefd5 100644 --- a/invesalius/data/trackers.py +++ b/invesalius/data/trackers.py @@ -242,7 +242,7 @@ def RobotTracker(tracker_id): trck_init = None trck_init_robot = None tracker_id = None - dlg_device = dlg.SetTrackerDevice2Robot() + dlg_device = dlg.SetTrackerDeviceToRobot() if dlg_device.ShowModal() == ID_OK: tracker_id = dlg_device.GetValue() if tracker_id: diff --git a/invesalius/gui/dialogs.py b/invesalius/gui/dialogs.py index 01b4273..685394d 100644 --- a/invesalius/gui/dialogs.py +++ b/invesalius/gui/dialogs.py @@ -4323,7 +4323,7 @@ class SetOptitrackconfigs(wx.Dialog): return fn_cal, fn_userprofile -class SetTrackerDevice2Robot(wx.Dialog): +class SetTrackerDeviceToRobot(wx.Dialog): def __init__(self, title=_("Setting tracker device:")): wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200), style=wx.DEFAULT_DIALOG_STYLE|wx.FRAME_FLOAT_ON_PARENT|wx.STAY_ON_TOP|wx.RESIZE_BORDER) diff --git a/invesalius/gui/task_navigator.py b/invesalius/gui/task_navigator.py index 271f46f..4260bd6 100644 --- a/invesalius/gui/task_navigator.py +++ b/invesalius/gui/task_navigator.py @@ -742,7 +742,7 @@ class NeuronavigationPanel(wx.Panel): self.navigation.StopNavigation() if self.tracker.tracker_id == const.ROBOT: Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False, - m_change_robot2ref=None) + m_change_robot_to_head=None) # Enable all navigation buttons choice_ref.Enable(True) @@ -1348,7 +1348,7 @@ class MarkersPanel(wx.Panel): Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status') Publisher.subscribe(self.UpdateSeedCoordinates, 'Update tracts') Publisher.subscribe(self.OnChangeCurrentSession, 'Current session changed') - Publisher.subscribe(self.UpdateRobotCoord, 'Update raw coord') + Publisher.subscribe(self.UpdateRobotCoordinates, 'Update raw coordinates') def __find_target_marker(self): """Return the index of the marker currently selected as target (there @@ -1429,9 +1429,9 @@ class MarkersPanel(wx.Panel): def UpdateSeedCoordinates(self, root=None, affine_vtk=None, coord_offset=(0, 0, 0)): self.current_seed = coord_offset - def UpdateRobotCoord(self, coord_raw, markers_flag): - self.current_head = coord_raw[1] - self.current_robot = coord_raw[2] + def UpdateRobotCoordinates(self, coordinates_raw, markers_flag): + self.current_head = coordinates_raw[1] + self.current_robot = coordinates_raw[2] def OnMouseRightDown(self, evt): # TODO: Enable the "Set as target" only when target is created with registered object @@ -1444,12 +1444,12 @@ class MarkersPanel(wx.Panel): target_menu = menu_id.Append(1, _('Set as target')) menu_id.Bind(wx.EVT_MENU, self.OnMenuSetTarget, target_menu) menu_id.AppendSeparator() - send_target_2_robot = menu_id.Append(3, _('Send target to robot')) - menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTarget2Robot, send_target_2_robot) + send_target_to_robot = menu_id.Append(3, _('Send target to robot')) + menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot) if self.tracker.tracker_id == const.ROBOT: - send_target_2_robot.Enable(True) + send_target_to_robot.Enable(True) else: - send_target_2_robot.Enable(False) + send_target_to_robot.Enable(False) # TODO: Create the remove target option so the user can disable the target without removing the marker # target_menu_rem = menu_id.Append(3, _('Remove target')) # menu_id.Bind(wx.EVT_MENU, self.OnMenuRemoveTarget, target_menu_rem) @@ -1500,7 +1500,7 @@ class MarkersPanel(wx.Panel): Publisher.sendMessage('Set new color', index=index, color=color_new) - def OnMenuSendTarget2Robot(self, evt): + def OnMenuSendTargetToRobot(self, evt): if isinstance(evt, int): self.lc.Focus(evt) @@ -1517,9 +1517,9 @@ class MarkersPanel(wx.Panel): rot = tr.euler_matrix(a, b, g, 'rzyx') m_ref_target = tr.concatenate_matrices(trans, rot) - m_change_robot2ref = np.linalg.inv(m_ref_target) @ m_robot_target + m_change_robot_to_head = np.linalg.inv(m_ref_target) @ m_robot_target - Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot2ref=m_change_robot2ref) + Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot_to_head=m_change_robot_to_head) def OnDeleteAllMarkers(self, evt=None): if evt is None: @@ -1561,7 +1561,7 @@ class MarkersPanel(wx.Panel): if self.__find_target_marker() in index: Publisher.sendMessage('Disable or enable coil tracker', status=False) Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False, - m_change_robot2ref=None) + m_change_robot_to_head=None) wx.MessageBox(_("Target deleted."), _("InVesalius 3")) self.__delete_multiple_markers(index) diff --git a/invesalius/navigation/navigation.py b/invesalius/navigation/navigation.py index 4d80a18..c3f285d 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, self.robottarget_queue = vis_queues + self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_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, [coord_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait() + coord, [coordinates_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait() got_coords = True # print('UpdateScene: get {}'.format(count)) @@ -116,7 +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 raw coordinates', coordinates_raw=coordinates_raw, markers_flag=markers_flag) wx.CallAfter(Publisher.sendMessage, 'Update slice viewer') if view_obj: @@ -147,8 +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.object_at_target_queue = QueueCustom(maxsize=1) + self.robot_target_queue = QueueCustom(maxsize=1) # self.visualization_queue = QueueCustom(maxsize=1) self.serial_port_queue = QueueCustom(maxsize=1) self.coord_tracts_queue = QueueCustom(maxsize=1) @@ -236,7 +236,7 @@ class Navigation(): self.event.clear() vis_components = [self.serial_port_in_use, self.view_tracts, self.peel_loaded] - vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robottarget_queue] + vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue] Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components) @@ -270,7 +270,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, self.objattarget_queue] + queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.object_at_target_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 index 63e6c49..87337db 100644 --- a/invesalius/navigation/robot.py +++ b/invesalius/navigation/robot.py @@ -37,8 +37,8 @@ class Robot(): def __init__(self, tracker): self.tracker = tracker self.trk_init = None - self.robottarget_queue = None - self.objattarget_queue = None + self.robot_target_queue = None + self.object_at_target_queue = None self.process_tracker = None self.thread_robot = None @@ -62,8 +62,8 @@ class Robot(): self.process_tracker = elfin_process.TrackerProcessing() 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 + M_tracker_to_robot = dlg_correg_robot.GetValue() + db.transform_tracker_to_robot.M_tracker_to_robot = M_tracker_to_robot self.robot_server = self.tracker.trk_init[1][0] self.trk_init = self.tracker.trk_init else: @@ -76,31 +76,33 @@ class Robot(): if tracker.event_robot.is_set(): tracker.event_robot.clear() self.thread_robot = ControlRobot(self.trk_init, tracker, self.robotcoordinates, - [coord_queue, self.robottarget_queue, self.objattarget_queue], - self.process_tracker, tracker.event_robot) + [coord_queue, self.robot_target_queue, self.object_at_target_queue], + self.process_tracker, tracker.event_robot) self.thread_robot.start() def StopRobotThreadNavigation(self): self.thread_robot.join() + #TODO: initialize process_tracker every time a "send coordinates to robot" is requested + self.process_tracker.__init__() def OnSendCoordinates(self, coord): self.robot_server.SendCoordinates(coord) - def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot2ref): + def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot_to_head): try: - self.robottarget_queue.put_nowait([robot_tracker_flag, m_change_robot2ref]) + self.robot_target_queue.put_nowait([robot_tracker_flag, m_change_robot_to_head]) except queue.Full: pass def OnObjectTarget(self, state): try: - if self.objattarget_queue: - self.objattarget_queue.put_nowait(state) + if self.object_at_target_queue: + self.object_at_target_queue.put_nowait(state) except queue.Full: pass def SetRobotQueues(self, queues): - self.robottarget_queue, self.objattarget_queue = queues + self.robot_target_queue, self.object_at_target_queue = queues class RobotCoordinates(): @@ -115,7 +117,7 @@ class RobotCoordinates(): class ControlRobot(threading.Thread): - def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event): + def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event_robot): threading.Thread.__init__(self, name='ControlRobot') self.trck_init_robot = trck_init[1][0] @@ -126,20 +128,20 @@ class ControlRobot(threading.Thread): self.robot_tracker_flag = False self.objattarget_flag = False self.target_flag = False - self.m_change_robot2ref = None + self.m_change_robot_to_head = None self.coord_inv_old = None self.coord_queue = queues[0] - self.robottarget_queue = queues[1] - self.objattarget_queue = queues[2] + self.robot_target_queue = queues[1] + self.object_at_target_queue = queues[2] self.process_tracker = process_tracker - self.event = event - self.arcmotion_flag = False - self.arcmotion_step_flag = None - self.target_linearout = None - self.target_linearin = None + self.event_robot = event_robot + self.arc_motion_flag = False + self.arc_motion_step_flag = None + self.target_linear_out = None + self.target_linear_in = None self.target_arc = None - def getcoordsfromdevices(self): + def get_coordinates_from_tracker_devices(self): coord_robot_raw = self.trck_init_robot.Run() coord_robot = np.array(coord_robot_raw) coord_robot[3], coord_robot[5] = coord_robot[5], coord_robot[3] @@ -150,32 +152,32 @@ class ControlRobot(threading.Thread): return coord_raw, coord_robot_raw, markers_flag def robot_move_decision(self, distance_target, coord_inv, coord_robot_raw, current_ref_filtered): - if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arcmotion_flag: + if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag: self.trck_init_robot.SendCoordinates(coord_inv, const.ROBOT_MOTIONS["normal"]) - elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arcmotion_flag: + elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag: actual_point = coord_robot_raw - if not self.arcmotion_flag: + if not self.arc_motion_flag: coord_head = self.process_tracker.estimate_head_center(self.tracker, current_ref_filtered).tolist() - self.target_linearout, self.target_arc = self.process_tracker.arcmotion(coord_robot_raw, coord_head, - coord_inv) - self.arcmotion_flag = True - self.arcmotion_step_flag = const.ROBOT_MOTIONS["linear out"] + self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(coord_robot_raw, coord_head, + coord_inv) + self.arc_motion_flag = True + self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"] - if self.arcmotion_flag and self.arcmotion_step_flag == const.ROBOT_MOTIONS["linear out"]: - coord = self.target_linearout - if np.allclose(np.array(actual_point), np.array(self.target_linearout), 0, 1): - self.arcmotion_step_flag = const.ROBOT_MOTIONS["arc"] + if self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["linear out"]: + coord = self.target_linear_out + if np.allclose(np.array(actual_point), np.array(self.target_linear_out), 0, 1): + self.arc_motion_step_flag = const.ROBOT_MOTIONS["arc"] coord = self.target_arc - elif self.arcmotion_flag and self.arcmotion_step_flag == const.ROBOT_MOTIONS["arc"]: + elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]: coord_head = self.process_tracker.estimate_head_center(self.tracker, current_ref_filtered).tolist() - _, new_target_arc = self.process_tracker.arcmotion(coord_robot_raw, coord_head, - coord_inv) + _, new_target_arc = self.process_tracker.compute_arc_motion(coord_robot_raw, coord_head, + coord_inv) if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1): None else: @@ -186,11 +188,11 @@ class ControlRobot(threading.Thread): coord = self.target_arc if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10): - self.arcmotion_flag = False - self.arcmotion_step_flag = const.ROBOT_MOTIONS["normal"] + self.arc_motion_flag = False + self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"] coord = coord_inv - self.trck_init_robot.SendCoordinates(coord, self.arcmotion_step_flag) + self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag) def control(self, coords_tracker_in_robot, coord_robot_raw, markers_flag): coord_ref_tracker_in_robot = coords_tracker_in_robot[1] @@ -202,7 +204,7 @@ class ControlRobot(threading.Thread): current_ref_filtered = self.process_tracker.kalman_filter(current_ref) if self.process_tracker.head_move_threshold(current_ref_filtered): coord_inv = self.process_tracker.head_move_compensation(current_ref_filtered, - self.m_change_robot2ref) + self.m_change_robot_to_head) if self.coord_inv_old is None: self.coord_inv_old = coord_inv @@ -220,19 +222,19 @@ class ControlRobot(threading.Thread): self.trck_init_robot.StopRobot() def run(self): - while not self.event.is_set(): - coords_tracker_in_robot, coord_robot_raw, markers_flag = self.getcoordsfromdevices() + while not self.event_robot.is_set(): + coords_tracker_in_robot, coord_robot_raw, markers_flag = self.get_coordinates_from_tracker_devices() - if self.robottarget_queue.empty(): + if self.robot_target_queue.empty(): None else: - self.robot_tracker_flag, self.m_change_robot2ref = self.robottarget_queue.get_nowait() - self.robottarget_queue.task_done() + self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait() + self.robot_target_queue.task_done() - if self.objattarget_queue.empty(): + if self.object_at_target_queue.empty(): None else: - self.target_flag = self.objattarget_queue.get_nowait() - self.objattarget_queue.task_done() + self.target_flag = self.object_at_target_queue.get_nowait() + self.object_at_target_queue.task_done() self.control(coords_tracker_in_robot, coord_robot_raw, markers_flag) diff --git a/invesalius/navigation/tracker.py b/invesalius/navigation/tracker.py index 3e0cdf4..869ca78 100644 --- a/invesalius/navigation/tracker.py +++ b/invesalius/navigation/tracker.py @@ -92,7 +92,7 @@ class Tracker(): print("Tracker still connected!") def ConnectToRobot(self, navigation, tracker, robot): - robot.SetRobotQueues([navigation.robottarget_queue, navigation.objattarget_queue]) + robot.SetRobotQueues([navigation.robot_target_queue, navigation.object_at_target_queue]) robot.OnRobotConnection() trk_init_robot = self.trk_init[1][0] if trk_init_robot: -- libgit2 0.21.2