From 3a9ab9cd0df30d332aba58994620b8b4b83e280a Mon Sep 17 00:00:00 2001 From: Renan Date: Mon, 4 Apr 2022 20:37:40 +0300 Subject: [PATCH] Decouple robot control (#404) --- invesalius/constants.py | 12 ------------ invesalius/data/bases.py | 50 ++++++-------------------------------------------- invesalius/data/coordinates.py | 54 ++++++++++++++++++++---------------------------------- invesalius/data/coregistration.py | 31 ++++++++++++++++++++++++++++--- invesalius/data/elfin.py | 252 ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ invesalius/data/elfin_processing.py | 212 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- invesalius/data/trackers.py | 39 +++++++++++---------------------------- invesalius/gui/dialogs.py | 87 ++++++++++++++++++++++++++++++++++++++------------------------------------------------- invesalius/gui/task_navigator.py | 140 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++----------------------------------------------------------------- invesalius/navigation/icp.py | 6 ++---- invesalius/navigation/navigation.py | 32 ++++++++++++++------------------ invesalius/navigation/robot.py | 308 -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- invesalius/navigation/tracker.py | 13 +------------ invesalius/net/remote_control.py | 2 ++ 14 files changed, 197 insertions(+), 1041 deletions(-) delete mode 100644 invesalius/data/elfin.py delete mode 100644 invesalius/data/elfin_processing.py delete mode 100644 invesalius/navigation/robot.py diff --git a/invesalius/constants.py b/invesalius/constants.py index adb8d48..982ed9d 100644 --- a/invesalius/constants.py +++ b/invesalius/constants.py @@ -832,7 +832,6 @@ SEED_RADIUS = 1.5 # Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation. SLEEP_NAVIGATION = 0.2 SLEEP_COORDINATES = 0.05 -SLEEP_ROBOT = 0.01 BRAIN_OPACITY = 0.6 N_CPU = psutil.cpu_count() @@ -864,14 +863,3 @@ PULSE_DURATION_IN_MILLISECONDS = 0.2 #Robot ROBOT_ElFIN_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 #mm/s -ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm -ROBOT_VERSOR_SCALE_FACTOR = 70 -#Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%. -ROBOT_WORKING_SPACE = 760 #mm -ROBOT_MOVE_STATE = {"free to move": 0, - "in motion": 1009, - "waiting for execution": 1013, - "error": 1025} diff --git a/invesalius/data/bases.py b/invesalius/data/bases.py index f982319..41a1e49 100644 --- a/invesalius/data/bases.py +++ b/invesalius/data/bases.py @@ -163,6 +163,12 @@ def transform_icp(m_img, m_icp): return m_img +def inverse_transform_icp(m_img, m_icp): + coord_img = [m_img[0, -1], -m_img[1, -1], m_img[2, -1], 1] + m_img[0, -1], m_img[1, -1], m_img[2, -1], _ = np.linalg.inv(m_icp) @ coord_img + m_img[0, -1], m_img[1, -1], m_img[2, -1] = m_img[0, -1], -m_img[1, -1], m_img[2, -1] + + return m_img def object_registration(fiducials, orients, coord_raw, m_change): """ @@ -244,47 +250,3 @@ 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 - -def compute_robot_to_head_matrix(raw_target_robot): - """ - :param head: nx6 array of head coordinates from tracking device in robot space - :param robot: nx6 array of robot coordinates - - :return: target_robot_matrix: 3x3 array representing change of basis from robot to head in robot system - """ - head, robot = raw_target_robot - # compute head target matrix - m_head_target = dco.coordinates_to_transformation_matrix( - position=head[:3], - orientation=head[3:], - axes='rzyx', - ) - - # compute robot target matrix - m_robot_target = dco.coordinates_to_transformation_matrix( - position=robot[:3], - orientation=robot[3:], - axes='rzyx', - ) - robot_to_head_matrix = np.linalg.inv(m_head_target) @ m_robot_target - - return robot_to_head_matrix - - -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(): - return None - - M_tracker = dco.coordinates_to_transformation_matrix( - position=tracker_coord[:3], - orientation=tracker_coord[3:6], - axes='rzyx', - ) - M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker - - translation, angles_as_deg = dco.transformation_matrix_to_coordinates(M_tracker_in_robot, axes='rzyx') - tracker_in_robot = list(translation) + list(angles_as_deg) - - return tracker_in_robot diff --git a/invesalius/data/coordinates.py b/invesalius/data/coordinates.py index d952929..8a91e83 100644 --- a/invesalius/data/coordinates.py +++ b/invesalius/data/coordinates.py @@ -22,7 +22,6 @@ import numpy as np import threading import wx -import invesalius.data.bases as db import invesalius.data.transformations as tr import invesalius.constants as const @@ -47,14 +46,20 @@ class TrackerCoordinates(): def SetCoordinates(self, coord, markers_flag): self.coord = coord self.markers_flag = markers_flag - if self.previous_markers_flag != self.markers_flag and not self.nav_status: - wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag) - self.previous_markers_flag = self.markers_flag + if not self.nav_status: + wx.CallAfter(Publisher.sendMessage, 'Update tracker coordinates', + coord=self.coord.tolist(), markers_flag=self.markers_flag) + if self.previous_markers_flag != self.markers_flag: + wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag) + self.previous_markers_flag = self.markers_flag def GetCoordinates(self): - if self.previous_markers_flag != self.markers_flag and self.nav_status: - wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag) - self.previous_markers_flag = self.markers_flag + if self.nav_status: + wx.CallAfter(Publisher.sendMessage, 'Update tracker coordinates', + coord=self.coord.tolist(), markers_flag=self.markers_flag) + if self.previous_markers_flag != self.markers_flag: + wx.CallAfter(Publisher.sendMessage, 'Sensors ID', markers_flag=self.markers_flag) + self.previous_markers_flag = self.markers_flag return self.coord, self.markers_flag @@ -195,16 +200,6 @@ def PolarisCoord(trck_init, trck_id, ref_mode): return coord, [trck.probeID, trck.refID, trck.objID] -def ElfinCoord(trck_init): - if len(trck_init) > 2: - robotcoordinates = trck_init[-1] - coord = robotcoordinates.GetRobotCoordinates() - if coord is None: - coord = np.array([0, 0, 0, 0, 0, 0]) - else: - coord = np.array([0, 0, 0, 0, 0, 0]) - - return coord def CameraCoord(trck_init, trck_id, ref_mode): trck = trck_init[0] @@ -346,6 +341,13 @@ def PolhemusSerialCoord(trck_init, trck_id, ref_mode): return coord +def RobotCoord(trk_init, trck_id, ref_mode): + #trck_id is the tracker related to the robot ID. To get the tracker ID, combined with the robot, + # it is required to get trk_init[1] + tracker_id = trk_init[1] + coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], tracker_id, ref_mode) + + return np.vstack([coord_tracker[0], coord_tracker[1], coord_tracker[2]]), markers_flag def DebugCoordRandom(trk_init, trck_id, ref_mode): """ @@ -497,22 +499,6 @@ def dynamic_reference_m(probe, reference): return coord_rot -def RobotCoord(trk_init, trck_id, ref_mode): - coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0][0], trk_init[1], ref_mode) - coord_robot = ElfinCoord([trk_init[0][1]]+trk_init[1:]) - - 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] - ref_tracker_in_robot = coord_tracker[1] - obj_tracker_in_robot = coord_tracker[2] - - return np.vstack([probe_tracker_in_robot, ref_tracker_in_robot, coord_robot, obj_tracker_in_robot]), markers_flag - - def dynamic_reference_m2(probe, reference): """ Apply dynamic reference correction to probe coordinates. Uses the alpha, beta and gama @@ -590,4 +576,4 @@ class ReceiveCoordinates(threading.Thread): while not self.event.is_set(): coord_raw, markers_flag = GetCoordinatesForThread(self.trck_init, self.trck_id, const.DEFAULT_REF_MODE) self.TrackerCoordinates.SetCoordinates(coord_raw, markers_flag) - sleep(const.SLEEP_COORDINATES) \ No newline at end of file + sleep(const.SLEEP_COORDINATES) diff --git a/invesalius/data/coregistration.py b/invesalius/data/coregistration.py index 14625c9..690f9ef 100644 --- a/invesalius/data/coregistration.py +++ b/invesalius/data/coregistration.py @@ -101,6 +101,32 @@ def tracker_to_image(m_change, m_probe_ref, r_obj_img, m_obj_raw, s0_dyn): m_img[:3, :3] = r_obj[:3, :3] return m_img +def image_to_tracker(m_change, target, icp): + """Compute the transformation matrix to the tracker coordinate system + + :param m_change: Corregistration transformation obtained from fiducials + :type m_change: numpy.ndarray + :param target: Target in invesalius coordinate system + :type target: numpy.ndarray + :param icp: ICP transformation matrix + :type icp: numpy.ndarray + + :return: 4 x 4 numpy double array + :rtype: numpy.ndarray + """ + m_target_in_image = dco.coordinates_to_transformation_matrix( + position=target[:3], + orientation=[0, 0, 0], + axes='sxyz', + ) + if icp.use_icp: + m_target_in_image = bases.inverse_transform_icp(m_target_in_image, icp.m_icp) + m_target_in_tracker = np.linalg.inv(m_change) @ m_target_in_image + + # invert y coordinate + m_target_in_tracker[2, -1] = -m_target_in_tracker[2, -1] + + return m_target_in_tracker def corregistrate_object_dynamic(inp, coord_raw, ref_mode_id, icp): @@ -193,7 +219,6 @@ class CoordinateCorregistrate(threading.Thread): self.object_at_target_queue = queues[3] self.icp = None self.m_icp = None - self.robot_tracker_flag = None self.last_coord = None self.tracker_id = tracker_id self.target = target @@ -254,7 +279,7 @@ class CoordinateCorregistrate(threading.Thread): if self.icp: m_img = bases.transform_icp(m_img, self.m_icp) - self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj]) + self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj]) # print('CoordCoreg: put {}'.format(count)) # count += 1 @@ -307,7 +332,7 @@ class CoordinateCorregistrateNoObject(threading.Thread): if self.icp: m_img = bases.transform_icp(m_img, self.m_icp) - self.coord_queue.put_nowait([coord, [coord_raw, markers_flag], m_img, view_obj]) + self.coord_queue.put_nowait([coord, markers_flag, m_img, view_obj]) if self.view_tracts: self.coord_tracts_queue.put_nowait(m_img_flip) diff --git a/invesalius/data/elfin.py b/invesalius/data/elfin.py deleted file mode 100644 index e056fd7..0000000 --- a/invesalius/data/elfin.py +++ /dev/null @@ -1,252 +0,0 @@ -#!/usr/bin/env python3 - -import sys -from time import sleep -from socket import socket, AF_INET, SOCK_STREAM -import invesalius.constants as const - -class Elfin_Server(): - """ - This class is similar to tracker devices wrappers. - It follows the same functions as the others (Initialize, Run and Close) - """ - def __init__(self, server_ip, port_number): - self.server_ip = server_ip - self.port_number = port_number - - def Initialize(self): - message_size = 1024 - robot_id = 0 - self.cobot = Elfin() - self.cobot.connect(self.server_ip, self.port_number, message_size, robot_id) - print("connected!") - - def Run(self): - return self.cobot.ReadPcsActualPos() - - def SendCoordinates(self, target, motion_type=const.ROBOT_MOTIONS["normal"]): - """ - It's not possible to send a move command to elfin if the robot is during a move. - Status 1009 means robot in motion. - """ - status = self.cobot.ReadMoveState() - if status == const.ROBOT_MOVE_STATE["free to move"]: - if motion_type == const.ROBOT_MOTIONS["normal"] or motion_type == const.ROBOT_MOTIONS["linear out"]: - self.cobot.MoveL(target) - elif motion_type == const.ROBOT_MOTIONS["arc"]: - self.cobot.MoveC(target) - elif status == const.ROBOT_MOVE_STATE["error"]: - self.StopRobot() - - def StopRobot(self): - # Takes some microseconds to the robot actual stops after the command. - # The sleep time is required to guarantee the stop - self.cobot.GrpStop() - sleep(0.1) - - def Close(self): - self.StopRobot() - #TODO: robot function to close? self.cobot.close() - -class Elfin: - def __init__(self): - """ - Class to communicate with elfin robot. This class follows "HansRobot Communication Protocol Interface". - """ - self.end_msg = ",;" - - def connect(self, server_ip, port_number, message_size, robot_id): - mySocket = socket(AF_INET, SOCK_STREAM) - mySocket.connect((server_ip, port_number)) - - 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.message_size).decode('utf-8').split(',') - status = self.check_status(data) - if status and type(data) != bool: - if len(data) > 3: - return data[2:-1] - return status - - def check_status(self, recv_message): - status = recv_message[1] - if status == 'OK': - return True - - elif status == 'Fail': - print("Error code: ", recv_message[2]) - return False - - def Electrify(self): - """ - Function: Power the robot - Notes: successful completion of power up before returning, power up time is - about 44s. - :return: - if Error Return False - if not Error Return True - """ - message = "Electrify" + self.end_msg - status = self.send(message) - return status - - def BlackOut(self): - """ - Function: Robot blackout - Notes: successful power outage will only return, power failure time is 3s. - :return: - if Error Return False - if not Error Return True - """ - message = "BlackOut" + self.end_msg - status = self.send(message) - return status - - def StartMaster(self): - """ - Function: Start master station - Notes: the master station will not be returned until successfully started, startup - master time is about 4s. - :return: - if Error Return False - if not Error Return True - """ - message = "StartMaster" + self.end_msg - status = self.send(message) - return status - - def CloseMaster(self): - """ - Function: Close master station - Notes: the master station will not be returned until successfully closed, shut - down the master station time is about 2s. - :return: - if Error Return False - if not Error Return True - """ - message = "CloseMaster" + self.end_msg - status = self.send(message) - return status - - def GrpPowerOn(self): - """ - Function: Robot servo on - :return: - if Error Return False - if not Error Return True - """ - message = "GrpPowerOn," + self.robot_id + self.end_msg - status = self.send(message) - return status - - def GrpPowerOff(self): - """ - Function: Robot servo off - :return: - if Error Return False - if not Error Return True - """ - message = "GrpPowerOff," + self.robot_id + self.end_msg - status = self.send(message) - return status - - def GrpStop(self): - """ - Function: Stop robot - :return: - if Error Return False - if not Error Return True - """ - message = "GrpStop," + self.robot_id + self.end_msg - status = self.send(message) - return status - - def SetOverride(self, override): - """ - function: Set speed ratio - :param override: - double: set speed ratio, range of 0.01~1 - :return: - if Error Return False - if not Error Return True - """ - - message = "SetOverride," + self.robot_id + ',' + str(override) + self.end_msg - status = self.send(message) - return status - - def ReadPcsActualPos(self): - """Function: Get the actual position of the space coordinate - :return: - if True Return x,y,z,a,b,c - if Error Return False - """ - message = "ReadPcsActualPos," + self.robot_id + self.end_msg - coord = self.send(message) - if coord: - return [float(s) for s in coord] - - return coord - - def MoveL(self, target): - """ - function: Robot moves straight to the specified space coordinates - :param: target:[X,Y,Z,RX,RY,RZ] - :return: - """ - target = [str(s) for s in target] - target = (",".join(target)) - message = "MoveL," + self.robot_id + ',' + target + self.end_msg - return self.send(message) - - def SetToolCoordinateMotion(self, status): - """ - function: Function: Set tool coordinate motion - :param: int Switch 0=close 1=open - :return: - if Error Return False - if not Error Return True - """ - message = "SetToolCoordinateMotion," + self.robot_id + ',' + str(status) + self.end_msg - status = self.send(message) - return status - - def ReadMoveState(self): - """ - Function: Get the motion state of the robot - :return: - Current state of motion of robot: - 0=motion completion; - 1009=in motion; - 1013=waiting for execution; - 1025 =Error reporting - """ - message = "ReadMoveState," + self.robot_id + self.end_msg - status = int(self.send(message)[0]) - return status - - def MoveHoming(self): - """ - Function: Robot returns to the origin - :return: - if Error Return False - if not Error Return True - """ - message = "MoveHoming," + self.robot_id + self.end_msg - status = self.send(message) - return status - - def MoveC(self, target): - """ - function: Arc motion - :param: Through position[X,Y,Z],GoalCoord[X,Y,Z,RX,RY,RZ],Type[0 or 1],; - :return: - """ - target = [str(s) for s in target] - target = (",".join(target)) - 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 deleted file mode 100644 index 69d3512..0000000 --- a/invesalius/data/elfin_processing.py +++ /dev/null @@ -1,212 +0,0 @@ -#-------------------------------------------------------------------------- -# 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 numpy as np -import cv2 -from time import time - -import invesalius.data.coregistration as dcr -import invesalius.data.coordinates as dco -import invesalius.constants as const - - -class KalmanTracker: - """ - Kalman filter to avoid sudden fluctuation from tracker device. - The filter strength can be set by the cov_process, and cov_measure parameter - It is required to create one instance for each variable (x, y, z, a, b, g) - """ - def __init__(self, - state_num=2, - covariance_process=0.001, - covariance_measure=0.1): - - self.state_num = state_num - measure_num = 1 - - # The filter itself. - self.filter = cv2.KalmanFilter(state_num, measure_num, 0) - - self.state = np.zeros((state_num, 1), dtype=np.float32) - self.measurement = np.array((measure_num, 1), np.float32) - self.prediction = np.zeros((state_num, 1), np.float32) - - - self.filter.transitionMatrix = np.array([[1, 1], - [0, 1]], np.float32) - self.filter.measurementMatrix = np.array([[1, 1]], np.float32) - self.filter.processNoiseCov = np.array([[1, 0], - [0, 1]], np.float32) * covariance_process - self.filter.measurementNoiseCov = np.array([[1]], np.float32) * covariance_measure - - def update_kalman(self, measurement): - self.prediction = self.filter.predict() - self.measurement = np.array([[np.float32(measurement[0])]]) - - self.filter.correct(self.measurement) - self.state = self.filter.statePost - - -class TrackerProcessing: - def __init__(self): - self.coord_vel = [] - self.timestamp = [] - self.velocity_vector = [] - self.kalman_coord_vector = [] - self.velocity_std = 0 - - self.tracker_stabilizers = [KalmanTracker( - state_num=2, - covariance_process=0.001, - covariance_measure=0.1) for _ in range(6)] - - def kalman_filter(self, coord_tracker): - kalman_array = [] - pose_np = np.array((coord_tracker[:3], coord_tracker[3:])).flatten() - for value, ps_stb in zip(pose_np, self.tracker_stabilizers): - ps_stb.update_kalman([value]) - kalman_array.append(ps_stb.state[0]) - coord_kalman = np.hstack(kalman_array) - - self.kalman_coord_vector.append(coord_kalman[:3]) - if len(self.kalman_coord_vector) < 20: #avoid initial fluctuations - coord_kalman = coord_tracker - print('initializing filter') - else: - del self.kalman_coord_vector[0] - - return coord_kalman - - 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]) - distance = (coord_final - coord_init) - - return velocity, distance - - def compute_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 - versor_factor = (((final_point-init_point) / norm) * const.ROBOT_VERSOR_SCALE_FACTOR).tolist() - - return versor_factor - - - def compute_arc_motion(self, current_robot_coordinates, head_center_coordinates, new_robot_coordinates): - head_center = head_center_coordinates[0], head_center_coordinates[1], head_center_coordinates[2], \ - new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5] - - versor_factor_move_out = self.compute_versors(head_center, current_robot_coordinates) - init_move_out_point = current_robot_coordinates[0] + versor_factor_move_out[0], \ - current_robot_coordinates[1] + versor_factor_move_out[1], \ - current_robot_coordinates[2] + versor_factor_move_out[2], \ - current_robot_coordinates[3], current_robot_coordinates[4], current_robot_coordinates[5] - - middle_point = ((new_robot_coordinates[0] + current_robot_coordinates[0]) / 2, - (new_robot_coordinates[1] + current_robot_coordinates[1]) / 2, - (new_robot_coordinates[2] + current_robot_coordinates[2]) / 2, - 0, 0, 0) - versor_factor_middle_arc = (np.array(self.compute_versors(head_center, middle_point))) * 2 - middle_arc_point = middle_point[0] + versor_factor_middle_arc[0], \ - middle_point[1] + versor_factor_middle_arc[1], \ - middle_point[2] + versor_factor_middle_arc[2] - - versor_factor_arc = self.compute_versors(head_center, new_robot_coordinates) - final_ext_arc_point = new_robot_coordinates[0] + versor_factor_arc[0], \ - new_robot_coordinates[1] + versor_factor_arc[1], \ - new_robot_coordinates[2] + versor_factor_arc[2], \ - new_robot_coordinates[3], new_robot_coordinates[4], new_robot_coordinates[5], 0 - - target_arc = middle_arc_point + final_ext_arc_point - - return init_move_out_point, target_arc - - def compute_head_move_threshold(self, current_ref): - """ - Checks if the head velocity is bellow the threshold - """ - self.coord_vel.append(current_ref) - self.timestamp.append(time()) - if len(self.coord_vel) >= 10: - head_velocity, head_distance = self.estimate_head_velocity(self.coord_vel, self.timestamp) - self.velocity_vector.append(head_velocity) - - del self.coord_vel[0] - del self.timestamp[0] - - if len(self.velocity_vector) >= 30: - self.velocity_std = np.std(self.velocity_vector) - del self.velocity_vector[0] - - if self.velocity_std > const.ROBOT_HEAD_VELOCITY_THRESHOLD: - print('Velocity threshold activated') - return False - else: - return True - - return False - - def compute_head_move_compensation(self, current_head, m_change_robot_to_head): - """ - Estimates the new robot position to reach the target - """ - M_current_head = dco.coordinates_to_transformation_matrix( - position=current_head[:3], - orientation=current_head[3:6], - axes='rzyx', - ) - m_robot_new = M_current_head @ m_change_robot_to_head - - translation, angles_as_deg = dco.transformation_matrix_to_coordinates(m_robot_new, axes='sxyz') - new_robot_position = list(translation) + list(angles_as_deg) - - return new_robot_position - - def estimate_head_center(self, tracker, current_head): - """ - Estimates the actual head center position using fiducials - """ - m_probe_head_left, m_probe_head_right, m_probe_head_nasion = tracker.GetMatrixTrackerFiducials() - m_current_head = dcr.compute_marker_transformation(np.array([current_head]), 0) - - m_ear_left_new = m_current_head @ m_probe_head_left - m_ear_right_new = m_current_head @ m_probe_head_right - - return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2 - - def correction_distance_calculation_target(self, coord_inv, actual_point): - """ - Estimates the Euclidean distance between the actual position and the target - """ - correction_distance_compensation = np.sqrt((coord_inv[0]-actual_point[0]) ** 2 + - (coord_inv[1]-actual_point[1]) ** 2 + - (coord_inv[2]-actual_point[2]) ** 2) - - return correction_distance_compensation - - def estimate_robot_target_length(self, robot_target): - """ - Estimates the length of the 3D vector of the robot target - """ - robot_target_length = np.sqrt(robot_target[0] ** 2 + robot_target[1] ** 2 + robot_target[2] ** 2) - - return robot_target_length - diff --git a/invesalius/data/trackers.py b/invesalius/data/trackers.py index 9ca70a5..50bbc15 100644 --- a/invesalius/data/trackers.py +++ b/invesalius/data/trackers.py @@ -18,6 +18,7 @@ #-------------------------------------------------------------------------- import invesalius.constants as const import invesalius.gui.dialogs as dlg +from invesalius.pubsub import pub as Publisher # TODO: Disconnect tracker when a new one is connected # TODO: Test if there are too many prints when connection fails # TODO: Redesign error messages. No point in having "Could not connect to default tracker" in all trackers @@ -223,42 +224,23 @@ def OptitrackTracker(tracker_id): print('#####') return trck_init, lib_mode -def ElfinRobot(robot_IP): - trck_init = None - try: - import invesalius.data.elfin as elfin - print("Trying to connect Robot via: ", robot_IP) - trck_init = elfin.Elfin_Server(robot_IP, const.ROBOT_ElFIN_PORT) - trck_init.Initialize() - lib_mode = 'wrapper' - print('Connect to elfin robot tracking device.') - - except: - lib_mode = 'error' - trck_init = None - print('Could not connect to elfin robot tracker.') - - # return tracker initialization variable and type of connection - return trck_init, lib_mode - def RobotTracker(tracker_id): from wx import ID_OK - trck_init = None - trck_init_robot = None - tracker_id = None + dlg_device = dlg.SetTrackerDeviceToRobot() if dlg_device.ShowModal() == ID_OK: tracker_id = dlg_device.GetValue() if tracker_id: - trck_connection = TrackerConnection(tracker_id, None, 'connect') - if trck_connection[0]: + trck_init = TrackerConnection(tracker_id, None, 'connect') + if trck_init[0]: dlg_ip = dlg.SetRobotIP() if dlg_ip.ShowModal() == ID_OK: robot_IP = dlg_ip.GetValue() - trck_init = trck_connection - trck_init_robot = ElfinRobot(robot_IP) + Publisher.sendMessage('Connect to robot', robot_IP=robot_IP) + + return trck_init, tracker_id - return [(trck_init, trck_init_robot), tracker_id] + return None, None def DebugTrackerRandom(tracker_id): trck_init = True @@ -392,8 +374,9 @@ def DisconnectTracker(tracker_id, trck_init): lib_mode = 'serial' print('Tracker disconnected.') elif tracker_id == const.ROBOT: - trck_init[0][0].Close() - trck_init[1][0].Close() + Publisher.sendMessage('Reset robot', data=None) + if trck_init[1] == const.DEBUGTRACKRANDOM or trck_init[1] == const.DEBUGTRACKAPPROACH: + trck_init[0].Close() trck_init = False lib_mode = 'wrapper' print('Tracker disconnected.') diff --git a/invesalius/gui/dialogs.py b/invesalius/gui/dialogs.py index 8d6f615..4ecb2e3 100644 --- a/invesalius/gui/dialogs.py +++ b/invesalius/gui/dialogs.py @@ -57,8 +57,6 @@ except ImportError: from wx import AboutDialogInfo, AboutBox import invesalius.constants as const -import invesalius.data.coordinates as dco -import invesalius.data.transformations as tr import invesalius.gui.widgets.gradient as grad import invesalius.session as ses import invesalius.utils as utils @@ -3597,11 +3595,7 @@ class ObjectCalibrationDialog(wx.Dialog): # and not change the function to the point of potentially breaking it.) # if self.obj_ref_id and fiducial_index == 4: - if self.tracker_id == const.ROBOT: - trck_init_robot = self.trk_init[1][0] - coord = trck_init_robot.Run() - else: - coord = coord_raw[self.obj_ref_id, :] + coord = coord_raw[self.obj_ref_id, :] else: coord = coord_raw[0, :] @@ -4323,7 +4317,7 @@ class SetTrackerDeviceToRobot(wx.Dialog): def _init_gui(self): # ComboBox for spatial tracker device selection tooltip = wx.ToolTip(_("Choose the tracking device")) - trackers = const.TRACKERS + trackers = const.TRACKERS.copy() if not ses.Session().debug: del trackers[-3:] tracker_options = [_("Select tracker:")] + trackers @@ -4425,10 +4419,8 @@ class CreateTransformationMatrixRobot(wx.Dialog): M_robot_2_tracker is created by an affine transformation. Robot TCP should be calibrated to the center of the tracker marker ''' #TODO: make aboutbox - self.tracker_coord = [] - self.tracker_angles = [] - self.robot_coord = [] - self.robot_angles = [] + self.matrix_tracker_to_robot = [] + self.robot_status = False self.tracker = tracker @@ -4470,6 +4462,8 @@ class CreateTransformationMatrixRobot(wx.Dialog): btn_load = wx.Button(self, -1, label=_('Load'), size=wx.Size(65, 23)) btn_load.Bind(wx.EVT_BUTTON, self.OnLoadReg) + btn_load.Enable(False) + self.btn_load = btn_load # Create a horizontal sizers border = 1 @@ -4524,16 +4518,17 @@ class CreateTransformationMatrixRobot(wx.Dialog): main_sizer.Fit(self) self.CenterOnParent() + self.__bind_events() - def affine_correg(self, tracker, robot): - m_change = tr.affine_matrix_from_points(robot[:].T, tracker[:].T, - shear=False, scale=False, usesvd=False) - return m_change + def __bind_events(self): + Publisher.subscribe(self.OnUpdateTransformationMatrix, 'Update robot transformation matrix') + Publisher.subscribe(self.OnCoordinatesAdquired, 'Coordinates for the robot transformation matrix collected') + Publisher.subscribe(self.OnRobotConnectionStatus, 'Robot connection status') def OnContinuousAcquisition(self, evt=None, btn=None): value = btn.GetValue() if value: - self.timer.Start(500) + self.timer.Start(100) else: self.timer.Stop() @@ -4541,56 +4536,50 @@ class CreateTransformationMatrixRobot(wx.Dialog): self.OnCreatePoint(evt=None) def OnCreatePoint(self, evt): - coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates() - #robot thread is not initialized yet - coord_raw_robot = self.tracker.trk_init[0][1][0].Run() - coord_raw_tracker_obj = coord_raw[3] - - if markers_flag[2]: - self.tracker_coord.append(coord_raw_tracker_obj[:3]) - self.tracker_angles.append(coord_raw_tracker_obj[3:]) - self.robot_coord.append(coord_raw_robot[:3]) - self.robot_angles.append(coord_raw_robot[3:]) - self.txt_number.SetLabel(str(int(self.txt_number.GetLabel())+1)) - else: - print('Cannot detect the coil markers, pls try again') + Publisher.sendMessage('Collect coordinates for the robot transformation matrix', data=None) + + def OnCoordinatesAdquired(self): + self.txt_number.SetLabel(str(int(self.txt_number.GetLabel())+1)) - if len(self.tracker_coord) >= 3: + if self.robot_status and int(self.txt_number.GetLabel()) >= 3: self.btn_apply_reg.Enable(True) + def OnRobotConnectionStatus(self, data): + self.robot_status = data + if self.robot_status: + self.btn_load.Enable(True) + if int(self.txt_number.GetLabel()) >= 3: + self.btn_apply_reg.Enable(True) + def OnReset(self, evt): + Publisher.sendMessage('Reset coordinates collection for the robot transformation matrix', data=None) if self.btn_cont_point: self.btn_cont_point.SetValue(False) self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point) - self.tracker_coord = [] - self.tracker_angles = [] - self.robot_coord = [] - self.robot_angles = [] - self.M_tracker_2_robot = [] self.txt_number.SetLabel('0') self.btn_apply_reg.Enable(False) self.btn_save.Enable(False) self.btn_ok.Enable(False) + self.matrix_tracker_to_robot = [] + def OnApply(self, evt): if self.btn_cont_point: self.btn_cont_point.SetValue(False) self.OnContinuousAcquisition(evt=None, btn=self.btn_cont_point) - tracker_coord = np.array(self.tracker_coord) - robot_coord = np.array(self.robot_coord) - - M_robot_2_tracker = self.affine_correg(tracker_coord, robot_coord) - M_tracker_2_robot = tr.inverse_matrix(M_robot_2_tracker) - self.M_tracker_2_robot = M_tracker_2_robot + Publisher.sendMessage('Robot transformation matrix estimation', data=None) self.btn_save.Enable(True) self.btn_ok.Enable(True) #TODO: make a colored circle to sinalize that the transformation was made (green) (red if not) + def OnUpdateTransformationMatrix(self, data): + self.matrix_tracker_to_robot = np.array(data) + def OnSaveReg(self, evt): filename = ShowLoadSaveDialog(message=_(u"Save robot transformation file as..."), wildcard=_("Robot transformation files (*.rbtf)|*.rbtf"), @@ -4598,10 +4587,10 @@ class CreateTransformationMatrixRobot(wx.Dialog): default_filename="robottransform.rbtf", save_ext="rbtf") if filename: - if self.M_tracker_2_robot is not None: + if self.matrix_tracker_to_robot is not None: with open(filename, 'w', newline='') as file: writer = csv.writer(file, delimiter='\t') - writer.writerows(self.M_tracker_2_robot) + writer.writerows(np.vstack(self.matrix_tracker_to_robot).tolist()) def OnLoadReg(self, evt): filename = ShowLoadSaveDialog(message=_(u"Load robot transformation"), @@ -4611,12 +4600,12 @@ class CreateTransformationMatrixRobot(wx.Dialog): reader = csv.reader(file, delimiter='\t') content = [row for row in reader] - self.M_tracker_2_robot = np.vstack(list(np.float_(content))) - print("Matrix tracker to robot:", self.M_tracker_2_robot) - self.btn_ok.Enable(True) + self.matrix_tracker_to_robot = np.vstack(list(np.float_(content))) + print("Matrix tracker to robot:", self.matrix_tracker_to_robot) + Publisher.sendMessage('Load robot transformation matrix', data=self.matrix_tracker_to_robot.tolist()) + if self.robot_status: + self.btn_ok.Enable(True) - def GetValue(self): - return self.M_tracker_2_robot class SetNDIconfigs(wx.Dialog): def __init__(self, title=_("Setting NDI polaris configs:")): diff --git a/invesalius/gui/task_navigator.py b/invesalius/gui/task_navigator.py index 3fe7759..8cd0bb1 100644 --- a/invesalius/gui/task_navigator.py +++ b/invesalius/gui/task_navigator.py @@ -20,10 +20,7 @@ import dataclasses from functools import partial import itertools -import csv -import queue import time -import threading import nibabel as nb import numpy as np @@ -41,7 +38,6 @@ except ImportError: has_robot = False import wx -import vtk try: import wx.lib.agw.foldpanelbar as fpb @@ -51,7 +47,6 @@ except ImportError: import wx.lib.colourselect as csel import wx.lib.masked.numctrl from invesalius.pubsub import pub as Publisher -from time import sleep import invesalius.constants as const @@ -64,6 +59,7 @@ import invesalius.data.tractography as dti import invesalius.data.record_coords as rec import invesalius.data.vtk_utils as vtk_utils import invesalius.data.bases as db +import invesalius.data.coregistration as dcr import invesalius.gui.dialogs as dlg import invesalius.project as prj import invesalius.session as ses @@ -73,7 +69,6 @@ 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 from invesalius.data.converters import to_vtk from invesalius.net.neuronavigation_api import NeuronavigationApi @@ -176,7 +171,7 @@ class InnerFoldPanel(wx.Panel): # tracker = Tracker() pedal_connection = PedalConnection() if HAS_PEDAL_CONNECTION else None - + icp = ICP() neuronavigation_api = NeuronavigationApi() navigation = Navigation( pedal_connection=pedal_connection, @@ -194,6 +189,7 @@ class InnerFoldPanel(wx.Panel): parent=item, navigation=navigation, tracker=tracker, + icp=icp, pedal_connection=pedal_connection, neuronavigation_api=neuronavigation_api, ) @@ -213,7 +209,7 @@ class InnerFoldPanel(wx.Panel): # Fold 3 - Markers panel item = fold_panel.AddFoldPanel(_("Markers"), collapsed=True) - mtw = MarkersPanel(item, navigation, tracker) + mtw = MarkersPanel(item, navigation, tracker, icp) fold_panel.ApplyCaptionStyle(item, style) fold_panel.AddFoldPanelWindow(item, mtw, spacing= 0, @@ -352,7 +348,7 @@ class InnerFoldPanel(wx.Panel): class NeuronavigationPanel(wx.Panel): - def __init__(self, parent, navigation, tracker, pedal_connection, neuronavigation_api): + def __init__(self, parent, navigation, tracker, icp, pedal_connection, neuronavigation_api): wx.Panel.__init__(self, parent) try: default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR) @@ -369,9 +365,8 @@ class NeuronavigationPanel(wx.Panel): self.neuronavigation_api = neuronavigation_api self.navigation = navigation - self.icp = ICP() + self.icp = icp self.tracker = tracker - self.robot = Robot(tracker) self.nav_status = False self.tracker_fiducial_being_set = None @@ -558,6 +553,7 @@ class NeuronavigationPanel(wx.Panel): Publisher.subscribe(self.UpdateTarget, 'Update target') Publisher.subscribe(self.OnStartNavigation, 'Start navigation') Publisher.subscribe(self.OnStopNavigation, 'Stop navigation') + Publisher.subscribe(self.OnDialogRobotDestroy, 'Dialog robot destroy') def LoadImageFiducials(self, label, coord): fiducial = self.GetFiducialByAttribute(const.IMAGE_FIDUCIALS, 'label', label) @@ -671,8 +667,6 @@ class NeuronavigationPanel(wx.Panel): self.checkbox_icp.SetValue(False) def OnDisconnectTracker(self): - if self.tracker.tracker_id == const.ROBOT: - self.robot.StopRobotThreadNavigation() self.tracker.DisconnectTracker() self.ResetICP() self.tracker.UpdateUI(self.select_tracker_elem, self.numctrls_fiducial[3:6], self.txtctrl_fre) @@ -691,7 +685,12 @@ class NeuronavigationPanel(wx.Panel): self.tracker.SetTracker(choice) if self.tracker.tracker_id == const.ROBOT: - self.tracker.ConnectToRobot(self.navigation, self.tracker, self.robot) + self.dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker) + if self.dlg_correg_robot.ShowModal() == wx.ID_OK: + Publisher.sendMessage('Robot navigation mode', robot_mode=True) + else: + Publisher.sendMessage('Disconnect tracker') + wx.MessageBox(_("Not possible to connect to the robot."), _("InVesalius 3")) self.ResetICP() self.tracker.UpdateUI(ctrl, self.numctrls_fiducial[3:6], self.txtctrl_fre) @@ -776,8 +775,8 @@ 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_robot_to_head=None) + Publisher.sendMessage('Update robot target', robot_tracker_flag=False, + target_index=None, target=None) # Enable all navigation buttons choice_ref.Enable(True) @@ -786,6 +785,10 @@ class NeuronavigationPanel(wx.Panel): for btn_c in self.btns_set_fiducial: btn_c.Enable(True) + def OnDialogRobotDestroy(self): + if self.dlg_correg_robot: + self.dlg_correg_robot.Destroy() + def CheckFiducialRegistrationError(self): self.navigation.UpdateFiducialRegistrationError(self.tracker) fre, fre_ok = self.navigation.GetFiducialRegistrationError(self.icp) @@ -820,7 +823,7 @@ class NeuronavigationPanel(wx.Panel): for btn_c in self.btns_set_fiducial: btn_c.Enable(False) - self.navigation.StartNavigation(self.tracker) + self.navigation.EstimateTrackerToInVTransformationMatrix(self.tracker) if not self.CheckFiducialRegistrationError(): # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok @@ -830,9 +833,11 @@ class NeuronavigationPanel(wx.Panel): if self.icp.use_icp: self.checkbox_icp.Enable(True) self.checkbox_icp.SetValue(True) - # Update FRE once more after starting the navigation, due to the optional use of ICP, - # which improves FRE. - self.CheckFiducialRegistrationError() + # Update FRE once more after starting the navigation, due to the optional use of ICP, + # which improves FRE. + self.CheckFiducialRegistrationError() + + self.navigation.StartNavigation(self.tracker) def OnNavigate(self, evt, btn_nav): select_tracker_elem = self.select_tracker_elem @@ -881,7 +886,6 @@ class NeuronavigationPanel(wx.Panel): ) self.tracker.__init__() self.icp.__init__() - self.robot.__init__(self.tracker) class ObjectRegistrationPanel(wx.Panel): @@ -1248,20 +1252,7 @@ class MarkersPanel(wx.Panel): if field.type is bool: setattr(self, field.name, str_val=='True') - @dataclasses.dataclass - class Robot_Marker: - """Class for storing robot target.""" - m_robot_target : list = None - - @property - def robot_target_matrix(self): - return self.m_robot_target - - @robot_target_matrix.setter - def robot_target_matrix(self, new_m_robot_target): - self.m_robot_target = new_m_robot_target - - def __init__(self, parent, navigation, tracker): + def __init__(self, parent, navigation, tracker, icp): wx.Panel.__init__(self, parent) try: default_colour = wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENUBAR) @@ -1273,6 +1264,7 @@ class MarkersPanel(wx.Panel): self.navigation = navigation self.tracker = tracker + self.icp = icp self.__bind_events() @@ -1280,11 +1272,9 @@ class MarkersPanel(wx.Panel): self.current_coord = [0, 0, 0, None, None, None] self.current_seed = 0, 0, 0 - self.current_robot_target_matrix = [None] * 9 + self.markers = [] - self.robot_markers = [] self.nav_status = False - self.raw_target_robot = None, None self.marker_colour = const.MARKER_COLOUR self.marker_size = const.MARKER_SIZE @@ -1383,7 +1373,6 @@ 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.UpdateRobotCoordinates, 'Update raw coordinates') def __find_target_marker(self): """ @@ -1417,7 +1406,6 @@ class MarkersPanel(wx.Panel): """ for i in reversed(index): del self.markers[i] - del self.robot_markers[i] self.lc.DeleteItem(i) for n in range(0, self.lc.GetItemCount()): self.lc.SetItem(n, 0, str(n + 1)) @@ -1470,34 +1458,37 @@ class MarkersPanel(wx.Panel): def UpdateSeedCoordinates(self, root=None, affine_vtk=None, coord_offset=(0, 0, 0), coord_offset_w=(0, 0, 0)): self.current_seed = coord_offset_w - def UpdateRobotCoordinates(self, coordinates_raw, markers_flag): - self.raw_target_robot = coordinates_raw[1], coordinates_raw[2] - def OnMouseRightDown(self, evt): # TODO: Enable the "Set as target" only when target is created with registered object menu_id = wx.Menu() edit_id = menu_id.Append(0, _('Edit label')) menu_id.Bind(wx.EVT_MENU, self.OnMenuEditMarkerLabel, edit_id) - color_id = menu_id.Append(2, _('Edit color')) + color_id = menu_id.Append(1, _('Edit color')) menu_id.Bind(wx.EVT_MENU, self.OnMenuSetColor, color_id) menu_id.AppendSeparator() - target_menu = menu_id.Append(1, _('Set as target')) + target_menu = menu_id.Append(2, _('Set as target')) menu_id.Bind(wx.EVT_MENU, self.OnMenuSetTarget, target_menu) menu_id.AppendSeparator() - 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 all([elem is not None for elem in self.markers[self.lc.GetFocusedItem()].coord[3:]]): + check_target_angles = all([elem is not None for elem in self.markers[self.lc.GetFocusedItem()].coord[3:]]) + # Enable "Send target to robot" button only if tracker is robot, if navigation is on and if target is not none + if self.tracker.tracker_id == const.ROBOT: + send_target_to_robot_compensation = menu_id.Append(3, _('Sets target to robot head move compensation')) + menu_id.Bind(wx.EVT_MENU, self.OnMenuSetRobotCompensation, send_target_to_robot_compensation) + send_target_to_robot = menu_id.Append(4, _('Send target from InVesalius to robot')) + menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot) + if self.nav_status and check_target_angles: + send_target_to_robot_compensation.Enable(True) + send_target_to_robot.Enable(True) + else: + send_target_to_robot_compensation.Enable(False) + send_target_to_robot.Enable(False) + + if check_target_angles: target_menu.Enable(True) else: target_menu.Enable(False) - # Enable "Send target to robot" button only if tracker is robot, if navigation is on and if target is not none - m_target_robot = np.array([self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix]) - if self.tracker.tracker_id == const.ROBOT and self.nav_status and m_target_robot.any(): - send_target_to_robot.Enable(True) - else: - 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) @@ -1547,14 +1538,33 @@ class MarkersPanel(wx.Panel): Publisher.sendMessage('Set new color', index=index, color=color_new) + def OnMenuSetRobotCompensation(self, evt): + if isinstance(evt, int): + self.lc.Focus(evt) + + Publisher.sendMessage('Reset robot process', data=None) + matrix_tracker_fiducials = self.tracker.GetMatrixTrackerFiducials() + Publisher.sendMessage('Update tracker fiducials matrix', + matrix_tracker_fiducials=matrix_tracker_fiducials) + Publisher.sendMessage('Update robot target', robot_tracker_flag=True, target_index=self.lc.GetFocusedItem(), target=None) + def OnMenuSendTargetToRobot(self, evt): if isinstance(evt, int): self.lc.Focus(evt) + index = self.lc.GetFocusedItem() + if index == -1: + wx.MessageBox(_("No data selected."), _("InVesalius 3")) + return + + Publisher.sendMessage('Reset robot process', data=None) + matrix_tracker_fiducials = self.tracker.GetMatrixTrackerFiducials() + Publisher.sendMessage('Update tracker fiducials matrix', + matrix_tracker_fiducials=matrix_tracker_fiducials) - m_target_robot = self.robot_markers[self.lc.GetFocusedItem()].robot_target_matrix + target_coord = self.markers[index].coord[:3] + target = dcr.image_to_tracker(self.navigation.m_change, target_coord, self.icp) - Publisher.sendMessage('Reset robot process') - Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot_to_head=m_target_robot) + Publisher.sendMessage('Update robot target', robot_tracker_flag=True, target_index=self.lc.GetFocusedItem(), target=target.tolist()) def OnDeleteAllMarkers(self, evt=None): if evt is not None: @@ -1566,9 +1576,11 @@ class MarkersPanel(wx.Panel): Publisher.sendMessage('Disable or enable coil tracker', status=False) if evt is not None: wx.MessageBox(_("Target deleted."), _("InVesalius 3")) + if self.tracker.tracker_id == const.ROBOT: + Publisher.sendMessage('Update robot target', robot_tracker_flag=False, + target_index=None, target=None) self.markers = [] - self.robot_markers = [] Publisher.sendMessage('Remove all markers', indexes=self.lc.GetItemCount()) self.lc.DeleteAllItems() Publisher.sendMessage('Stop Blink Marker', index='DeleteAll') @@ -1596,8 +1608,8 @@ class MarkersPanel(wx.Panel): if self.__find_target_marker() in index: Publisher.sendMessage('Disable or enable coil tracker', status=False) if self.tracker.tracker_id == const.ROBOT: - Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False, - m_change_robot_to_head=[]) + Publisher.sendMessage('Update robot target', robot_tracker_flag=False, + target_index=None, target=None) wx.MessageBox(_("Target deleted."), _("InVesalius 3")) self.__delete_multiple_markers(index) @@ -1702,12 +1714,11 @@ class MarkersPanel(wx.Panel): new_marker.session_id = session_id or self.current_session if self.tracker.tracker_id == const.ROBOT and self.nav_status: - self.current_robot_target_matrix = db.compute_robot_to_head_matrix(self.raw_target_robot) + current_head_robot_target_status = True else: - self.current_robot_target_matrix = [None] * 9 + current_head_robot_target_status = False - new_robot_marker = self.Robot_Marker() - new_robot_marker.robot_target_matrix = self.current_robot_target_matrix + Publisher.sendMessage('Add marker to robot control', data=current_head_robot_target_status) # Note that ball_id is zero-based, so we assign it len(self.markers) before the new marker is added if all([elem is not None for elem in new_marker.coord[3:]]): @@ -1723,7 +1734,6 @@ class MarkersPanel(wx.Panel): self.markers.append(new_marker) - self.robot_markers.append(new_robot_marker) # Add item to list control in panel num_items = self.lc.GetItemCount() diff --git a/invesalius/navigation/icp.py b/invesalius/navigation/icp.py index 295cd34..dbda638 100644 --- a/invesalius/navigation/icp.py +++ b/invesalius/navigation/icp.py @@ -21,10 +21,10 @@ import wx import invesalius.data.bases as db import invesalius.gui.dialogs as dlg -from invesalius.pubsub import pub as Publisher +from invesalius.utils import Singleton -class ICP(): +class ICP(metaclass=Singleton): def __init__(self): self.use_icp = False self.m_icp = None @@ -35,7 +35,6 @@ class ICP(): if not self.use_icp: if dlg.ICPcorregistration(navigation.fre): - Publisher.sendMessage('Stop navigation') use_icp, self.m_icp = self.OnICP(navigation, tracker, navigation.m_change) if use_icp: self.icp_fre = db.calculate_fre(tracker.tracker_fiducials_raw, navigation.all_fiducials, @@ -43,7 +42,6 @@ class ICP(): self.SetICP(navigation, use_icp) else: print("ICP canceled") - Publisher.sendMessage('Start navigation') def OnICP(self, navigation, tracker, m_change): ref_mode_id = navigation.GetReferenceMode() diff --git a/invesalius/navigation/navigation.py b/invesalius/navigation/navigation.py index 86e7301..2ae8af7 100644 --- a/invesalius/navigation/navigation.py +++ b/invesalius/navigation/navigation.py @@ -34,6 +34,7 @@ import invesalius.data.tractography as dti import invesalius.data.transformations as tr import invesalius.data.vtk_utils as vtk_utils from invesalius.pubsub import pub as Publisher +from invesalius.utils import Singleton class QueueCustom(queue.Queue): @@ -83,7 +84,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.robot_target_queue = vis_queues + self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue = vis_queues self.sle = sle self.event = event self.neuronavigation_api = neuronavigation_api @@ -93,7 +94,7 @@ class UpdateNavigationScene(threading.Thread): while not self.event.is_set(): got_coords = False try: - coord, [coordinates_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait() + coord, markers_flag, m_img, view_obj = self.coord_queue.get_nowait() got_coords = True # print('UpdateScene: get {}'.format(count)) @@ -117,7 +118,6 @@ 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 coordinates', coordinates_raw=coordinates_raw, markers_flag=markers_flag) wx.CallAfter(Publisher.sendMessage, 'Update slice viewer') wx.CallAfter(Publisher.sendMessage, 'Sensor ID', markers_flag=markers_flag) @@ -140,7 +140,7 @@ class UpdateNavigationScene(threading.Thread): self.coord_queue.task_done() -class Navigation(): +class Navigation(metaclass=Singleton): def __init__(self, pedal_connection, neuronavigation_api): self.pedal_connection = pedal_connection self.neuronavigation_api = neuronavigation_api @@ -157,7 +157,6 @@ class Navigation(): self.coord_queue = QueueCustom(maxsize=1) self.icp_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) @@ -245,9 +244,14 @@ class Navigation(): if state and permission_to_stimulate: self.serial_port_connection.SendPulse() - def StartNavigation(self, tracker): + def EstimateTrackerToInVTransformationMatrix(self, tracker): tracker_fiducials, tracker_fiducials_raw = tracker.GetTrackerFiducials() + self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials]) + self.m_change = tr.affine_matrix_from_points(self.all_fiducials[3:, :].T, self.all_fiducials[:3, :].T, + shear=False, scale=False) + + def StartNavigation(self, tracker): # initialize jobs list jobs_list = [] @@ -255,17 +259,9 @@ 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.robot_target_queue] + vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue] Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components) - - self.all_fiducials = np.vstack([self.image_fiducials, tracker_fiducials]) - - # fiducials matrix - m_change = tr.affine_matrix_from_points(self.all_fiducials[3:, :].T, self.all_fiducials[:3, :].T, - shear=False, scale=False) - self.m_change = m_change - errors = False if self.track_obj: @@ -279,14 +275,14 @@ class Navigation(): # obj_reg[0] is object 3x3 fiducial matrix and obj_reg[1] is 3x3 orientation matrix obj_fiducials, obj_orients, obj_ref_mode, obj_name = self.obj_reg - coreg_data = [m_change, obj_ref_mode] + coreg_data = [self.m_change, obj_ref_mode] if self.ref_mode_id: coord_raw, markers_flag = tracker.TrackerCoordinates.GetCoordinates() else: coord_raw = np.array([None]) - obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change) + obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, self.m_change) coreg_data.extend(obj_data) queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.object_at_target_queue] @@ -295,7 +291,7 @@ class Navigation(): self.event, self.sleep_nav, tracker.tracker_id, self.target)) else: - coreg_data = (m_change, 0) + coreg_data = (self.m_change, 0) queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue] jobs_list.append(dcr.CoordinateCorregistrateNoObject(self.ref_mode_id, tracker, coreg_data, self.view_tracts, queues, diff --git a/invesalius/navigation/robot.py b/invesalius/navigation/robot.py deleted file mode 100644 index 731e65d..0000000 --- a/invesalius/navigation/robot.py +++ /dev/null @@ -1,308 +0,0 @@ -#-------------------------------------------------------------------------- -# 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 numpy as np -import wx -import queue -import threading -from time import sleep - -import invesalius.data.bases as db -import invesalius.gui.dialogs as dlg -import invesalius.constants as const -from invesalius.pubsub import pub as Publisher - -try: - import invesalius.data.elfin as elfin - import invesalius.data.elfin_processing as elfin_process - has_robot = True -except ImportError: - has_robot = False - -class Robot(): - def __init__(self, tracker): - """ - Class to establish the connection between the robot and InVesalius - :param tracker: tracker.py instance - """ - self.tracker = tracker - self.trk_init = None - self.robot_target_queue = None - self.object_at_target_queue = None - self.process_tracker = None - - self.thread_robot = None - - self.robotcoordinates = RobotCoordinates() - - 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') - Publisher.subscribe(self.OnResetProcessTracker, 'Reset robot process') - - def OnRobotConnection(self): - if not self.tracker.trk_init[0][0][0] or not self.tracker.trk_init[0][1][0]: - dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, self.tracker.trk_init[1]) - self.tracker.tracker_id = 0 - self.tracker.tracker_connected = False - else: - self.tracker.trk_init.append(self.robotcoordinates) - self.process_tracker = elfin_process.TrackerProcessing() - dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker) - if dlg_correg_robot.ShowModal() == wx.ID_OK: - 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[0][1][0] - self.trk_init = self.tracker.trk_init - else: - dlg.ShowNavigationTrackerWarning(self.tracker.tracker_id, 'disconnect') - self.tracker.trk_init = None - self.tracker.tracker_id = 0 - self.tracker.tracker_connected = False - - def StartRobotThreadNavigation(self, tracker, coord_queue): - if tracker.event_robot.is_set(): - tracker.event_robot.clear() - self.thread_robot = ControlRobot(self.trk_init, tracker, self.robotcoordinates, - [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() - self.OnResetProcessTracker() - - def OnResetProcessTracker(self): - self.process_tracker.__init__() - - def OnSendCoordinates(self, coord): - self.robot_server.SendCoordinates(coord) - - def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot_to_head): - try: - 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.object_at_target_queue: - self.object_at_target_queue.put_nowait(state) - except queue.Full: - pass - - def SetRobotQueues(self, queues): - self.robot_target_queue, self.object_at_target_queue = queues - - -class RobotCoordinates(): - """ - Class to set/get robot coordinates. Robot coordinates are acquired in ControlRobot thread. - The class is required to avoid acquisition conflict with different threads (coordinates and navigation) - """ - def __init__(self): - self.coord = None - - def SetRobotCoordinates(self, coord): - self.coord = coord - - def GetRobotCoordinates(self): - return self.coord - - -class ControlRobot(threading.Thread): - def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event_robot): - """Class (threading) to perform the robot control. - A distinguish thread is required to increase perform and separate robot control from navigation thread - (safetly layer for robot positining). - - There is no GUI involved, them no Sleep is required - - :param trck_init: Initialization variable of tracking device and connection type. See tracker.py. - :param tracker: tracker.py instance - :param robotcoordinates: RobotCoordinates() instance - :param queues: Queue instance that manage coordinates and robot target - :param process_tracker: TrackerProcessing() instance from elfin_process - :param event_robot: Threading event to ControlRobot when tasks is done - """ - threading.Thread.__init__(self, name='ControlRobot') - - self.trck_init_robot = trck_init[0][1][0] - self.trck_init_tracker = trck_init[0][0][0] - self.trk_id = trck_init[1] - self.tracker = tracker - self.robotcoordinates = robotcoordinates - self.robot_tracker_flag = False - self.objattarget_flag = False - self.target_flag = False - self.m_change_robot_to_head = None - self.coord_inv_old = None - self.coord_queue = queues[0] - self.robot_target_queue = queues[1] - self.object_at_target_queue = queues[2] - self.process_tracker = process_tracker - 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 - self.previous_robot_status = False - - 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] - self.robotcoordinates.SetRobotCoordinates(coord_robot) - - coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates() - - return coord_raw, coord_robot_raw, markers_flag - - def robot_motion_reset(self): - self.trck_init_robot.StopRobot() - self.arc_motion_flag = False - self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"] - - def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered): - """ - There are two types of robot movements. - We can imagine in two concentric spheres of different sizes. The inside sphere is to compensate for small head movements. - It was named "normal" moves. - The outside sphere is for the arc motion. The arc motion is a safety feature for long robot movements. - Even for a new target or a sudden huge head movement. - 1) normal: - A linear move from the actual position until the target position. - This movement just happens when move distance is below a threshold (const.ROBOT_ARC_THRESHOLD_DISTANCE) - 2) arc motion: - It can be divided into three parts. - The first one represents the movement from the inner sphere to the outer sphere. - The robot moves back using a radial move (it use the center of the head as a reference). - The second step is the actual arc motion (along the outer sphere). - A middle point, between the actual position and the target, is required. - The last step is to make a linear move until the target (goes to the inner sphere) - - """ - #Check if the target is inside the working space - if self.process_tracker.estimate_robot_target_length(new_robot_coordinates) < const.ROBOT_WORKING_SPACE: - #Check the target distance to define the motion mode - if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag: - self.trck_init_robot.SendCoordinates(new_robot_coordinates, const.ROBOT_MOTIONS["normal"]) - - elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag: - actual_point = current_robot_coordinates - if not self.arc_motion_flag: - head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker, - current_head_filtered).tolist() - - self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates, - new_robot_coordinates) - self.arc_motion_flag = True - self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"] - - 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.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]: - head_center_coordinates = self.process_tracker.estimate_head_center(self.tracker, - current_head_filtered).tolist() - - _, new_target_arc = self.process_tracker.compute_arc_motion(current_robot_coordinates, head_center_coordinates, - new_robot_coordinates) - if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1): - None - else: - if self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) >= \ - const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8: - self.target_arc = new_target_arc - - coord = self.target_arc - - if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10): - self.arc_motion_flag = False - self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"] - coord = new_robot_coordinates - - self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag) - robot_status = True - else: - print("Head is too far from the robot basis") - robot_status = False - - return robot_status - - def robot_control(self, current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag): - coord_head_tracker_in_robot = current_tracker_coordinates_in_robot[1] - marker_head_flag = markers_flag[1] - coord_obj_tracker_in_robot = current_tracker_coordinates_in_robot[2] - marker_obj_flag = markers_flag[2] - robot_status = False - - if self.robot_tracker_flag: - current_head = coord_head_tracker_in_robot - if current_head is not None and marker_head_flag: - current_head_filtered = self.process_tracker.kalman_filter(current_head) - if self.process_tracker.compute_head_move_threshold(current_head_filtered): - new_robot_coordinates = self.process_tracker.compute_head_move_compensation(current_head_filtered, - self.m_change_robot_to_head) - robot_status = True - if self.coord_inv_old is None: - self.coord_inv_old = new_robot_coordinates - - if np.allclose(np.array(new_robot_coordinates), np.array(current_robot_coordinates), 0, 0.01): - #avoid small movements (0.01 mm) - pass - elif not np.allclose(np.array(new_robot_coordinates), np.array(self.coord_inv_old), 0, 5): - #if the head moves (>5mm) before the robot reach the target - self.trck_init_robot.StopRobot() - self.coord_inv_old = new_robot_coordinates - else: - distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) - robot_status = self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered) - self.coord_inv_old = new_robot_coordinates - else: - self.trck_init_robot.StopRobot() - - return robot_status - - def run(self): - while not self.event_robot.is_set(): - current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag = self.get_coordinates_from_tracker_devices() - - if not self.robot_target_queue.empty(): - self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait() - self.robot_motion_reset() - self.robot_target_queue.task_done() - - if not self.object_at_target_queue.empty(): - self.target_flag = self.object_at_target_queue.get_nowait() - self.object_at_target_queue.task_done() - - robot_status = self.robot_control(current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag) - - if self.previous_robot_status != robot_status: - wx.CallAfter(Publisher.sendMessage, 'Update robot status', robot_status=robot_status) - self.previous_robot_status = robot_status - - sleep(const.SLEEP_ROBOT) diff --git a/invesalius/navigation/tracker.py b/invesalius/navigation/tracker.py index a7708d8..22aba92 100644 --- a/invesalius/navigation/tracker.py +++ b/invesalius/navigation/tracker.py @@ -42,7 +42,6 @@ class Tracker(): self.thread_coord = None self.event_coord = threading.Event() - self.event_robot = threading.Event() self.TrackerCoordinates = dco.TrackerCoordinates() @@ -78,11 +77,8 @@ class Tracker(): if self.thread_coord: self.event_coord.set() - self.event_robot.set() self.thread_coord.join() self.event_coord.clear() - self.event_robot.clear() - Publisher.sendMessage('Update status text in GUI', label=_("Tracker disconnected")) @@ -92,13 +88,6 @@ class Tracker(): label=_("Tracker still connected")) print("Tracker still connected!") - def ConnectToRobot(self, navigation, tracker, robot): - robot.SetRobotQueues([navigation.robot_target_queue, navigation.object_at_target_queue]) - robot.OnRobotConnection() - trk_init_robot = self.trk_init[0][1][0] - if trk_init_robot: - robot.StartRobotThreadNavigation(tracker, navigation.coord_queue) - Publisher.sendMessage('Robot navigation mode', robot_mode=True) def IsTrackerInitialized(self): return self.trk_init and self.tracker_id and self.tracker_connected @@ -158,7 +147,7 @@ class Tracker(): 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] + return [m_probe_ref_left.tolist(), m_probe_ref_right.tolist(), m_probe_ref_nasion.tolist()] def GetTrackerInfo(self): return self.trk_init, self.tracker_id diff --git a/invesalius/net/remote_control.py b/invesalius/net/remote_control.py index 0600f33..c3e189c 100644 --- a/invesalius/net/remote_control.py +++ b/invesalius/net/remote_control.py @@ -79,5 +79,7 @@ class RemoteControl: }) except TypeError: pass + except socketio.exceptions.BadNamespaceError: + pass Publisher.add_sendMessage_hook(_emit) -- libgit2 0.21.2