Enable clear errors message from neuronavigator

main_loop.py

@@ -281,6 +281,7 @@ def main(connection=None):
                             const.FUNCTION_REQUEST_CONFIG: robot_control.send_config,
                             const.FUNCTION_REQUEST_PID: robot_control.send_pid_factors,
                             const.FUNCTION_SET_COIL_INDEX: robot_control.on_set_coil_index,
+                            const.FUNCTION_CLEAN_ERRORS: robot_control.on_clean_errors,
                         }
                         get_function[const.PUB_MESSAGES.index(topic[i])](buf[i]["data"])
 

robot/constants.py

@@ -161,7 +161,8 @@
     "Neuronavigation to Robot: Update robot control pid factors",
     "Neuronavigation to Robot: Request config",
     "Dashboard to Robot: Request pid factors",
-    "Neuronavigation to Robot: Set coil index"
+    "Neuronavigation to Robot: Set coil index",
+    "Neuronavigation to Robot: Reset errors"
 ]
 
 FUNCTION_CONNECT_TO_ROBOT = 0
@@ -184,3 +185,4 @@
 FUNCTION_REQUEST_CONFIG = 17
 FUNCTION_REQUEST_PID = 18
 FUNCTION_SET_COIL_INDEX = 19
+FUNCTION_CLEAN_ERRORS = 20

robot/control/robot_control.py

@@ -1370,3 +1370,8 @@ def send_pid_factors(self, data):
         self.remote_control.send_message(
                 "Robot to Dashboard: PID factors",
                 {"pid_factors": self.pid_group.get_pid_factors()})
+
+    def on_clean_errors(self, data):
+        if self.robot:
+            self.on_set_objective({"objective": RobotObjective.NONE.value})
+            self.robot.clean_errors()

robot/robots/dobot/dobot.py

@@ -228,3 +228,9 @@ def _motion_loop(self):
                 print("break: TIMEOUT_MOTION")
                 break
             time.sleep(0.001)
+
+    def clean_errors(self):
+        self.connection.clear_error()
+        time.sleep(0.5)
+        self.connection.reset_robot()
+        time.sleep(1)

robot/robots/elfin/elfin.py

@@ -144,3 +144,10 @@ def disable_free_drive(self):
     def close(self):
         self.stop_robot()
         self.disconnect()
+
+    def clean_errors(self):
+        self.connection.stop_robot()
+        sleep(0.5)
+        self.connection.clean_errors()
+        sleep(1)
+        self.connection.power_on_robot()

robot/robots/elfin/elfin_connection.py

@@ -411,3 +411,15 @@ def move_circular(self, start_position, waypoint, target):
 
         success, _ = self._send_and_receive(request, verbose=True)
         return success
+
+    def power_on_robot(self):
+        request = "GrpPowerOn," + str(self.ROBOT_ID)
+        success, _ = self._send_and_receive(request, verbose=True)
+        return success
+
+    def clean_errors(self):
+        request = ("GrpReset,"
+                + str(self.ROBOT_ID))
+
+        success, _ = self._send_and_receive(request, verbose=True)
+        return success

robot/robots/robot.py

@@ -158,3 +158,10 @@ def close(self):
         Perform necessary cleanup and disconnection procedures for the robot.
         """
         pass
+
+    @abstractmethod
+    def clean_errors(self):
+        """
+        Clean robot errors
+        """
+        pass

robot/robots/universal_robot/universal_robot.py

@@ -194,3 +194,6 @@ def enable_free_drive(self):
 
     def disable_free_drive(self):
         pass
+
+    def clean_errors(self):
+        pass