Fixed #46: ReferencingInfo state bitsets were not parsed correctly; Parsing exceptions are now caught and logged

Author: cpr-mab

cri_lib/cri_controller.py

@@ -290,23 +290,30 @@ def _bg_receive_thread(self) -> None:
 
             continue_parsing = True
             while continue_parsing:
-                # check for an end of message
-                end_idx = message_buffer.find(b"CRIEND")
-                if end_idx != -1:
-                    start_idx = message_buffer.find(b"CRISTART")
-
-                    # check if there is a complete message
-                    if start_idx != -1:
-                        message = message_buffer[start_idx : end_idx + 6].decode()
-                        self._parse_message(message)
-
-                    # check if there is data left in the buffer
-                    if len(message_buffer) > end_idx + 7:
-                        message_buffer = message_buffer[end_idx + 7 :]
+                try:
+                    # check for an end of message
+                    end_idx = message_buffer.find(b"CRIEND")
+                    if end_idx != -1:
+                        start_idx = message_buffer.find(b"CRISTART")
+
+                        # check if there is a complete message
+                        if start_idx != -1:
+                            message = message_buffer[start_idx : end_idx + 6].decode()
+                            self._parse_message(message)
+
+                        # check if there is data left in the buffer
+                        if len(message_buffer) > end_idx + 7:
+                            message_buffer = message_buffer[end_idx + 7 :]
+                        else:
+                            message_buffer.clear()
                     else:
-                        message_buffer.clear()
-                else:
-                    continue_parsing = False
+                        continue_parsing = False
+                except Exception as ex:
+                    logger.error(
+                        "Receive Thread: Failed to parse message, clearing buffer: "
+                        + str(ex)
+                    )
+                    message_buffer.clear()
 
     def _wait_for_answer(
         self,

cri_lib/cri_protocol_parser.py

@@ -24,6 +24,28 @@
 logger = logging.getLogger(__name__)
 
 
+def to_ReferencingAxisState(cri_value: int) -> ReferencingAxisState:
+    """
+    Parses a referencing state bitset value from CRI into the ReferencingAxisState enum
+
+    Parameters
+    ----------
+        cri_value: referencing state value from CRI, this is an integer encoded bitset
+
+    Returns
+    -------
+        Parsed ReferencingAxisState value
+    """
+    all_axes_referenced = (cri_value & 0x1) != 0
+    is_referencing = (cri_value & 0x2) != 0
+    if is_referencing:
+        return ReferencingAxisState.REFERENCING
+    elif all_axes_referenced:
+        return ReferencingAxisState.REFERENCED
+    else:
+        return ReferencingAxisState.NOT_REFERENCED
+
+
 class CRIProtocolParser:
     """Class handling the parsing of CRI messages to the robot state."""
 
@@ -674,19 +696,19 @@ def _parse_info(self, parameters: list[str]) -> str | None:
                 parameters.insert(14, parameters[14][0])
 
             ref_state = ReferencingState(
-                global_state=ReferencingAxisState(int(parameters[1])),
-                A1=ReferencingAxisState(int(parameters[3])),
-                A2=ReferencingAxisState(int(parameters[4])),
-                A3=ReferencingAxisState(int(parameters[5])),
-                A4=ReferencingAxisState(int(parameters[6])),
-                A5=ReferencingAxisState(int(parameters[7])),
-                A6=ReferencingAxisState(int(parameters[8])),
-                E1=ReferencingAxisState(int(parameters[9])),
-                E2=ReferencingAxisState(int(parameters[10])),
-                E3=ReferencingAxisState(int(parameters[11])),
-                E4=ReferencingAxisState(int(parameters[12])),
-                E5=ReferencingAxisState(int(parameters[13])),
-                E6=ReferencingAxisState(int(parameters[14])),
+                global_state=to_ReferencingAxisState(int(parameters[1])),
+                A1=to_ReferencingAxisState(int(parameters[3])),
+                A2=to_ReferencingAxisState(int(parameters[4])),
+                A3=to_ReferencingAxisState(int(parameters[5])),
+                A4=to_ReferencingAxisState(int(parameters[6])),
+                A5=to_ReferencingAxisState(int(parameters[7])),
+                A6=to_ReferencingAxisState(int(parameters[8])),
+                E1=to_ReferencingAxisState(int(parameters[9])),
+                E2=to_ReferencingAxisState(int(parameters[10])),
+                E3=to_ReferencingAxisState(int(parameters[11])),
+                E4=to_ReferencingAxisState(int(parameters[12])),
+                E5=to_ReferencingAxisState(int(parameters[13])),
+                E6=to_ReferencingAxisState(int(parameters[14])),
                 mandatory=bool(parameters[16] == "1"),
                 ref_prog_enabled=bool(parameters[18] == "1"),
                 ref_prog_running=bool(parameters[19] == "1"),