From e9f17e0f0a42074779c66db8a5ce1be68f343d3e Mon Sep 17 00:00:00 2001 From: Renan Date: Thu, 28 Oct 2021 22:24:30 +0300 Subject: [PATCH] ADD: robot status for UI --- invesalius/constants.py | 1 + invesalius/data/coordinates.py | 4 ++-- invesalius/data/elfin.py | 3 ++- invesalius/data/trackers.py | 13 +++++++++---- invesalius/data/viewer_volume.py | 49 +++++++++++++++++++++++++++++++++++++++++++++++++ invesalius/gui/dialogs.py | 2 +- invesalius/navigation/robot.py | 22 ++++++++++++++++------ invesalius/navigation/tracker.py | 6 ++++-- navigation/objects/robot.stl | Bin 0 -> 505784 bytes 9 files changed, 84 insertions(+), 16 deletions(-) create mode 100644 navigation/objects/robot.stl diff --git a/invesalius/constants.py b/invesalius/constants.py index a20b1fe..e738fd2 100644 --- a/invesalius/constants.py +++ b/invesalius/constants.py @@ -832,6 +832,7 @@ SEED_RADIUS = 1.5 # Increased the default sleep parameter from 0.1 to 0.15 to decrease CPU load during navigation. SLEEP_NAVIGATION = 0.15 SLEEP_COORDINATES = 0.05 +SLEEP_ROBOT = 0.01 BRAIN_OPACITY = 0.5 N_CPU = psutil.cpu_count() diff --git a/invesalius/data/coordinates.py b/invesalius/data/coordinates.py index 9d2567b..7f9aa20 100644 --- a/invesalius/data/coordinates.py +++ b/invesalius/data/coordinates.py @@ -498,8 +498,8 @@ def dynamic_reference_m(probe, reference): 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:]) + 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]) diff --git a/invesalius/data/elfin.py b/invesalius/data/elfin.py index eceda82..e056fd7 100644 --- a/invesalius/data/elfin.py +++ b/invesalius/data/elfin.py @@ -45,7 +45,8 @@ class Elfin_Server(): sleep(0.1) def Close(self): - self.cobot.close() + self.StopRobot() + #TODO: robot function to close? self.cobot.close() class Elfin: def __init__(self): diff --git a/invesalius/data/trackers.py b/invesalius/data/trackers.py index 555c6a7..9ca70a5 100644 --- a/invesalius/data/trackers.py +++ b/invesalius/data/trackers.py @@ -211,13 +211,17 @@ def OptitrackTracker(tracker_id): if trck_init.Initialize(Cal_optitrack, User_profile_optitrack)==0: trck_init.Run() #Runs once Run function, to update cameras. + lib_mode = 'wrapper' else: trck_init = None + lib_mode = 'error' except ImportError: + lib_mode = 'error' print('Error') else: + lib_mode = None print('#####') - return trck_init, 'wrapper' + return trck_init, lib_mode def ElfinRobot(robot_IP): trck_init = None @@ -230,7 +234,7 @@ def ElfinRobot(robot_IP): print('Connect to elfin robot tracking device.') except: - lib_mode = 'disconnect' + lib_mode = 'error' trck_init = None print('Could not connect to elfin robot tracker.') @@ -254,7 +258,7 @@ def RobotTracker(tracker_id): trck_init = trck_connection trck_init_robot = ElfinRobot(robot_IP) - return [trck_init, trck_init_robot, tracker_id] + return [(trck_init, trck_init_robot), tracker_id] def DebugTrackerRandom(tracker_id): trck_init = True @@ -388,7 +392,8 @@ def DisconnectTracker(tracker_id, trck_init): lib_mode = 'serial' print('Tracker disconnected.') elif tracker_id == const.ROBOT: - trck_init[0].Close() + trck_init[0][0].Close() + trck_init[1][0].Close() trck_init = False lib_mode = 'wrapper' print('Tracker disconnected.') diff --git a/invesalius/data/viewer_volume.py b/invesalius/data/viewer_volume.py index 558ac69..59ce775 100644 --- a/invesalius/data/viewer_volume.py +++ b/invesalius/data/viewer_volume.py @@ -191,6 +191,7 @@ class Viewer(wx.Panel): self.target_coord = None self.aim_actor = None self.dummy_coil_actor = None + self.dummy_robot_actor = None self.target_mode = False self.polydata = None self.use_default_object = True @@ -313,6 +314,10 @@ class Viewer(wx.Panel): Publisher.subscribe(self.load_mask_preview, 'Load mask preview') Publisher.subscribe(self.remove_mask_preview, 'Remove mask preview') + # Related to robot tracking during neuronavigation + Publisher.subscribe(self.ActivateRobotMode, 'Robot navigation mode') + Publisher.subscribe(self.OnUpdateRobotStatus, 'Update robot status') + def SetStereoMode(self, mode): ren_win = self.interactor.GetRenderWindow() @@ -1681,6 +1686,50 @@ class Viewer(wx.Panel): self.actor_tracts = None self.Refresh() + def ActivateRobotMode(self, robot_mode=None): + self.robot_mode = robot_mode + if self.robot_mode: + self.ren_robot = vtk.vtkRenderer() + self.ren_robot.SetLayer(1) + + self.interactor.GetRenderWindow().AddRenderer(self.ren_robot) + self.ren_robot.SetViewport(0.02, 0.82, 0.08, 0.92) + filename = os.path.join(inv_paths.OBJ_DIR, "robot.stl") + + reader = vtk.vtkSTLReader() + reader.SetFileName(filename) + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputConnection(reader.GetOutputPort()) + + dummy_robot_actor = vtk.vtkActor() + dummy_robot_actor.SetMapper(mapper) + dummy_robot_actor.GetProperty().SetColor(1, 1, 1) + dummy_robot_actor.GetProperty().SetOpacity(1.) + self.dummy_robot_actor = dummy_robot_actor + + self.ren_robot.AddActor(dummy_robot_actor) + self.ren_robot.InteractiveOff() + + self.interactor.Render() + else: + self.DisableRobotMode() + + def OnUpdateRobotStatus(self, robot_status): + if self.dummy_robot_actor: + if robot_status: + self.dummy_robot_actor.GetProperty().SetColor(0, 1, 0) + else: + self.dummy_robot_actor.GetProperty().SetColor(1, 0, 0) + self.Refresh() + + def DisableRobotMode(self): + try: + self.interactor.GetRenderWindow().RemoveRenderer(self.ren_robot) + self.ren.RemoveActor(self.dummy_robot_actor) + self.interactor.Render() + except: + None + def __bind_events_wx(self): #self.Bind(wx.EVT_SIZE, self.OnSize) # self.canvas.subscribe_event('LeftButtonPressEvent', self.on_insert_point) diff --git a/invesalius/gui/dialogs.py b/invesalius/gui/dialogs.py index aa4b5a9..547124c 100644 --- a/invesalius/gui/dialogs.py +++ b/invesalius/gui/dialogs.py @@ -4540,7 +4540,7 @@ class CreateTransformationMatrixRobot(wx.Dialog): 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[1][0].Run() + coord_raw_robot = self.tracker.trk_init[0][1][0].Run() coord_raw_tracker_obj = coord_raw[3] if markers_flag[2]: diff --git a/invesalius/navigation/robot.py b/invesalius/navigation/robot.py index 97b0ea5..1a5fab3 100644 --- a/invesalius/navigation/robot.py +++ b/invesalius/navigation/robot.py @@ -20,6 +20,7 @@ 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 @@ -58,7 +59,7 @@ class Robot(): Publisher.subscribe(self.OnResetProcessTracker, 'Reset robot process') def OnRobotConnection(self): - if not self.tracker.trk_init[0][0] or not self.tracker.trk_init[1][0]: + 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 @@ -69,7 +70,7 @@ class Robot(): 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[1][0] + 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') @@ -144,9 +145,9 @@ class ControlRobot(threading.Thread): """ threading.Thread.__init__(self, name='ControlRobot') - self.trck_init_robot = trck_init[1][0] - self.trck_init_tracker = trck_init[0] - self.trk_id = trck_init[2] + 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 @@ -243,14 +244,19 @@ class ControlRobot(threading.Thread): 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 @@ -259,6 +265,7 @@ class ControlRobot(threading.Thread): 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 @@ -271,11 +278,13 @@ class ControlRobot(threading.Thread): self.coord_inv_old = new_robot_coordinates else: distance_target = self.process_tracker.correction_distance_calculation_target(new_robot_coordinates, current_robot_coordinates) - self.robot_move_decision(distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered) + 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() + wx.CallAfter(Publisher.sendMessage, 'Update robot status', robot_status=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() @@ -290,3 +299,4 @@ class ControlRobot(threading.Thread): self.object_at_target_queue.task_done() self.robot_control(current_tracker_coordinates_in_robot, current_robot_coordinates, markers_flag) + sleep(const.SLEEP_ROBOT) diff --git a/invesalius/navigation/tracker.py b/invesalius/navigation/tracker.py index 869ca78..a7708d8 100644 --- a/invesalius/navigation/tracker.py +++ b/invesalius/navigation/tracker.py @@ -51,7 +51,7 @@ class Tracker(): self.DisconnectTracker() self.trk_init = dt.TrackerConnection(new_tracker, None, 'connect') - if not self.trk_init[0]: + if not all(list(self.trk_init)): dlg.ShowNavigationTrackerWarning(self.tracker_id, self.trk_init[1]) self.tracker_id = 0 @@ -70,6 +70,7 @@ class Tracker(): label=_("Disconnecting tracker ...")) Publisher.sendMessage('Remove sensors ID') Publisher.sendMessage('Remove object data') + Publisher.sendMessage('Robot navigation mode', robot_mode=False) self.trk_init = dt.TrackerConnection(self.tracker_id, self.trk_init[0], 'disconnect') if not self.trk_init[0]: self.tracker_connected = False @@ -94,9 +95,10 @@ class Tracker(): 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[1][0] + 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 diff --git a/navigation/objects/robot.stl b/navigation/objects/robot.stl new file mode 100644 index 0000000..b3b7670 Binary files /dev/null and b/navigation/objects/robot.stl differ -- libgit2 0.21.2