feat: integrate Picamera2 for improved image capture; improve grbl error handling

playgrounds/camera_playground.py

@@ -1,29 +1,22 @@
+from picamera2 import Picamera2
 import cv2
 
 def main():
-    cap = cv2.VideoCapture(0)
-    if not cap.isOpened():
-        print("Cannot open camera")
-        return
+    picam2 = Picamera2()
+    picam2.configure(picam2.create_preview_configuration(main={"format": "BGR888", "size": (640, 480)}))
+    picam2.start()
 
-    print("Press 's' to save a still image. Press 'q' to quit.")
+    print("Type 's' to save a still image. Type 'q' to quit.")
     while True:
-        ret, frame = cap.read()
-        if not ret:
-            print("Can't receive frame (stream end?). Exiting ...")
+        cmd = input("Command [s/q]: ").strip().lower()
+        if cmd == 's':
+            frame = picam2.capture_array()
+            cv2.imwrite("still_image.jpg", frame)
+            print("Saved still_image.jpg")
+        elif cmd == 'q':
             break
 
-        cv2.imshow('Camera', frame)
-        key = cv2.waitKey(1) & 0xFF
-
-        if key == ord('s'):
-            cv2.imwrite('still_image.jpg', frame)
-            print("Image saved as still_image.jpg")
-        elif key == ord('q'):
-            break
-
-    cap.release()
-    cv2.destroyAllWindows()
+    picam2.stop()
 
 if __name__ == "__main__":
     main()

robot_control/src/api/grbl_handler.py

@@ -159,13 +159,6 @@ class GRBLHandler:
         if not response_lines:
             return None
         
-        for line in response_lines:
-            if not 'ALARM' in line:
-                continue
-            if ":1" in line:
-                raise RuntimeError("Hard Limit was triggered!")
-            raise RuntimeError(f"Grbl threw ALARM: {line}")
-        
         return response_lines
 
     async def wait_until_idle(self, timeout_s, position: Optional[tuple[float,float,float]] = None):
@@ -244,6 +237,29 @@ class GRBLHandler:
         if not response_lines:
             logger.warning(f"No GRBL response received! ({timeout_s}s)")
 
+        alarm_messages = {
+            "1": "Hard limit has been triggered. Machine position is likely lost due to sudden halt. Re-homing is highly recommended.",
+            "2": "Soft limit: G-code motion target exceeds machine travel. Machine position retained. Alarm may be safely unlocked.",
+            "3": "Abort during cycle: Machine position is likely lost due to sudden halt. Re-homing is highly recommended. May be due to issuing g-code commands that exceed the limit of the machine.",
+            "4": "Probe fail: Probe is not in the expected initial state before starting probe cycle. Move the bit away from the touch plate.",
+            "5": "Probe fail: Probe did not contact the workpiece within the programmed travel. Move the bit closer to the touch plate.",
+            "6": "Homing fail: The active homing cycle was reset.",
+            "7": "Homing fail: Safety door was opened during homing cycle.",
+            "8": "Homing fail: Pull off travel failed to clear limit switch. Try increasing pull-off setting or check wiring.",
+            "9": "Homing fail: Could not find limit switch within search distances. Check max travel, pull-off distance, or wiring.",
+            "10": "EStop asserted: Emergency stop pressed. Unclick E-stop, then clear this alarm to continue.",
+            "14": "Spindle at speed timeout: Spindle hasn’t gotten up to speed or is wired incorrectly. Check setup and try again.",
+            "15": "Homing fail: Could not find second limit switch for auto squared axis. Check max travel, pull-off distance, or wiring.",
+            "17": "Motor fault: Issue encountered with closed loop motor tracking. Position likely lost.",
+        }
+        for line in response_lines:
+            if 'ALARM' not in line:
+                continue
+            for code, message in alarm_messages.items():
+                if f":{code}" in line:
+                    raise RuntimeError(f"Grbl ALARM {code}: {message}")
+                raise RuntimeError(f"Grbl threw ALARM: {line}")
+
         return response_lines
 
     def register_position_callback(self, callback):

robot_control/src/vision/datamatrix.py

@@ -7,6 +7,11 @@ from typing import Optional
 
 logger = logging.getLogger(__name__)
 
+try:
+    from picamera2 import Picamera2
+    PICAMERA2_AVAILABLE = True
+except ImportError:
+    PICAMERA2_AVAILABLE = False
 
 class TestCamera:
     def __init__(self, test_files_path: str):
@@ -48,19 +53,30 @@ class DataMatrixReader:
         self.camera_id = config.camera_id
         self.camera = None
         self.bbox = config.bbox
-
-    def initialize(self):
+        self.picam2 = None
         if self.camera_id < 0:
             test_path = "./tests/files/"
             self.camera = TestCamera(test_path)
         else:
-            self.camera = cv2.VideoCapture(self.camera_id)
+            if not PICAMERA2_AVAILABLE:
+                raise RuntimeError("Picamera2 is required for camera operation.")
+            self.picam2 = Picamera2()
+            self.picam2.configure(self.picam2.create_preview_configuration(
+                main={"format": "BGR888", "size": (640, 480)}
+            ))
+            self.picam2.start()
 
     def read_datamatrix(self) -> Optional[str]:
-        if not self.camera:
-            raise Exception("No cam available")
-        
-        ret, frame = self.camera.read()
+        if self.camera_id < 0:
+            # Use TestCamera
+            ret, frame = self.camera.read()
+        else:
+            # Use Picamera2
+            if not self.picam2:
+                raise Exception("No cam available")
+            frame = self.picam2.capture_array()
+            ret = frame is not None and frame.size != 0
+
         if not ret or frame is None or frame.size == 0:
             raise Exception("Failed to capture image")
         if self.bbox:
@@ -76,5 +92,9 @@ class DataMatrixReader:
         return decoded_str
 
     def cleanup(self):
-        if self.camera:
-            self.camera.release()
+        if self.camera_id < 0:
+            if self.camera:
+                self.camera.release()
+        else:
+            if self.picam2:
+                self.picam2.stop()