From c5ca25b1dc9142cb6c1891083292484f712914f3 Mon Sep 17 00:00:00 2001
From: John <134429827+jbrhm@users.noreply.github.com>
Date: Sun, 24 Nov 2024 14:21:24 -0500
Subject: [PATCH] Port Over Image Capture Script (#39)

* Working Image Capturer

* Better Logging

* style

* pathlib

* style
---
 CMakeLists.txt           |  1 +
 scripts/image_capture.py | 65 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 66 insertions(+)
 create mode 100755 scripts/image_capture.py

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8c9e02fe..5ecc2c6a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -320,6 +320,7 @@ install(PROGRAMS
         superstructure/superstructure.py
         scripts/debug_course_publisher.py
 		scripts/visualizer.py
+		scripts/image_capture.py
 
         # starter project sources
         starter_project/autonomy/src/localization.py
diff --git a/scripts/image_capture.py b/scripts/image_capture.py
new file mode 100755
index 00000000..97fec048
--- /dev/null
+++ b/scripts/image_capture.py
@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg._image import Image
+from ament_index_python import get_package_share_directory
+
+import numpy as np
+import sys
+from pathlib import Path
+import datetime
+
+import cv2
+
+from rclpy.executors import ExternalShutdownException
+
+
+class ImageCapture(Node):
+    def __init__(self) -> None:
+        super().__init__("image_capture")
+
+        self.image_sub = self.create_subscription(Image, "/zed/left/image", self.save_image, 1)
+
+    def save_image(self, msg: Image):
+        img = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, -1)
+        unique_id = "{date:%Y-%m-%d_%H:%M:%S}".format(date=datetime.datetime.now())
+
+        path = (
+            Path(get_package_share_directory("mrover"))
+            / ".."
+            / ".."
+            / ".."
+            / ".."
+            / "src"
+            / "mrover"
+            / "data"
+            / "images"
+        )
+
+        if not path.exists():
+            path.mkdir(parents=True, exist_ok=True)
+
+        path = path / f"image_{unique_id}.jpg"
+
+        cv2.imwrite(str(path), img)
+
+        self.get_logger().info(f"Saved image_{unique_id}.jpg")
+        pass
+
+
+def main() -> None:
+    try:
+        rclpy.init(args=sys.argv)
+        while True:
+            input()
+            rclpy.spin_once(ImageCapture())
+        rclpy.shutdown()
+    except KeyboardInterrupt:
+        pass
+    except ExternalShutdownException:
+        sys.exit(1)
+
+
+if __name__ == "__main__":
+    main()