diff --git a/invesalius/constants.py b/invesalius/constants.py index 5387272..0d667f6 100644 --- a/invesalius/constants.py +++ b/invesalius/constants.py @@ -670,9 +670,9 @@ CAMERA = 5 POLARIS = 6 POLARISP4 = 7 OPTITRACK = 8 -DEBUGTRACKRANDOM = 9 -DEBUGTRACKAPPROACH = 10 -HYBRID = 11 +ROBOT = 9 +DEBUGTRACKRANDOM = 10 +DEBUGTRACKAPPROACH = 11 DEFAULT_TRACKER = SELECT NDICOMPORT = b'COM1' @@ -681,8 +681,8 @@ TRACKERS = [_("Claron MicronTracker"), _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"), _("Polhemus PATRIOT"), _("Camera tracker"), _("NDI Polaris"), _("NDI Polaris P4"), - _("Optitrack"), _("Debug tracker (random)"), - _("Debug tracker (approach)"), _("Hybrid tracker")] + _("Optitrack"), _("Robot tracker"), + _("Debug tracker (random)"), _("Debug tracker (approach)")] STATIC_REF = 0 DYNAMIC_REF = 1 @@ -811,4 +811,7 @@ WILDCARD_MARKER_FILES = _("Marker scanner coord files (*.mkss)|*.mkss") + "|" +\ ROBOT_ElFIN_IP = ['Select robot IP:', '143.107.220.251', '169.254.153.251', '127.0.0.1'] ROBOT_ElFIN_PORT = 10003 -MOTIONS = {"normal": 0, "linear out": 1, "arc": 2} \ No newline at end of file +ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2} + +ROBOT_HEAD_VELOCITY_THRESHOLD = 10 +ROBOT_ARC_THRESHOLD_DISTANCE = 100 diff --git a/invesalius/data/coordinates.py b/invesalius/data/coordinates.py index c0f8f16..3b323a8 100644 --- a/invesalius/data/coordinates.py +++ b/invesalius/data/coordinates.py @@ -65,9 +65,9 @@ def GetCoordinatesForThread(trck_init, trck_id, ref_mode): const.POLARIS: PolarisCoord, const.POLARISP4: PolarisP4Coord, const.OPTITRACK: OptitrackCoord, + const.ROBOT: RobotCoord, const.DEBUGTRACKRANDOM: DebugCoordRandom, - const.DEBUGTRACKAPPROACH: DebugCoordRandom, - const.HYBRID: HybridCoord} + const.DEBUGTRACKAPPROACH: DebugCoordRandom} coord, markers_flag = getcoord[trck_id](trck_init, trck_id, ref_mode) else: print("Select Tracker") @@ -450,7 +450,7 @@ def dynamic_reference_m(probe, reference): return coord_rot -def HybridCoord(trk_init, trck_id, ref_mode): +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:]) @@ -459,7 +459,6 @@ def HybridCoord(trk_init, trck_id, ref_mode): obj_tracker_in_robot = db.transform_tracker_2_robot().transformation_tracker_2_robot(coord_tracker[2]) if probe_tracker_in_robot is None: - #print("Getting raw tracker") probe_tracker_in_robot = coord_tracker[0] ref_tracker_in_robot = coord_tracker[1] obj_tracker_in_robot = coord_tracker[2] diff --git a/invesalius/data/coregistration.py b/invesalius/data/coregistration.py index eabdd76..08ca912 100644 --- a/invesalius/data/coregistration.py +++ b/invesalius/data/coregistration.py @@ -20,11 +20,9 @@ import numpy as np import queue import threading -from time import time, sleep -import csv +from time import sleep import invesalius.constants as const -import invesalius.data.coordinates as dco import invesalius.data.transformations as tr import invesalius.data.bases as bases @@ -202,13 +200,6 @@ class CoordinateCorregistrate(threading.Thread): # self.target[1] = -self.target[1] - # self.time_start = time() - # self.fieldnames = ["time", - # "x_nav", "y_nav", "z_nav", "a_nav", "b_nav", "c_nav", "target"] - # with open('data_nav.csv', 'w', newline='') as csv_file: - # csv_writer = csv.DictWriter(csv_file, fieldnames=self.fieldnames) - # csv_writer.writeheader() - def run(self): coreg_data = self.coreg_data view_obj = 1 @@ -268,22 +259,6 @@ class CoordinateCorregistrate(threading.Thread): if not self.icp_queue.empty(): self.icp_queue.task_done() - - # with open('data_nav.csv', 'a', newline='') as csv_file: - # csv_writer = csv.DictWriter(csv_file, fieldnames=self.fieldnames) - # - # info = { - # "time": time() - self.time_start, - # "x_nav": coord[0], - # "y_nav": coord[1], - # "z_nav": coord[2], - # "a_nav": coord[3], - # "b_nav": coord[4], - # "c_nav": coord[5], - # "target": self.target_flag - # } - # - # csv_writer.writerow(info) # The sleep has to be in both threads sleep(self.sle) except queue.Full: diff --git a/invesalius/data/elfin_robot.py b/invesalius/data/elfin_robot.py index 9ffa69a..bcc4394 100644 --- a/invesalius/data/elfin_robot.py +++ b/invesalius/data/elfin_robot.py @@ -2,7 +2,6 @@ import numpy as np import cv2 from time import time, sleep import threading -import csv import invesalius.data.elfin as elfin import invesalius.data.transformations as tr @@ -10,12 +9,10 @@ import invesalius.data.coregistration as dcr import invesalius.constants as const - class elfin_server(): def __init__(self, server_ip, port_number): self.server_ip = server_ip self.port_number = port_number - #print(cobot.ReadPcsActualPos()) def Initialize(self): SIZE = 1024 @@ -25,16 +22,14 @@ class elfin_server(): print("conected!") def Run(self): - #target = [540.0, -30.0, 850.0, 140.0, -81.0, -150.0] - #print("starting move") return self.cobot.ReadPcsActualPos() - def SendCoordinates(self, target, type=const.MOTIONS["normal"]): + def SendCoordinates(self, target, type=const.ROBOT_MOTIONS["normal"]): status = self.cobot.ReadMoveState() - if type == const.MOTIONS["normal"] or type == const.MOTIONS["linear out"]: + if type == const.ROBOT_MOTIONS["normal"] or type == const.ROBOT_MOTIONS["linear out"]: if status != 1009: self.cobot.MoveL(target) - elif type == const.MOTIONS["arc"]: + elif type == const.ROBOT_MOTIONS["arc"]: if status != 1009: self.cobot.MoveC(target) @@ -45,8 +40,8 @@ class elfin_server(): def Close(self): self.cobot.close() -class KalmanTracker: +class KalmanTracker: def __init__(self, state_num=2, cov_process=0.001, @@ -77,6 +72,7 @@ class KalmanTracker: self.filter.correct(self.measurement) self.state = self.filter.statePost + class TrackerProcessing: def __init__(self): self.coord_vel = [] @@ -90,7 +86,6 @@ class TrackerProcessing: cov_process=0.001, cov_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() @@ -108,7 +103,6 @@ class TrackerProcessing: 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) @@ -118,36 +112,35 @@ class TrackerProcessing: return velocity, distance - def Versores(self,init_point, final_point): + 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 - versorfator = (((final_point-init_point)/norm)*70).tolist() + versorfactor = (((final_point-init_point)/norm)*70).tolist() - return versorfator + return versorfactor def arcmotion(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] - versorfator1_calculado = self.Versores(pc,actual_point) - init_ext_point = actual_point[0]+versorfator1_calculado[0],\ - actual_point[1]+versorfator1_calculado[1],\ - actual_point[2]+versorfator1_calculado[2], \ + versorfactor1 = self.versors(pc, actual_point) + 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] - pm = ((p1[0]+actual_point[0])/2, (p1[1]+actual_point[1])/2,(p1[2]+actual_point[2])/2,0,0,0 ) #ponto médio da trajetória + 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.Versores(pc,pm)))*2 - pontointermediario = pm[0]+newarr[0],pm[1]+newarr[1],pm[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] - versorfator3 = self.Versores(pc,p1) - final_ext_point = p1[0]+versorfator3[0],p1[1]+versorfator3[1],p1[2]+versorfator3[2], coord_inv[3], coord_inv[4], coord_inv[5] - final_ext_point_arc = final_ext_point = p1[0]+versorfator3[0],p1[1]+versorfator3[1],p1[2]+versorfator3[2], coord_inv[3], coord_inv[4], coord_inv[5],0 - #type_arc = 0 - target_arc = pontointermediario+final_ext_point_arc + 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 return init_ext_point, target_arc @@ -165,7 +158,7 @@ class TrackerProcessing: self.velocity_std = np.std(self.velocity_vector) del self.velocity_vector[0] - if self.velocity_std > 10: + if self.velocity_std > const.ROBOT_HEAD_VELOCITY_THRESHOLD: print('Velocity threshold activated') return False else: @@ -173,7 +166,6 @@ class TrackerProcessing: return False - def head_move_compensation(self, current_ref, m_change_robot2ref): trans = tr.translation_matrix(current_ref[:3]) a, b, g = np.radians(current_ref[3:6]) @@ -184,8 +176,7 @@ class TrackerProcessing: _, _, angles, translate, _ = tr.decompose_matrix(m_robot_new) angles = np.degrees(angles) - return m_robot_new[0, -1], m_robot_new[1, -1], m_robot_new[2, -1], angles[0], angles[1], \ - angles[2] + return m_robot_new[0, -1], m_robot_new[1, -1], m_robot_new[2, -1], angles[0], angles[1], angles[2] def estimate_head_center(self, tracker, current_ref): m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion = tracker.GetMatrixTrackerFiducials() @@ -196,12 +187,11 @@ 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): - #a posição atual do robo está embaixo disso + 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 - correction_distance_compensation = pow(sum,0.5) + + (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 @@ -216,6 +206,7 @@ class RobotCoordinates(): def GetRobotCoordinates(self): return self.coord + class ControlRobot(threading.Thread): def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event): threading.Thread.__init__(self, name='ControlRobot') @@ -240,15 +231,6 @@ class ControlRobot(threading.Thread): self.target_linearin = None self.target_arc = None - self.time_start = time() - self.fieldnames = ["time", - "x_robot", "y_robot", "z_robot", "a_robot", "b_robot", "c_robot", - "x_tracker_obj", "y_tracker_obj", "z_tracker_obj", "a_tracker_obj", "b_tracker_obj", "c_tracker_obj", - "velocity_std", "target"] - with open('data_robot_and_tracker.csv', 'w', newline='') as csv_file: - csv_writer = csv.DictWriter(csv_file, fieldnames=self.fieldnames) - csv_writer.writeheader() - def getcoordsfromdevices(self): coord_robot_raw = self.trck_init_robot.Run() coord_robot = np.array(coord_robot_raw) @@ -259,6 +241,49 @@ 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: + self.trck_init_robot.SendCoordinates(coord_inv, const.ROBOT_MOTIONS["normal"]) + + elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arcmotion_flag: + actual_point = coord_robot_raw + if not self.arcmotion_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"] + + 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"] + coord = self.target_arc + + elif self.arcmotion_flag and self.arcmotion_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) + 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(coord_inv, coord_robot_raw) >= \ + 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.arcmotion_flag = False + self.arcmotion_step_flag = const.ROBOT_MOTIONS["normal"] + coord = coord_inv + + self.trck_init_robot.SendCoordinates(coord, self.arcmotion_step_flag) + def control(self, coords_tracker_in_robot, coord_robot_raw, markers_flag): coord_ref_tracker_in_robot = coords_tracker_in_robot[1] coord_obj_tracker_in_robot = coords_tracker_in_robot[2] @@ -266,8 +291,7 @@ class ControlRobot(threading.Thread): if self.robot_tracker_flag: current_ref = coord_ref_tracker_in_robot if current_ref is not None and markers_flag[1]: - #current_ref_filtered = self.process_tracker.kalman_filter(current_ref) - current_ref_filtered = current_ref + 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) @@ -275,62 +299,19 @@ class ControlRobot(threading.Thread): self.coord_inv_old = coord_inv if np.allclose(np.array(coord_inv), np.array(coord_robot_raw), 0, 0.01): - # print("At target within range 5") + # print("At target within range 1") pass elif not np.allclose(np.array(coord_inv), np.array(self.coord_inv_old), 0, 5): - # print("stop") self.trck_init_robot.StopRobot() self.coord_inv_old = coord_inv else: - #self.trck_init_robot.SendCoordinates(coord_inv, const.MOTIONS["normal"]) distance_target = self.process_tracker.correction_distance_calculation_target(coord_inv, coord_robot_raw) - if distance_target < 100 and not self.arcmotion_flag: - self.trck_init_robot.SendCoordinates(coord_inv, const.MOTIONS["normal"]) - - elif distance_target >= 100 or self.arcmotion_flag: - actual_point = coord_robot_raw - if not self.arcmotion_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.MOTIONS["linear out"] - - if self.arcmotion_flag and self.arcmotion_step_flag == const.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.MOTIONS["arc"] - coord = self.target_arc - - elif self.arcmotion_flag and self.arcmotion_step_flag == const.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) - 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(coord_inv, - coord_robot_raw) >= 80: - 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.arcmotion_flag = False - self.arcmotion_step_flag = const.MOTIONS["normal"] - coord = coord_inv - - self.trck_init_robot.SendCoordinates(coord, self.arcmotion_step_flag) + self.robot_move_decision(distance_target, coord_inv, coord_robot_raw, current_ref_filtered) self.coord_inv_old = coord_inv else: 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() @@ -348,33 +329,4 @@ class ControlRobot(threading.Thread): self.control(coords_tracker_in_robot, coord_robot_raw, markers_flag) - # if not self.robottarget_queue.empty(): - # self.robottarget_queue.task_done() - # if not self.objattarget_queue.empty(): - # self.objattarget_queue.task_done() - - - with open('data_robot_and_tracker.csv', 'a', newline='') as csv_file: - csv_writer = csv.DictWriter(csv_file, fieldnames=self.fieldnames) - - info = { - "time": time() - self.time_start, - "x_robot": coord_robot_raw[0], - "y_robot": coord_robot_raw[1], - "z_robot": coord_robot_raw[2], - "a_robot": coord_robot_raw[3], - "b_robot": coord_robot_raw[4], - "c_robot": coord_robot_raw[5], - "x_tracker_obj": coords_tracker_in_robot[2][0], - "y_tracker_obj": coords_tracker_in_robot[2][1], - "z_tracker_obj": coords_tracker_in_robot[2][2], - "a_tracker_obj": coords_tracker_in_robot[2][3], - "b_tracker_obj": coords_tracker_in_robot[2][4], - "c_tracker_obj": coords_tracker_in_robot[2][5], - "velocity_std": self.process_tracker.velocity_std, - "target": self.target_flag - } - - csv_writer.writerow(info) - - sleep(0.02) \ No newline at end of file + sleep(0.01) diff --git a/invesalius/data/plot_realtime.py b/invesalius/data/plot_realtime.py deleted file mode 100644 index 286fec2..0000000 --- a/invesalius/data/plot_realtime.py +++ /dev/null @@ -1,39 +0,0 @@ -import random -from itertools import count -import pandas as pd -import matplotlib -import matplotlib.pyplot as plt -from matplotlib.animation import FuncAnimation - -#plt.style.use('fivethirtyeight') -matplotlib.use('TkAgg') - -x_vals = [] -y_vals = [] - -index = count() -fig, (ax1, ax2) = plt.subplots(2) - -def animate(i): - data = pd.read_csv('D:\\Repository\\invesalius3\\data.csv') - x = data['time'] - y1 = data['x'] - y2 = data['xf'] - status_x = data['statusx'] - - ax1.cla() - ax2.cla() - - ax1.plot(x, y1, label='Coord x') - ax1.plot(x, y2, label='Coord x after Kalman') - - ax2.plot(x, status_x, label='STD', color='red') - - ax1.legend(loc='upper left') - ax2.legend(loc='upper left') - - -ani = FuncAnimation(fig, animate, interval=0.1) - -plt.tight_layout() -plt.show() \ No newline at end of file diff --git a/invesalius/data/trackers.py b/invesalius/data/trackers.py index 38cae0e..f08e9e5 100644 --- a/invesalius/data/trackers.py +++ b/invesalius/data/trackers.py @@ -41,10 +41,10 @@ def TrackerConnection(tracker_id, trck_init, action): const.CAMERA: CameraTracker, # CAMERA const.POLARIS: PolarisTracker, # POLARIS const.POLARISP4: PolarisP4Tracker, # POLARISP4 - const.OPTITRACK: OptitrackTracker, #Optitrack + const.OPTITRACK: OptitrackTracker, #Optitrack + const.ROBOT: RobotTracker, #Robot const.DEBUGTRACKRANDOM: DebugTrackerRandom, - const.DEBUGTRACKAPPROACH: DebugTrackerApproach, - const.HYBRID: HybridTracker} + const.DEBUGTRACKAPPROACH: DebugTrackerApproach} trck_init = trck_fcn[tracker_id](tracker_id) @@ -66,36 +66,68 @@ def DefaultTracker(tracker_id): # return tracker initialization variable and type of connection return trck_init, 'wrapper' -def OptitrackTracker(tracker_id): - """ - Imports optitrack wrapper from Motive 2.2. Initialize cameras, attach listener, loads Calibration, loads User Profile - (Rigid bodies information). +def ClaronTracker(tracker_id): + import invesalius.constants as const + from invesalius import inv_paths - Parameters - ---------- - tracker_id : Optitrack ID + trck_init = None + try: + import pyclaron - Returns - ------- - trck_init : local name for Optitrack module - """ - from wx import ID_OK + lib_mode = 'wrapper' + trck_init = pyclaron.pyclaron() + trck_init.CalibrationDir = inv_paths.MTC_CAL_DIR.encode(const.FS_ENCODE) + trck_init.MarkerDir = inv_paths.MTC_MAR_DIR.encode(const.FS_ENCODE) + trck_init.NumberFramesProcessed = 1 + trck_init.FramesExtrapolated = 0 + trck_init.PROBE_NAME = const.MTC_PROBE_NAME.encode(const.FS_ENCODE) + trck_init.REF_NAME = const.MTC_REF_NAME.encode(const.FS_ENCODE) + trck_init.OBJ_NAME = const.MTC_OBJ_NAME.encode(const.FS_ENCODE) + trck_init.Initialize() + + if trck_init.GetIdentifyingCamera(): + trck_init.Run() + print("MicronTracker camera identified.") + else: + trck_init = None + + except ImportError: + lib_mode = 'error' + print('The ClaronTracker library is not installed.') + + return trck_init, lib_mode + +def PolhemusTracker(tracker_id): + try: + trck_init = PlhWrapperConnection(tracker_id) + lib_mode = 'wrapper' + if not trck_init: + print('Could not connect with Polhemus wrapper, trying USB connection...') + trck_init = PlhUSBConnection(tracker_id) + lib_mode = 'usb' + if not trck_init: + print('Could not connect with Polhemus USB, trying serial connection...') + trck_init = PlhSerialConnection(tracker_id) + lib_mode = 'serial' + except: + trck_init = None + lib_mode = 'error' + print('Could not connect to Polhemus by any method.') + + return trck_init, lib_mode + +def CameraTracker(tracker_id): trck_init = None - dlg_port = dlg.SetOptitrackconfigs() - if dlg_port.ShowModal() == ID_OK: - Cal_optitrack, User_profile_optitrack = dlg_port.GetValue() - try: - import optitrack - trck_init = optitrack.optr() + try: + import invesalius.data.camera_tracker as cam + trck_init = cam.camera() + trck_init.Initialize() + print('Connect to camera tracking device.') - if trck_init.Initialize(Cal_optitrack, User_profile_optitrack)==0: - trck_init.Run() #Runs once Run function, to update cameras. - else: - trck_init = None - except ImportError: - print('Error') - else: - print('#####') + except: + print('Could not connect to camera tracker.') + + # return tracker initialization variable and type of connection return trck_init, 'wrapper' def PolarisTracker(tracker_id): @@ -127,7 +159,6 @@ def PolarisTracker(tracker_id): # return tracker initialization variable and type of connection return trck_init, lib_mode - def PolarisP4Tracker(tracker_id): from wx import ID_OK trck_init = None @@ -156,19 +187,36 @@ def PolarisP4Tracker(tracker_id): # return tracker initialization variable and type of connection return trck_init, lib_mode +def OptitrackTracker(tracker_id): + """ + Imports optitrack wrapper from Motive 2.2. Initialize cameras, attach listener, loads Calibration, loads User Profile + (Rigid bodies information). -def CameraTracker(tracker_id): - trck_init = None - try: - import invesalius.data.camera_tracker as cam - trck_init = cam.camera() - trck_init.Initialize() - print('Connect to camera tracking device.') + Parameters + ---------- + tracker_id : Optitrack ID - except: - print('Could not connect to camera tracker.') + Returns + ------- + trck_init : local name for Optitrack module + """ + from wx import ID_OK + trck_init = None + dlg_port = dlg.SetOptitrackconfigs() + if dlg_port.ShowModal() == ID_OK: + Cal_optitrack, User_profile_optitrack = dlg_port.GetValue() + try: + import optitrack + trck_init = optitrack.optr() - # return tracker initialization variable and type of connection + if trck_init.Initialize(Cal_optitrack, User_profile_optitrack)==0: + trck_init.Run() #Runs once Run function, to update cameras. + else: + trck_init = None + except ImportError: + print('Error') + else: + print('#####') return trck_init, 'wrapper' def ElfinRobot(robot_IP): @@ -189,57 +237,24 @@ def ElfinRobot(robot_IP): # return tracker initialization variable and type of connection return trck_init, lib_mode -def ClaronTracker(tracker_id): - import invesalius.constants as const - from invesalius import inv_paths - +def RobotTracker(tracker_id): + from wx import ID_OK trck_init = None - try: - import pyclaron - - lib_mode = 'wrapper' - trck_init = pyclaron.pyclaron() - trck_init.CalibrationDir = inv_paths.MTC_CAL_DIR.encode(const.FS_ENCODE) - trck_init.MarkerDir = inv_paths.MTC_MAR_DIR.encode(const.FS_ENCODE) - trck_init.NumberFramesProcessed = 1 - trck_init.FramesExtrapolated = 0 - trck_init.PROBE_NAME = const.MTC_PROBE_NAME.encode(const.FS_ENCODE) - trck_init.REF_NAME = const.MTC_REF_NAME.encode(const.FS_ENCODE) - trck_init.OBJ_NAME = const.MTC_OBJ_NAME.encode(const.FS_ENCODE) - trck_init.Initialize() - - if trck_init.GetIdentifyingCamera(): - trck_init.Run() - print("MicronTracker camera identified.") - else: - trck_init = None - - except ImportError: - lib_mode = 'error' - print('The ClaronTracker library is not installed.') - - return trck_init, lib_mode - - -def PolhemusTracker(tracker_id): - try: - trck_init = PlhWrapperConnection(tracker_id) - lib_mode = 'wrapper' - if not trck_init: - print('Could not connect with Polhemus wrapper, trying USB connection...') - trck_init = PlhUSBConnection(tracker_id) - lib_mode = 'usb' - if not trck_init: - print('Could not connect with Polhemus USB, trying serial connection...') - trck_init = PlhSerialConnection(tracker_id) - lib_mode = 'serial' - except: - trck_init = None - lib_mode = 'error' - print('Could not connect to Polhemus by any method.') - - return trck_init, lib_mode + trck_init_robot = None + tracker_id = None + dlg_device = dlg.SetTrackerDevice2Robot() + 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]: + 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) + return [trck_init, trck_init_robot, tracker_id] def DebugTrackerRandom(tracker_id): trck_init = True @@ -351,26 +366,6 @@ def PlhUSBConnection(tracker_id): return trck_init -def HybridTracker(tracker_id): - from wx import ID_OK - trck_init = None - trck_init_robot = None - tracker_id = None - dlg_device = dlg.SetTrackerDevice2Robot() - 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]: - 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) - - return [trck_init, trck_init_robot, tracker_id] - - def DisconnectTracker(tracker_id, trck_init): """ Disconnect current spatial tracker @@ -390,7 +385,7 @@ def DisconnectTracker(tracker_id, trck_init): trck_init = False lib_mode = 'serial' print('Tracker disconnected.') - elif tracker_id == const.HYBRID: + elif tracker_id == const.ROBOT: trck_init[0].Close() trck_init = False lib_mode = 'wrapper' diff --git a/invesalius/gui/dialogs.py b/invesalius/gui/dialogs.py index f0d5afe..583d9d6 100644 --- a/invesalius/gui/dialogs.py +++ b/invesalius/gui/dialogs.py @@ -875,9 +875,9 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode): const.POLARIS: 'NDI Polaris', const.POLARISP4: 'NDI Polaris P4', const.OPTITRACK: 'Optitrack', + const.ROBOT: 'Robotic navigation', const.DEBUGTRACKRANDOM: 'Debug tracker device (random)', - const.DEBUGTRACKAPPROACH: 'Debug tracker device (approach)', - const.HYBRID: 'Hybrid tracker device'} + const.DEBUGTRACKAPPROACH: 'Debug tracker device (approach)'} if lib_mode == 'choose': msg = _('No tracking device selected') @@ -3482,7 +3482,7 @@ class ObjectCalibrationDialog(wx.Dialog): if self.trk_init and self.tracker_id: coord_raw, markers_flag = self.TrackerCoordinates.GetCoordinates() if self.obj_ref_id and btn_id == 4: - if self.tracker_id == const.HYBRID: + if self.tracker_id == const.ROBOT: trck_init_robot = self.trk_init[1][0] coord = trck_init_robot.Run() else: diff --git a/invesalius/gui/task_navigator.py b/invesalius/gui/task_navigator.py index d3108a7..e18c08f 100644 --- a/invesalius/gui/task_navigator.py +++ b/invesalius/gui/task_navigator.py @@ -1045,7 +1045,7 @@ class NeuronavigationPanel(wx.Panel): # self.tracker.TrackerCoordinates, self.navigation.event).start() #sleep(5) - if self.tracker.tracker_id == const.HYBRID: + if self.tracker.tracker_id == const.ROBOT: self.robot.SetRobotQueues([self.navigation.robottarget_queue, self.navigation.objattarget_queue]) self.robot.OnRobotConnection(self.tracker, self.robotcoordinates) @@ -1143,8 +1143,6 @@ class NeuronavigationPanel(wx.Panel): btn_c.Enable(False) self.navigation.StartNavigation(self.tracker) - #if self.tracker.tracker_id == const.HYBRID: - # self.robot.StartRobotNavigation(self.tracker, self.navigation.coord_queue, self.navigation.robot_event) if not self.CheckFiducialRegistrationError(): # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok -- libgit2 0.21.2