ADD: function to stop and reset motion state

Renan
1 parent b50c2ba8
Showing 1 changed file with 6 additions and 3 deletions Show diff stats
invesalius/navigation/robot.py
@@ -175,6 +175,11 @@ class ControlRobot(threading.Thread):
  
         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.
@@ -277,9 +282,7 @@ class ControlRobot(threading.Thread):
  
             if not self.robot_target_queue.empty():
                 self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait()
-                self.trck_init_robot.StopRobot()
-                self.arc_motion_flag = False
-                self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
+                self.robot_motion_reset()
                 self.robot_target_queue.task_done()
  
             if not self.object_at_target_queue.empty():
...	...	@@ -175,6 +175,11 @@ class ControlRobot(threading.Thread):
175	175
176	176	return coord_raw, coord_robot_raw, markers_flag
177	177
	178	+ def robot_motion_reset(self):
	179	+ self.trck_init_robot.StopRobot()
	180	+ self.arc_motion_flag = False
	181	+ self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
	182	+
178	183	def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered):
179	184	"""
180	185	There are two types of robot movements.
...	...	@@ -277,9 +282,7 @@ class ControlRobot(threading.Thread):
277	282
278	283	if not self.robot_target_queue.empty():
279	284	self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait()
280		- self.trck_init_robot.StopRobot()
281		- self.arc_motion_flag = False
282		- self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
	285	+ self.robot_motion_reset()
283	286	self.robot_target_queue.task_done()
284	287
285	288	if not self.object_at_target_queue.empty():
...	...